diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h
index 5851fff..b2cd24f 100644
--- a/include/ur_modern_driver/robot_state_RT.h
+++ b/include/ur_modern_driver/robot_state_RT.h
@@ -64,6 +64,7 @@ private:
 
 public:
 	RobotStateRT(std::condition_variable& msg_cond);
+	~RobotStateRT();
 	double getVersion();
 	double getTime();
 	std::vector<double> getQTarget();
diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp
index 3233c85..3760f20 100644
--- a/src/robot_state_RT.cpp
+++ b/src/robot_state_RT.cpp
@@ -45,6 +45,12 @@ RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) {
 	pMsg_cond_ = &msg_cond;
 }
 
+RobotStateRT::~RobotStateRT() {
+	/* Make sure nobody is waiting after this thread is destroyed */
+	new_data_available_ = true;
+	pMsg_cond_->notify_all();
+}
+
 bool RobotStateRT::getNewDataAvailable() {
 	return new_data_available_;
 }