diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index eec1fe7bbc..f357ba7aa9 100644 --- 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 index 48f5509794..c143ab4862 100644 --- 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)