fix: The previous controller was still active after controller switching

The issue has been caused by an undefined behavior where `run_function_` was
replaced upon controller switching in a different thread, while `run_function_`
was being executed.

In the fix, the controller switching process waits for the exit of `run_function_`, and
then sets `run_function_` to the corresponding value of the new controller.

CHANGELOG.md

@@ -12,6 +12,7 @@ Requires `libfranka` >= 0.8.0
   * `franka_control`: Fix a bug where `error_recovery` actions recover future errors ([#316](https://github.com/frankaemika/franka_ros/issues/316)).
   * `franka_gazebo`: `FrankaHWSim` only acts on joints belonging to a Franka robot. This allows to combine a Franka robot and others (like mobile platforms) in same URDF ([#313](https://github.com/frankaemika/franka_ros/issues/313))
   * `franka_description`: `<xacro:franka_robot/>` macro now supports to customize the `parent` frame and its `xyz` + `rpy` offset
+  * `franka_hw`: Fix the bug where the previous controller is still running after switching the controller. ([#326](https://github.com/frankaemika/franka_ros/issues/326))
 
 ## 0.10.1 - 2022-09-15
 

franka_hw/src/franka_hw.cpp

@@ -297,9 +297,13 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
   requested_control_mode &= ~stop_control_mode;
   requested_control_mode |= start_control_mode;
 
-  if (!setRunFunction(requested_control_mode, get_limit_rate_(), get_cutoff_frequency_(),
-                      get_internal_controller_())) {
-    return false;
+  {
+    controller_active_ = false;
+    std::lock_guard<std::mutex> lock(robot_mutex_);
+    if (!setRunFunction(requested_control_mode, get_limit_rate_(), get_cutoff_frequency_(),
+                        get_internal_controller_())) {
+      return false;
+    }
   }
 
   if (current_control_mode_ != requested_control_mode) {
@@ -309,7 +313,6 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
                     << ", cutoff_frequency=" << get_cutoff_frequency_()
                     << ", internal_controller=" << get_internal_controller_());
     current_control_mode_ = requested_control_mode;
-    controller_active_ = false;
   }
 
   return true;