Pass node options to costmap node (#5202)

* Passing parent node options from controller_server to costmap node

Signed-off-by: Marco Bassa <[email protected]>

* Passing parent node options to global costmap

Signed-off-by: Marco Bassa <[email protected]>

---------

Signed-off-by: Marco Bassa <[email protected]>

Author: MarcoMatteoBassa

nav2_controller/src/controller_server.cpp

@@ -68,7 +68,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
   // The costmap node is used in the implementation of the controller
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "local_costmap", std::string{get_namespace()},
-    get_parameter("use_sim_time").as_bool());
+    get_parameter("use_sim_time").as_bool(), options);
 }
 
 ControllerServer::~ControllerServer()

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -91,7 +91,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   explicit Costmap2DROS(
     const std::string & name,
     const std::string & parent_namespace = "/",
-    const bool & use_sim_time = false);
+    const bool & use_sim_time = false,
+    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
   /**
    * @brief Common initialization for constructors
@@ -430,6 +431,27 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
 };
 
+// free functions
+
+/**
+  * @brief Given a specified argument name, replaces it with the specified
+  * new value. If the argument is not in the existing list, a new argument
+  * is created with the specified option.
+  */
+void replaceOrAddArgument(
+  std::vector<std::string> & arguments, const std::string & option,
+  const std::string & arg_name, const std::string & new_argument);
+
+/**
+  * @brief Given the node options of a parent node, expands of replaces
+  *         the fields for the node name, namespace and use_sim_time
+  */
+rclcpp::NodeOptions getChildNodeOptions(
+  const std::string & name,
+  const std::string & parent_namespace,
+  const bool & use_sim_time,
+  const rclcpp::NodeOptions & parent_options);
+
 }  // namespace nav2_costmap_2d
 
 #endif  // NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -71,21 +71,48 @@ Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options)
   init();
 }
 
+void replaceOrAddArgument(
+  std::vector<std::string> & arguments, const std::string & option,
+  const std::string & arg_name, const std::string & new_argument)
+{
+  auto argument = std::find_if(arguments.begin(), arguments.end(),
+      [arg_name](const std::string & value){return value.find(arg_name) != std::string::npos;});
+  if (argument != arguments.end()) {
+    *argument = new_argument;
+  } else {
+    arguments.push_back("--ros-args");
+    arguments.push_back(option);
+    arguments.push_back(new_argument);
+  }
+}
+
+rclcpp::NodeOptions getChildNodeOptions(
+  const std::string & name,
+  const std::string & parent_namespace,
+  const bool & use_sim_time,
+  const rclcpp::NodeOptions & parent_options)
+{
+  std::vector<std::string> new_arguments = parent_options.arguments();
+  replaceOrAddArgument(new_arguments, "-r", "__ns",
+      "__ns:=" + nav2_util::add_namespaces(parent_namespace, name));
+  replaceOrAddArgument(new_arguments, "-r", "__node", name + ":" + "__node:=" + name);
+  replaceOrAddArgument(new_arguments, "-p", "use_sim_time",
+      "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"));
+  return rclcpp::NodeOptions().arguments(new_arguments);
+}
+
 Costmap2DROS::Costmap2DROS(
   const std::string & name,
   const std::string & parent_namespace,
-  const bool & use_sim_time)
+  const bool & use_sim_time,
+  const rclcpp::NodeOptions & options)
 : nav2_util::LifecycleNode(name, "",
     // NodeOption arguments take precedence over the ones provided on the command line
     // use this to make sure the node is placed on the provided namespace
     // TODO(orduno) Pass a sub-node instead of creating a new node for better handling
     //              of the namespaces
-    rclcpp::NodeOptions().arguments({
-    "--ros-args", "-r", std::string("__ns:=") +
-    nav2_util::add_namespaces(parent_namespace, name),
-    "--ros-args", "-r", name + ":" + std::string("__node:=") + name,
-    "--ros-args", "-p", "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"),
-  })),
+    getChildNodeOptions(name, parent_namespace, use_sim_time, options)
+),
   name_(name),
   default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"},
   default_types_{

nav2_planner/src/planner_server.cpp

@@ -64,7 +64,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
   // Setup the global costmap
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "global_costmap", std::string{get_namespace()},
-    get_parameter("use_sim_time").as_bool());
+    get_parameter("use_sim_time").as_bool(), options);
 }
 
 PlannerServer::~PlannerServer()