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

…1898)
ros-controls · Nov 29, 2024 · c9a0ab9 · c9a0ab9
1 parent 73fe227
commit c9a0ab9
Show file tree

Hide file tree

Showing 3 changed files with 64 additions and 17 deletions.
diff --git 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<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";
@@ -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/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp
@@ -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") +

diff --git 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<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;