Add controller node options args to be able to set controller specifi…

…c node options arguments
ros-controls · Aug 23, 2024 · ce3ba5c · ce3ba5c
1 parent 08492a1
commit ce3ba5c
Show file tree

Hide file tree

Showing 3 changed files with 42 additions and 0 deletions.
diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py
@@ -26,6 +26,7 @@
     load_controller,
     switch_controllers,
     unload_controller,
+    set_controller_parameters,
     set_controller_parameters_from_param_file,
     bcolors,
 )
@@ -122,6 +123,12 @@ def main(args=None):
         action="store_true",
         required=False,
     )
+    parser.add_argument(
+        "--controller-ros-args",
+        help="The --ros-args to be passed to the controller node for remapping topics etc",
+        default=None,
+        required=False,
+    )
 
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
@@ -172,6 +179,15 @@ def main(args=None):
                     + bcolors.ENDC
                 )
             else:
+                if controller_ros_args := args.controller_ros_args.split():
+                    if not set_controller_parameters(
+                        node,
+                        controller_manager_name,
+                        controller_name,
+                        "node_options_args",
+                        controller_ros_args,
+                    ):
+                        return 1
                 if param_file:
                     if not set_controller_parameters_from_param_file(
                         node,

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -509,6 +509,17 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
     controller_spec.info.fallback_controllers_names = fallback_controllers;
   }
 
+  const std::string node_options_args_param = controller_name + ".node_options_args";
+  std::vector<std::string> node_options_args;
+  if (!has_parameter(node_options_args_param))
+  {
+    declare_parameter(node_options_args_param, rclcpp::ParameterType::PARAMETER_STRING_ARRAY);
+  }
+  if (get_parameter(node_options_args_param, node_options_args) && !node_options_args.empty())
+  {
+    controller_spec.info.node_options_args = node_options_args;
+  }
+
   return add_controller_impl(controller_spec);
 }
 
@@ -2862,6 +2873,18 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options(
     node_options_arguments.push_back("use_sim_time:=true");
   }
 
+  // Add options parsed through the spawner
+  if (
+    !controller.info.node_options_args.empty() &&
+    !check_for_element(controller.info.node_options_args, ros_args_arg))
+  {
+    node_options_arguments.push_back(ros_args_arg);
+  }
+  for (const auto & arg : controller.info.node_options_args)
+  {
+    node_options_arguments.push_back(arg);
+  }
+
   std::string arguments;
   arguments.reserve(1000);
   for (const auto & arg : node_options_arguments)

diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp
@@ -41,6 +41,9 @@ struct ControllerInfo
 
   /// List of fallback controller names to be activated if this controller fails.
   std::vector<std::string> fallback_controllers_names;
+
+  /// Controller node options arguments
+  std::vector<std::string> node_options_args;
 };
 
 }  // namespace hardware_interface