diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 60e121e3f2..d99a7f7330 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -189,6 +189,10 @@ if(BUILD_TESTING) test_controller ros2_control_test_assets::ros2_control_test_assets ) + + install(FILES test/test_controller_spawner_with_fallback_controllers.yaml + DESTINATION test) + install(FILES test/test_controller_spawner_with_type.yaml DESTINATION test) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 4f52a26e0b..dbefd360b9 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -210,6 +210,14 @@ def main(args=None): action="store_true", required=False, ) + parser.add_argument( + "--fallback_controllers", + help="Fallback controllers list are activated as a fallback strategy when the" + " spawned controllers fail. When the argument is provided, it takes precedence over" + " the fallback_controllers list in the param file", + default=None, + nargs="+", + ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) @@ -249,6 +257,7 @@ def main(args=None): return 1 for controller_name in controller_names: + fallback_controllers = args.fallback_controllers controller_type = args.controller_type prefixed_controller_name = controller_name if controller_namespace: @@ -329,6 +338,43 @@ def main(args=None): ) return 1 + if not fallback_controllers and param_file: + fallback_controllers = get_parameter_from_param_file( + controller_name, param_file, "fallback_controllers" + ) + + if fallback_controllers: + parameter = Parameter() + parameter.name = prefixed_controller_name + ".fallback_controllers" + parameter.value = get_parameter_value(string_value=str(fallback_controllers)) + + response = call_set_parameters( + node=node, node_name=controller_manager_name, parameters=[parameter] + ) + assert len(response.results) == 1 + result = response.results[0] + if result.successful: + node.get_logger().info( + bcolors.OKCYAN + + 'Setting fallback_controllers to ["' + + ",".join(fallback_controllers) + + '"] for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + else: + node.get_logger().fatal( + bcolors.FAIL + + 'Could not set fallback_controllers to ["' + + ",".join(fallback_controllers) + + '"] for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + return 1 + ret = load_controller(node, controller_manager_name, controller_name) if not ret.ok: node.get_logger().fatal( diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 43fdf5d015..6cb7b025b1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -482,6 +482,17 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.info.parameters_file = parameters_file; } + const std::string fallback_ctrl_param = controller_name + ".fallback_controllers"; + std::vector fallback_controllers; + if (!has_parameter(fallback_ctrl_param)) + { + declare_parameter(fallback_ctrl_param, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); + } + if (get_parameter(fallback_ctrl_param, fallback_controllers) && !fallback_controllers.empty()) + { + controller_spec.info.fallback_controllers_names = fallback_controllers; + } + return add_controller_impl(controller_spec); } diff --git a/controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml b/controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml new file mode 100644 index 0000000000..bc93f162c1 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml @@ -0,0 +1,13 @@ +ctrl_1: + ros__parameters: + joint_names: ["joint1"] + +ctrl_2: + ros__parameters: + joint_names: ["joint2"] + fallback_controllers: ["ctrl_6", "ctrl_7", "ctrl_8"] + +ctrl_3: + ros__parameters: + joint_names: ["joint3"] + fallback_controllers: ["ctrl_9"] diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 0030d9d148..d65689c6f2 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -324,3 +324,58 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } + +TEST_F(TestLoadController, spawner_test_fallback_controllers) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_fallback_controllers.yaml"; + + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 " + "-p " + + test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT( + ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + } + + // Try to spawn now the controller with fallback controllers inside the yaml + EXPECT_EQ( + call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT( + ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); + ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT( + ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8")); + ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto ctrl_3 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); + ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9")); + ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + } +} diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 61a51a134a..a38fb99cb3 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -38,6 +38,9 @@ struct ControllerInfo /// List of claimed interfaces by the controller. std::vector claimed_interfaces; + + /// List of fallback controller names to be activated if this controller fails. + std::vector fallback_controllers_names; }; } // namespace hardware_interface