From 8abdd858151f5c18436e91f5863db209ee9da48f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 23 Aug 2024 11:50:34 +0200 Subject: [PATCH 1/9] Add controller node options args to be able to set controller specific node options arguments --- .../controller_manager/spawner.py | 16 +++++++++++++ controller_manager/src/controller_manager.cpp | 23 +++++++++++++++++++ .../hardware_interface/controller_info.hpp | 3 +++ 3 files changed, 42 insertions(+) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index c5a23defe4..f13dbd65f6 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.split(): + if not set_controller_parameters( + node, + controller_manager_name, + controller_name, + "node_options_args", + controller_ros_args, + ): + return 1 if param_files: if not set_controller_parameters_from_param_files( node, diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9f20e2b584..989e87de7e 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); } @@ -3475,6 +3486,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, ros_args_arg)) + { + node_options_arguments.push_back(ros_args_arg); + } + 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/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 From 78d17b853c0ec4687416ecdb3a3184f185a46cc1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 23 Aug 2024 12:17:56 +0200 Subject: [PATCH 2/9] fix the splitting when not parsed --- controller_manager/controller_manager/spawner.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index f13dbd65f6..5e586bc2bf 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -210,13 +210,13 @@ def main(args=None): + bcolors.ENDC ) else: - if controller_ros_args := args.controller_ros_args.split(): + 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, + controller_ros_args.split(), ): return 1 if param_files: From 729ab6c986a258a7b22cd09e77da34c84d5a8afe Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 24 Aug 2024 02:32:17 +0200 Subject: [PATCH 3/9] Use the consexpr for --ros-args --- controller_manager/src/controller_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 989e87de7e..717b29adad 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3489,9 +3489,9 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( // Add options parsed through the spawner if ( !controller.info.node_options_args.empty() && - !check_for_element(controller.info.node_options_args, ros_args_arg)) + !check_for_element(controller.info.node_options_args, RCL_ROS_ARGS_FLAG) { - node_options_arguments.push_back(ros_args_arg); + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); } for (const auto & arg : controller.info.node_options_args) { From fbee93c81990a2376b94cd69a5bbf5ec2fb5d5af Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 24 Aug 2024 02:34:48 +0200 Subject: [PATCH 4/9] Add missing ) --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 717b29adad..d1aa6f6caf 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3489,7 +3489,7 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( // 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) + !check_for_element(controller.info.node_options_args, RCL_ROS_ARGS_FLAG)) { node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); } From 5bd412441a9a28c1386f38f1bc3415e458ba0091 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 25 Aug 2024 23:04:10 +0200 Subject: [PATCH 5/9] Add deprecation notice to remapping args to the controller manager node --- controller_manager/src/controller_manager.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d1aa6f6caf..47ccf17cc8 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3464,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)) From b97b3316be54403af26a458590256854f4d37d00 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 25 Aug 2024 23:06:25 +0200 Subject: [PATCH 6/9] update the documentaion on spawners --- controller_manager/doc/userdoc.rst | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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: From 6bc9d8dce212cc040c9477fdcbcbe3f5d14e4e54 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 11 Sep 2024 23:28:12 +0200 Subject: [PATCH 7/9] Added test for the remapped service from the controller using the spawner args --- controller_manager/CMakeLists.txt | 2 + controller_manager/package.xml | 1 + .../test/test_controller/test_controller.cpp | 11 ++++++ .../test/test_controller/test_controller.hpp | 6 ++- .../test/test_spawner_unspawner.cpp | 39 +++++++++++++++++++ 5 files changed, 57 insertions(+), 2 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1b0f308613..d13cb594b3 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( 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/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 { From 0aef41f07c81fb107f1b0f777ed49b8553c60f39 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 11 Sep 2024 23:32:45 +0200 Subject: [PATCH 8/9] update release_notes --- doc/release_notes.rst | 1 + 1 file changed, 1 insertion(+) 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 `_). From 32583ca053673270dea5ad4c37ae894fa47ab2d2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 21 Nov 2024 00:48:47 +0100 Subject: [PATCH 9/9] Fix missing test_controller dependency to the test_hardware_spawner --- controller_manager/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index d13cb594b3..a97f171dd0 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -212,6 +212,7 @@ if(BUILD_TESTING) ) target_link_libraries(test_hardware_spawner controller_manager + test_controller ros2_control_test_assets::ros2_control_test_assets )