From efed75143631e63cb5eb3163989a8909481140f5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 28 Nov 2024 22:49:39 +0100 Subject: [PATCH 1/5] add logic for 'params_file' to handle both string and string_array --- controller_manager/src/controller_manager.cpp | 39 ++++++++++++------- .../hardware_interface/controller_info.hpp | 2 +- 2 files changed, 25 insertions(+), 16 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ebab7a2674..a9caec1110 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -551,16 +551,28 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c // read_only params, dynamic maps lists etc // Now check if the parameters_file parameter exist const std::string param_name = controller_name + ".params_file"; - std::vector parameters_files; + controller_spec.info.parameters_files.clear(); // Check if parameter has been declared - if (!has_parameter(param_name)) - { - declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); - } - if (get_parameter(param_name, parameters_files) && !parameters_files.empty()) + rclcpp::Parameter params_files_parameter; + if (get_parameter(param_name, params_files_parameter)) { - controller_spec.info.parameters_files = parameters_files; + if (params_files_parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) + { + controller_spec.info.parameters_files = params_files_parameter.as_string_array(); + } + else if (params_files_parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + { + controller_spec.info.parameters_files.push_back(params_files_parameter.as_string()); + } + else + { + RCLCPP_ERROR( + get_logger(), + "The 'params_file' param needs to be a string or a string array for '%s', but it is of " + "type %s", + controller_name.c_str(), params_files_parameter.get_type_name().c_str()); + } } const std::string fallback_ctrl_param = controller_name + ".fallback_controllers"; @@ -3250,17 +3262,14 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back(arg); } - if (controller.info.parameters_files.has_value()) + for (const auto & parameters_file : controller.info.parameters_files) { - for (const auto & parameters_file : controller.info.parameters_files.value()) + if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) { - if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) - { - node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); - } - node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); - node_options_arguments.push_back(parameters_file); + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); } + node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); + node_options_arguments.push_back(parameters_file); } // ensure controller's `use_sim_time` parameter matches controller_manager's diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 4563e2dc75..3ad89551d5 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -34,7 +34,7 @@ struct ControllerInfo std::string type; /// Controller param file - std::optional> parameters_files; + std::vector parameters_files; /// List of claimed interfaces by the controller. std::vector claimed_interfaces; From bb88d531c0fb17928a90d014bca5f393998f0ace Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 29 Nov 2024 00:42:52 +0100 Subject: [PATCH 2/5] Add test to the case of the parsing a string path --- .../test/test_spawner_unspawner.cpp | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index ca580b1130..78230d7404 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -255,6 +255,41 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param) } } +TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + cm_->set_parameter(rclcpp::Parameter( + "ctrl_with_parameters_and_type.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter( + rclcpp::Parameter("ctrl_with_parameters_and_type.params_file", test_file_path)); + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner("ctrl_with_parameters_and_type --load-only -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({test_file_path})); + ctrl_with_parameters_and_type.c->get_node()->declare_parameter( + "joint_names", std::vector({"random_joint"})); + ASSERT_THAT( + ctrl_with_parameters_and_type.c->get_node()->get_parameter("joint_names").as_string_array(), + std::vector({"joint0"})); +} + TEST_F(TestLoadController, spawner_test_type_in_params_file) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + From 0bdb06f5469792b711d34ab31176573ed6c72e91 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 29 Nov 2024 00:43:19 +0100 Subject: [PATCH 3/5] Update docs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- 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 a9caec1110..1a725b21fc 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -553,7 +553,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c const std::string param_name = controller_name + ".params_file"; controller_spec.info.parameters_files.clear(); - // Check if parameter has been declared + // get_parameter checks if parameter has been declared rclcpp::Parameter params_files_parameter; if (get_parameter(param_name, params_files_parameter)) { From 597d46b38af6f90c86410fb5bb1feb61125f82a3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 29 Nov 2024 08:05:15 +0100 Subject: [PATCH 4/5] Fix the compatibility build by checking if the parameter has already been declared --- controller_manager/test/test_spawner_unspawner.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 78230d7404..cd2eee31f8 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -280,13 +280,16 @@ TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); ASSERT_THAT( ctrl_with_parameters_and_type.info.parameters_files, std::vector({test_file_path})); - ctrl_with_parameters_and_type.c->get_node()->declare_parameter( - "joint_names", std::vector({"random_joint"})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } ASSERT_THAT( - ctrl_with_parameters_and_type.c->get_node()->get_parameter("joint_names").as_string_array(), + ctrl_node->get_parameter("joint_names").as_string_array(), std::vector({"joint0"})); } From 72483f002963c07af79820199b5156fd388379d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 29 Nov 2024 09:47:52 +0100 Subject: [PATCH 5/5] Clarify comment --- 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 1a725b21fc..82546aaeda 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -553,7 +553,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c const std::string param_name = controller_name + ".params_file"; controller_spec.info.parameters_files.clear(); - // get_parameter checks if parameter has been declared + // get_parameter checks if parameter has been declared/set rclcpp::Parameter params_files_parameter; if (get_parameter(param_name, params_files_parameter)) {