From 38a56002fd3ffa946445cc065b456548a78fafb2 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 13 Dec 2024 15:35:17 +0100 Subject: [PATCH] Fix the spawner to support full wildcard parameter entries (backport #1933) (#1939) --- controller_manager/CMakeLists.txt | 3 + .../controller_manager_services.py | 7 ++ controller_manager/doc/userdoc.rst | 86 ++++++++++++------- ...t_controller_spawner_wildcard_entries.yaml | 10 +++ .../test/test_spawner_unspawner.cpp | 65 ++++++++++++++ 5 files changed, 138 insertions(+), 33 deletions(-) create mode 100644 controller_manager/test/test_controller_spawner_wildcard_entries.yaml diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1a53d69bde..72b63962da 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -192,6 +192,9 @@ if(BUILD_TESTING) install(FILES test/test_controller_overriding_parameters.yaml DESTINATION test) + install(FILES test/test_controller_spawner_wildcard_entries.yaml + DESTINATION test) + ament_add_gmock( test_hardware_management_srvs test/test_hardware_management_srvs.cpp diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index a5148098f0..a401258ab0 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -256,6 +256,7 @@ def get_params_files_with_controller_parameters( f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" ) WILDCARD_KEY = "/**" + ROS_PARAMS_KEY = "ros__parameters" parameters = yaml.safe_load(f) # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name' for key in [ @@ -264,6 +265,8 @@ def get_params_files_with_controller_parameters( f"{WILDCARD_KEY}/{controller_name}", f"{WILDCARD_KEY}{namespaced_controller}", ]: + if parameter_file in controller_parameter_files: + break if key in parameters: if key == controller_name and namespace != "/": node.get_logger().fatal( @@ -274,6 +277,8 @@ def get_params_files_with_controller_parameters( if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: controller_parameter_files.append(parameter_file) + if WILDCARD_KEY in parameters and ROS_PARAMS_KEY in parameters[WILDCARD_KEY]: + controller_parameter_files.append(parameter_file) return controller_parameter_files @@ -306,6 +311,8 @@ def get_parameter_from_param_files( if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: controller_param_dict = parameters[WILDCARD_KEY][key] + if WILDCARD_KEY in parameters and ROS_PARAMS_KEY in parameters[WILDCARD_KEY]: + controller_param_dict = parameters[WILDCARD_KEY] if controller_param_dict and ( not isinstance(controller_param_dict, dict) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 27ded48e5e..dc81c8adef 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -122,57 +122,77 @@ There are two scripts to interact with controller manager from launch files: 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: + .. code-block:: yaml + + /**: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + + command_interfaces: + - position + ..... + + position_trajectory_controller_joint1: + ros__parameters: + joints: + - joint1 + + position_trajectory_controller_joint2: + ros__parameters: + joints: + - joint2 + .. code-block:: yaml /**/position_trajectory_controller: - ros__parameters: - type: joint_trajectory_controller/JointTrajectoryController - joints: - - joint1 - - joint2 + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 - command_interfaces: - - position - ..... + command_interfaces: + - position + ..... .. code-block:: yaml /position_trajectory_controller: - ros__parameters: - type: joint_trajectory_controller/JointTrajectoryController - joints: - - joint1 - - joint2 + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 - command_interfaces: - - position - ..... + command_interfaces: + - position + ..... .. code-block:: yaml position_trajectory_controller: - ros__parameters: - type: joint_trajectory_controller/JointTrajectoryController - joints: - - joint1 - - joint2 + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 - command_interfaces: - - position - ..... + command_interfaces: + - position + ..... .. code-block:: yaml /rrbot_1/position_trajectory_controller: - ros__parameters: - type: joint_trajectory_controller/JointTrajectoryController - joints: - - joint1 - - joint2 - - command_interfaces: - - position - ..... + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... ``unspawner`` ^^^^^^^^^^^^^^^^ diff --git a/controller_manager/test/test_controller_spawner_wildcard_entries.yaml b/controller_manager/test/test_controller_spawner_wildcard_entries.yaml new file mode 100644 index 0000000000..1a05ac03d6 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_wildcard_entries.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint1"] + param1: 1.0 + param2: 2.0 + +wildcard_ctrl_3: + ros__parameters: + param3: 3.0 diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index a21c3b3b2c..8a0c35a6a3 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -346,6 +346,71 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) test_file_path); } +TEST_F(TestLoadController, spawner_test_with_wildcard_entries_with_no_ctrl_name) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_wildcard_entries.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_1 wildcard_ctrl_2 wildcard_ctrl_3 -c test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 0); + + auto verify_ctrl_parameter = [](const auto & ctrl_node, bool has_param_3) + { + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint1"})); + + if (!ctrl_node->has_parameter("param1")) + { + ctrl_node->declare_parameter("param1", -10.0); + } + ASSERT_THAT(ctrl_node->get_parameter("param1").as_double(), 1.0); + + if (!ctrl_node->has_parameter("param2")) + { + ctrl_node->declare_parameter("param2", -10.0); + } + ASSERT_THAT(ctrl_node->get_parameter("param2").as_double(), 2.0); + + if (!ctrl_node->has_parameter("param3")) + { + ctrl_node->declare_parameter("param3", -10.0); + } + ASSERT_THAT(ctrl_node->get_parameter("param3").as_double(), has_param_3 ? 3.0 : -10.0); + }; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + + auto wildcard_ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(wildcard_ctrl_1.info.name, "wildcard_ctrl_1"); + ASSERT_EQ(wildcard_ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(wildcard_ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + verify_ctrl_parameter(wildcard_ctrl_1.c->get_node(), false); + + auto wildcard_ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(wildcard_ctrl_2.info.name, "wildcard_ctrl_2"); + ASSERT_EQ(wildcard_ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(wildcard_ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + verify_ctrl_parameter(wildcard_ctrl_2.c->get_node(), false); + + auto wildcard_ctrl_3 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(wildcard_ctrl_3.info.name, "wildcard_ctrl_3"); + ASSERT_EQ(wildcard_ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(wildcard_ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + verify_ctrl_parameter(wildcard_ctrl_3.c->get_node(), true); +} + TEST_F(TestLoadController, unload_on_kill) { // Launch spawner with unload on kill