Merge pull request #210 in SWDEV/franka_ros from bugfix/control-loop-…

…does-not-exit-upon-controller-switching to develop

* commit 'b27e85734457ffd45102cc55def74535215ac96c':
  fix: The previous controller was still active after controller switching

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;