bt_navigator_***_rclcpp_node - Namespacing + NodeOptions forwarding (#5310)

* `bt_navigator_***_rclcpp_node`  - Namespacing + NodeOptions forwarding

Closes #5242 
Inspired by: #5202 

- `bt_navigator_***_rclcpp_node` now gets the same namespace of  `bt_navigator`
- `bt_navigator_***_rclcpp_node` now gets the same node options of  `bt_navigator` (apart from node name, that is still forced to respect the pattern `bt_navigator_***_rclcpp_node` )

Signed-off-by: Patrick Roncagliolo <ronca.pat@gmail.com>

* Deduplicate `replaceOrAddArgument` implementation

Moved to `nav2_ros_common/node_utils.hpp`, namespace `nav2`

Signed-off-by: Patrick Roncagliolo <ronca.pat@gmail.com>

---------

Signed-off-by: Patrick Roncagliolo <ronca.pat@gmail.com>

Author: roncapat

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -137,15 +137,12 @@ bool BtActionServer<ActionT, NodeT>::on_configure()
   std::string client_node_name = action_name_;
   std::replace(client_node_name.begin(), client_node_name.end(), '/', '_');
   // Use suffix '_rclcpp_node' to keep parameter file consistency #1773
-  auto options = rclcpp::NodeOptions().arguments(
-    {"--ros-args",
-      "-r",
-      std::string("__node:=") +
-      std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node",
-      "-p",
-      "use_sim_time:=" +
-      std::string(node->get_parameter("use_sim_time").as_bool() ? "true" : "false"),
-      "--"});
+
+  auto new_arguments = node->get_node_options().arguments();
+  nav2::replaceOrAddArgument(new_arguments, "-r", "__node", std::string("__node:=") +
+    std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node");
+  auto options = node->get_node_options();
+  options = options.arguments(new_arguments);
 
   // Support for handling the topic-based goal pose from rviz
   client_node_ = std::make_shared<nav2::LifecycleNode>("_", options);

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -432,15 +432,6 @@ class Costmap2DROS : public nav2::LifecycleNode
 
 // 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

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -71,32 +71,17 @@ 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",
+  nav2::replaceOrAddArgument(new_arguments, "-r", "__ns",
       "__ns:=" + nav2::add_namespaces(parent_namespace, name));
-  replaceOrAddArgument(new_arguments, "-r", "__node", name + ":" + "__node:=" + name);
-  replaceOrAddArgument(new_arguments, "-p", "use_sim_time",
+  nav2::replaceOrAddArgument(new_arguments, "-r", "__node", name + ":" + "__node:=" + name);
+  nav2::replaceOrAddArgument(new_arguments, "-p", "use_sim_time",
       "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"));
   return rclcpp::NodeOptions().arguments(new_arguments);
 }

nav2_ros_common/include/nav2_ros_common/node_utils.hpp

@@ -378,6 +378,26 @@ inline void setIntrospectionMode(
   #endif
 }
 
+/**
+  * @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.
+  */
+inline 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);
+  }
+}
+
 }  // namespace nav2
 
 #endif  // NAV2_ROS_COMMON__NODE_UTILS_HPP_