From 65d40920b8a850dca7c05795210b22f92e31c675 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 19 Dec 2024 08:19:12 +0100 Subject: [PATCH] Add controller node options args to be able to set controller specific node arguments (#1713) --- controller_manager/CMakeLists.txt | 3 ++ .../controller_manager/spawner.py | 16 ++++++++ controller_manager/doc/userdoc.rst | 4 +- controller_manager/package.xml | 1 + controller_manager/src/controller_manager.cpp | 35 +++++++++++++++++ .../test/test_controller/test_controller.cpp | 11 ++++++ .../test/test_controller/test_controller.hpp | 6 ++- .../test/test_spawner_unspawner.cpp | 39 +++++++++++++++++++ doc/release_notes.rst | 1 + .../hardware_interface/controller_info.hpp | 3 ++ 10 files changed, 116 insertions(+), 3 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1b0f308613..a97f171dd0 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -58,10 +58,12 @@ target_link_libraries(ros2_control_node PRIVATE if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) + find_package(example_interfaces REQUIRED) # Plugin Libraries that are built and installed for use in testing add_library(test_controller SHARED test/test_controller/test_controller.cpp) target_link_libraries(test_controller PUBLIC controller_manager) + ament_target_dependencies(test_controller PUBLIC example_interfaces) target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface test/test_controller/test_controller.xml) install( @@ -210,6 +212,7 @@ if(BUILD_TESTING) ) target_link_libraries(test_hardware_spawner controller_manager + test_controller ros2_control_test_assets::ros2_control_test_assets ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index b8abe8469f..1a55f17e6d 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -26,6 +26,7 @@ load_controller, switch_controllers, unload_controller, + set_controller_parameters, set_controller_parameters_from_param_files, bcolors, ) @@ -145,6 +146,12 @@ def main(args=None): action="store_true", required=False, ) + parser.add_argument( + "--controller-ros-args", + help="The --ros-args to be passed to the controller node for remapping topics etc", + default=None, + required=False, + ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) @@ -203,6 +210,15 @@ def main(args=None): + bcolors.ENDC ) else: + if controller_ros_args := args.controller_ros_args: + if not set_controller_parameters( + node, + controller_manager_name, + controller_name, + "node_options_args", + controller_ros_args.split(), + ): + return 1 if param_files: if not set_controller_parameters_from_param_files( node, diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 4cbc7a73a1..f6967930eb 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -142,7 +142,7 @@ There are two scripts to interact with controller manager from launch files: $ ros2 run controller_manager spawner -h usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] - [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT] + [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT] [--controller-ros-args CONTROLLER_ROS_ARGS] controller_names [controller_names ...] positional arguments: @@ -167,6 +167,8 @@ There are two scripts to interact with controller manager from launch files: Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether + --controller-ros-args CONTROLLER_ROS_ARGS + The --ros-args to be passed to the controller node for remapping topics etc The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files: diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 5447040537..4bbd6306b1 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -36,6 +36,7 @@ python3-coverage hardware_interface_testing ros2_control_test_assets + example_interfaces ament_cmake diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9f20e2b584..47ccf17cc8 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -616,6 +616,17 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.info.fallback_controllers_names = fallback_controllers; } + const std::string node_options_args_param = controller_name + ".node_options_args"; + std::vector node_options_args; + if (!has_parameter(node_options_args_param)) + { + declare_parameter(node_options_args_param, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); + } + if (get_parameter(node_options_args_param, node_options_args) && !node_options_args.empty()) + { + controller_spec.info.node_options_args = node_options_args; + } + return add_controller_impl(controller_spec); } @@ -3453,6 +3464,18 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back(arg); } + // Add deprecation notice if the arguments are from the controller_manager node + if ( + check_for_element(node_options_arguments, RCL_REMAP_FLAG) || + check_for_element(node_options_arguments, RCL_SHORT_REMAP_FLAG)) + { + RCLCPP_WARN( + get_logger(), + "The use of remapping arguments to the controller_manager node is deprecated. Please use the " + "'--controller-ros-args' argument of the spawner to pass remapping arguments to the " + "controller node."); + } + for (const auto & parameters_file : controller.info.parameters_files) { if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) @@ -3475,6 +3498,18 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back("use_sim_time:=true"); } + // Add options parsed through the spawner + if ( + !controller.info.node_options_args.empty() && + !check_for_element(controller.info.node_options_args, RCL_ROS_ARGS_FLAG)) + { + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); + } + for (const auto & arg : controller.info.node_options_args) + { + node_options_arguments.push_back(arg); + } + std::string arguments; arguments.reserve(1000); for (const auto & arg : node_options_arguments) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 04ae8c02c2..d20e7598a9 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -101,6 +101,17 @@ CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; } CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + const std::string service_name = get_node()->get_name() + std::string("/set_bool"); + service_ = get_node()->create_service( + service_name, + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_INFO_STREAM( + get_node()->get_logger(), "Setting response to " << std::boolalpha << request->data); + response->success = request->data; + }); return CallbackReturn::SUCCESS; } diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index d57fd9ddd9..ee9e668cfa 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -15,11 +15,14 @@ #ifndef TEST_CONTROLLER__TEST_CONTROLLER_HPP_ #define TEST_CONTROLLER__TEST_CONTROLLER_HPP_ +#include #include #include #include "controller_interface/controller_interface.hpp" #include "controller_manager/visibility_control.h" +#include "example_interfaces/srv/set_bool.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace test_controller @@ -68,10 +71,9 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC std::vector get_state_interface_data() const; - const std::string & getRobotDescription() const; - void set_external_commands_for_testing(const std::vector & commands); + rclcpp::Service::SharedPtr service_; unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 107c557dbb..a4159d7fbe 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -664,6 +664,45 @@ TEST_F(TestLoadController, spawner_with_many_controllers) } } +TEST_F(TestLoadController, test_spawner_parsed_controller_ros_args) +{ + ControllerManagerRunner cm_runner(this); + 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)); + std::stringstream ss; + + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + // Now as the controller is active, we can call check for the service + std::shared_ptr node = rclcpp::Node::make_shared("set_bool_client"); + auto set_bool_service = node->create_client("/set_bool"); + ASSERT_FALSE(set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_FALSE(set_bool_service->service_is_ready()); + // Now check the service availability in the controller namespace + auto ctrl_1_set_bool_service = + node->create_client("/ctrl_1/set_bool"); + ASSERT_TRUE(ctrl_1_set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_TRUE(ctrl_1_set_bool_service->service_is_ready()); + + // Now test the remapping of the service name with the controller_ros_args + EXPECT_EQ( + call_spawner( + "ctrl_2 -c test_controller_manager --controller-ros-args '-r /ctrl_2/set_bool:=/set_bool'"), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + // Now as the controller is active, we can call check for the remapped service + ASSERT_TRUE(set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_TRUE(set_bool_service->service_is_ready()); + // Now check the service availability in the controller namespace + auto ctrl_2_set_bool_service = + node->create_client("/ctrl_2/set_bool"); + ASSERT_FALSE(ctrl_2_set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_FALSE(ctrl_2_set_bool_service->service_is_ready()); +} + class TestLoadControllerWithoutRobotDescription : public ControllerManagerFixture { diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a187e62437..9b5360ced0 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -76,6 +76,7 @@ controller_manager * The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). +* The spawner now supports the ``--controller-ros-args`` argument to pass the ROS 2 node arguments to the controller node to be able to remap topics, services and etc (`#1713 `_). * The spawner now supports parsing multiple ``-p`` or ``--param-file`` arguments, this should help in loading multiple parameter files for a controller or for multiple controllers (`#1805 `_). * ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 `_). * ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_). diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 3ad89551d5..d814ca7ae2 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -41,6 +41,9 @@ struct ControllerInfo /// List of fallback controller names to be activated if this controller fails. std::vector fallback_controllers_names; + + /// Controller node options arguments + std::vector node_options_args; }; } // namespace hardware_interface