From a18aabcbf0a620b6527abc75c9b4f17cbadaefcd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 3 Nov 2025 11:01:05 +0100 Subject: [PATCH 1/2] Avoid deadlocks for failed command switching --- controller_manager/src/controller_manager.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a7d3af2478..c651a30020 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2964,6 +2964,8 @@ void ControllerManager::manage_switch() { RCLCPP_ERROR(get_logger(), "Error while performing mode switch."); // If the hardware switching fails, there is no point in continuing to switch controllers + switch_params_.do_switch = false; + switch_params_.cv.notify_all(); return; } execution_time_.switch_perform_mode_time = From c3d1b6673ef5fd25e2299d644ec9739fb9547575 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 3 Nov 2025 22:59:31 +0100 Subject: [PATCH 2/2] Add tests for the failing perform command mode switch case --- controller_manager/CMakeLists.txt | 1 + .../test/test_controller_manager_srvs.cpp | 149 ++++++++++++++++++ .../test/test_components/test_actuator.cpp | 8 + 3 files changed, 158 insertions(+) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1bd47cd923..d3b4c336ff 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -161,6 +161,7 @@ if(BUILD_TESTING) ${controller_manager_msgs_TARGETS} ) set_tests_properties(test_controller_manager_srvs PROPERTIES TIMEOUT 120) + ament_add_gmock(test_controller_manager_urdf_passing test/test_controller_manager_urdf_passing.cpp ) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 30fb8ec992..4a883b7627 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -2678,3 +2679,151 @@ TEST_F(TestControllerManagerSrvs, switch_controller_test_two_controllers_using_s lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_2->get_lifecycle_state().id()); } + +class TestControllerManagerSrvsWithFailingPerformSwitch : public TestControllerManagerSrvs +{ + void SetUp() + { + executor_ = std::make_shared(); + const std::string robot_description = ros2_control_test_assets::minimal_robot_urdf; + const std::regex plugin_regex(R"((.*?\s*))"); + const std::string replacement = "$1true\n"; + const std::string result = std::regex_replace(robot_description, plugin_regex, replacement); + + cm_ = std::make_shared( + std::make_unique( + result, rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface(), true), + executor_, TEST_CM_NAME); + run_updater_ = false; + time_ = rclcpp::Time(0, 0, cm_->get_trigger_clock()->get_clock_type()); + SetUpSrvsCMExecutor(); + } +}; + +TEST_F( + TestControllerManagerSrvsWithFailingPerformSwitch, switch_controller_with_failing_perform_switch) +{ + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create set of controllers + static constexpr char TEST_CONTROLLER_1[] = "test_controller_1"; + static constexpr char TEST_CONTROLLER_2[] = "test_controller_2"; + + // create non-chained controller + auto test_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/max_velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_1->set_command_interface_configuration(cmd_cfg); + test_controller_1->set_state_interface_configuration(state_cfg); + + auto test_controller_2 = std::make_shared(); + cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint2/max_acceleration", "joint2/velocity"}}; + state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint2/position", "joint2/velocity"}}; + test_controller_2->set_command_interface_configuration(cmd_cfg); + test_controller_2->set_state_interface_configuration(state_cfg); + + // add controllers + cm_->add_controller( + test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); + + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + // check chainable controller + ASSERT_EQ(2u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, TEST_CONTROLLER_1); + ASSERT_EQ(result->controller[1].name, TEST_CONTROLLER_2); + + // configure controllers + for (const auto & controller : {TEST_CONTROLLER_1, TEST_CONTROLLER_2}) + { + cm_->configure_controller(controller); + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(2u, result->controller.size()); + + // reordered controllers + ASSERT_EQ(result->controller[0].name, TEST_CONTROLLER_2); + ASSERT_EQ(result->controller[1].name, TEST_CONTROLLER_1); + + // Check individual activation + auto res = cm_->switch_controller( + {TEST_CONTROLLER_1}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(100, 0)); + ASSERT_EQ(res, controller_interface::return_type::ERROR); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + res = cm_->switch_controller( + {TEST_CONTROLLER_1}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, false, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::ERROR); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Now test activating controller_2 + res = cm_->switch_controller( + {TEST_CONTROLLER_2}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + + res = cm_->switch_controller( + {}, {TEST_CONTROLLER_2}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Now try activating both the controllers at once, it should fail as the perform switch fails for + // one of the controller + res = cm_->switch_controller( + {TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, false, rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::ERROR); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 83a5eeba36..b0aac25fea 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -111,6 +111,14 @@ class TestActuator : public ActuatorInterface const std::vector & /*start_interfaces*/, const std::vector & /*stop_interfaces*/) override { + if (get_hardware_info().hardware_parameters.count("fail_on_perform_mode_switch")) + { + if (hardware_interface::parse_bool( + get_hardware_info().hardware_parameters.at("fail_on_perform_mode_switch"))) + { + return hardware_interface::return_type::ERROR; + } + } position_state_ += 0.1; return hardware_interface::return_type::OK; }