From d41c2a00f015d1d6f5502170cb88834470bc4e06 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 29 Nov 2024 10:41:45 +0100 Subject: [PATCH] add logic for 'params_file' to handle both string and string_array (#1898) --- controller_manager/src/controller_manager.cpp | 41 +++++++++++-------- .../test/test_spawner_unspawner.cpp | 38 +++++++++++++++++ .../hardware_interface/controller_info.hpp | 2 +- 3 files changed, 64 insertions(+), 17 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5f21c8e4ac..1f387b89ee 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -595,16 +595,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)) + // get_parameter checks if parameter has been declared/set + rclcpp::Parameter params_files_parameter; + if (get_parameter(param_name, params_files_parameter)) { - declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); - } - if (get_parameter(param_name, parameters_files) && !parameters_files.empty()) - { - 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()); + } } return add_controller_impl(controller_spec); @@ -2565,17 +2577,14 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true); std::vector node_options_arguments = controller_node_options.arguments(); - 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/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 43018b7a43..ecb238d78d 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -220,6 +220,44 @@ 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); + 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})); + 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({"joint0"})); +} + TEST_F(TestLoadController, spawner_test_type_in_arg) { ControllerManagerRunner cm_runner(this); diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index d684596d55..3e25ca731e 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;