Fixed broken Robot::joinMotion function

Author: Tim Schneider

include/franky/robot.hpp

@@ -183,12 +183,16 @@ class Robot : public franka::Robot {
   franka::RobotState current_state_;
   std::mutex state_mutex_;
   std::mutex control_mutex_;
-  std::optional<std::shared_future<std::exception_ptr>> control_future_;
+  std::condition_variable control_finished_condition_;
+  std::exception_ptr control_exception_;
+  std::thread control_thread_;
   MotionGeneratorVariant motion_generator_{std::nullopt};
   bool motion_generator_running_{false};
 
   [[nodiscard]] bool is_in_control_unsafe() const;
 
+  void joinMotionUnsafe(std::unique_lock<std::mutex> &lock);
+
   template<typename ControlSignalType>
   void moveInternal(
       const std::shared_ptr<Motion<ControlSignalType>> &motion,
@@ -204,13 +208,7 @@ class Robot : public franka::Robot {
           std::get<MotionGenerator<ControlSignalType>>(motion_generator_).updateMotion(motion);
         }
       } else {
-        if (control_future_.has_value()) {
-          control_future_->wait();
-          auto control_exception = control_future_->get();
-          if (control_exception != nullptr)
-            std::rethrow_exception(control_exception);
-          control_future_ = std::nullopt;
-        }
+        joinMotionUnsafe(lock);
 
         motion_generator_.emplace<MotionGenerator<ControlSignalType>>(this, motion);
         auto motion_generator = &std::get<MotionGenerator<ControlSignalType>>(motion_generator_);
@@ -220,8 +218,7 @@ class Robot : public franka::Robot {
               current_state_ = robot_state;
             });
         motion_generator_running_ = true;
-        control_future_ = std::async(
-            std::launch::async,
+        control_thread_ = std::thread(
             [this, control_func_executor, motion_generator]() {
               try {
                 bool done = false;
@@ -240,16 +237,15 @@ class Robot : public franka::Robot {
                   } else {
                     done = true;
                     motion_generator_running_ = false;
+                    control_finished_condition_.notify_all();
                   }
                 }
               } catch (...) {
-                {
-                  std::unique_lock<std::mutex> lock(control_mutex_);
-                  motion_generator_running_ = false;
-                }
-                return std::current_exception();
+                std::unique_lock<std::mutex> lock(control_mutex_);
+                control_exception_ = std::current_exception();
+                motion_generator_running_ = false;
+                control_finished_condition_.notify_all();
               }
-              return std::exception_ptr{nullptr};
             }
         );
       }

src/robot.cpp

@@ -110,8 +110,7 @@ void Robot::setCollisionBehavior(
 }
 
 bool Robot::is_in_control_unsafe() const {
-  return control_future_.has_value()
-      && control_future_->wait_for(std::chrono::seconds(0)) != std::future_status::ready;
+  return motion_generator_running_;
 }
 
 bool Robot::is_in_control() {
@@ -124,18 +123,20 @@ std::string Robot::fci_hostname() const {
 }
 
 void Robot::joinMotion() {
-  // This is to ensure the control thread safety of this operation. Otherwise, it is possible that the control_thread
-  // pointer gets overwritten at the exact moment we try to join the control_thread.
-  std::optional<std::shared_future<std::exception_ptr>> control_future;
-  {
-    std::unique_lock<std::mutex> lock(control_mutex_);
-    control_future = control_future_;
-  }
-  if (control_future.has_value())
-    control_future->wait();
-  auto control_exception = control_future->get();
-  if (control_exception != nullptr)
+  std::unique_lock<std::mutex> lock(control_mutex_);
+  joinMotionUnsafe(lock);
+}
+
+void Robot::joinMotionUnsafe(std::unique_lock<std::mutex> &lock) {
+  if (motion_generator_running_)
+    control_finished_condition_.wait(lock);
+  if (control_thread_.joinable())
+    control_thread_.join();
+  if (control_exception_ != nullptr) {
+    auto control_exception = control_exception_;
+    control_exception_ = nullptr;
     std::rethrow_exception(control_exception);
+  }
 }
 
 std::optional<ControlSignalType> Robot::current_control_signal_type() {