Update the fix logic to include the activation and deactivation list …

…post filters
ros-controls · Dec 15, 2024 · 7805687 · 7805687
1 parent 44d2552
commit 7805687
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 7 deletions.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -1490,8 +1490,8 @@ controller_interface::return_type ControllerManager::switch_controller(
       controller.info.claimed_interfaces.clear();
     }
     if (
-      std::find(activate_controllers.begin(), activate_controllers.end(), controller.info.name) !=
-      activate_controllers.end())
+      std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) !=
+      activate_request_.end())
     {
       if (!is_controller_active(controller.c))
       {
@@ -1500,10 +1500,13 @@ controller_interface::return_type ControllerManager::switch_controller(
         switch_result = controller_interface::return_type::ERROR;
       }
     }
+    /// @note The following is the case of the real controllers that are deactivated and doesn't
+    /// include the chained controllers that are deactivated and activated
     if (
-      std::find(
-        deactivate_controllers.begin(), deactivate_controllers.end(), controller.info.name) !=
-      deactivate_controllers.end())
+      std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name) !=
+        deactivate_request_.end() &&
+      std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) ==
+        activate_request_.end())
     {
       if (is_controller_active(controller.c))
       {

diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp
@@ -1177,7 +1177,7 @@ TEST_P(
   switch_test_controllers(
     {DIFF_DRIVE_CONTROLLER, ODOM_PUBLISHER_CONTROLLER, SENSOR_FUSION_CONTROLLER}, {PID_RIGHT_WHEEL},
     test_param.strictness, expected.at(test_param.strictness).future_status,
-    controller_interface::return_type::ERROR);
+    expected.at(test_param.strictness).return_type);
 
   // Preceding controller should stay deactivated and following controller
   // should be deactivated (if BEST_EFFORT)
@@ -1508,7 +1508,8 @@ TEST_P(
 
   switch_test_controllers(
     {DIFF_DRIVE_CONTROLLER}, {PID_LEFT_WHEEL, PID_RIGHT_WHEEL}, test_param.strictness,
-    expected.at(test_param.strictness).future_status, controller_interface::return_type::ERROR);
+    expected.at(test_param.strictness).future_status,
+    expected.at(test_param.strictness).return_type);
 
   // Preceding controller should stay deactivated and following controller
   // should be deactivated (if BEST_EFFORT)