move the NodeOptions logic into a separate method

ros-controls · Jan 22, 2024 · 604c0eb · 604c0eb
1 parent 201fd47
commit 604c0eb
Show file tree

Hide file tree

Showing 2 changed files with 46 additions and 29 deletions.
diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -414,6 +414,16 @@ class ControllerManager : public rclcpp::Node
     const std::vector<controller_manager::ControllerSpec> & controllers);
 
   void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+  /**
+   * @brief determine_controller_node_options - A method that retrieves the controller defined node
+   * options and adapts them, based on if there is a params file to be loaded or the use_sim_time
+   * needs to be set
+   * @param controller - controller info
+   * @return The node options that will be set to the controller LifeCycleNode
+   */
+  rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const;
+
   diagnostic_updater::Updater diagnostics_updater_;
 
   std::shared_ptr<rclcpp::Executor> executor_;

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -1317,35 +1317,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
     return nullptr;
   }
 
-  auto check_for_element = [](const auto & list, const auto & element)
-  { return std::find(list.begin(), list.end(), element) != list.end(); };
-
-  rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions().enable_logger_service(true);
-  std::vector<std::string> node_options_arguments = controller_node_options.arguments();
-  const std::string ros_args_arg = "--ros-args";
-  if (controller.info.parameters_file.has_value())
-  {
-    if (!check_for_element(node_options_arguments, ros_args_arg))
-    {
-      node_options_arguments.push_back(ros_args_arg);
-    }
-    node_options_arguments.push_back("--params-file");
-    node_options_arguments.push_back(controller.info.parameters_file.value());
-  }
-
-  // ensure controller's `use_sim_time` parameter matches controller_manager's
-  const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time");
-  if (use_sim_time.as_bool())
-  {
-    if (!check_for_element(node_options_arguments, ros_args_arg))
-    {
-      node_options_arguments.push_back(ros_args_arg);
-    }
-    node_options_arguments.push_back("-p");
-    node_options_arguments.push_back("use_sim_time:=true");
-  }
-
-  controller_node_options = controller_node_options.arguments(node_options_arguments);
+  const rclcpp::NodeOptions controller_node_options = determine_controller_node_options(controller);
   if (
     controller.c->init(
       controller.info.name, robot_description_, get_update_rate(), get_namespace(),
@@ -2628,4 +2600,39 @@ void ControllerManager::controller_activity_diagnostic_callback(
   }
 }
 
+rclcpp::NodeOptions ControllerManager::determine_controller_node_options(
+  const ControllerSpec & controller) const
+{
+  auto check_for_element = [](const auto & list, const auto & element)
+  { return std::find(list.begin(), list.end(), element) != list.end(); };
+
+  rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions().enable_logger_service(true);
+  std::vector<std::string> node_options_arguments = controller_node_options.arguments();
+  const std::string ros_args_arg = "--ros-args";
+  if (controller.info.parameters_file.has_value())
+  {
+    if (!check_for_element(node_options_arguments, ros_args_arg))
+    {
+      node_options_arguments.push_back(ros_args_arg);
+    }
+    node_options_arguments.push_back("--params-file");
+    node_options_arguments.push_back(controller.info.parameters_file.value());
+  }
+
+  // ensure controller's `use_sim_time` parameter matches controller_manager's
+  const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time");
+  if (use_sim_time.as_bool())
+  {
+    if (!check_for_element(node_options_arguments, ros_args_arg))
+    {
+      node_options_arguments.push_back(ros_args_arg);
+    }
+    node_options_arguments.push_back("-p");
+    node_options_arguments.push_back("use_sim_time:=true");
+  }
+
+  controller_node_options = controller_node_options.arguments(node_options_arguments);
+  return controller_node_options;
+}
+
 }  // namespace controller_manager