From b27e85734457ffd45102cc55def74535215ac96c Mon Sep 17 00:00:00 2001
From: Zheng Qu <zheng.qu@franka.de>
Date: Wed, 16 Aug 2023 11:16:54 +0200
Subject: [PATCH] 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                |  1 +
 franka_hw/src/franka_hw.cpp | 11 +++++++----
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 830d6e2b..353abc4b 100644
--- a/CHANGELOG.md
+++ b/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
 
diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp
index bc063a1e..8cb96139 100644
--- a/franka_hw/src/franka_hw.cpp
+++ b/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;