Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add logic for 'params_file' to handle both string and string_array #1898

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 25 additions & 16 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string> 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());
}
}

const std::string fallback_ctrl_param = controller_name + ".fallback_controllers";
Expand Down Expand Up @@ -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
Expand Down
38 changes: 38 additions & 0 deletions controller_manager/test/test_spawner_unspawner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -255,6 +255,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<std::string>({test_file_path}));
if (!ctrl_node->has_parameter("joint_names"))
{
ctrl_node->declare_parameter("joint_names", std::vector<std::string>({"random_joint"}));
}
ASSERT_THAT(
ctrl_node->get_parameter("joint_names").as_string_array(),
std::vector<std::string>({"joint0"}));
}

TEST_F(TestLoadController, spawner_test_type_in_params_file)
{
const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ struct ControllerInfo
std::string type;

/// Controller param file
std::optional<std::vector<std::string>> parameters_files;
std::vector<std::string> parameters_files;

/// List of claimed interfaces by the controller.
std::vector<std::string> claimed_interfaces;
Expand Down