bt_navigator_***_rclcpp_node - Namespacing + NodeOptions forwarding

Closes ros-navigation#5242 
Inspired by: ros-navigation#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 <[email protected]>

Author: roncapat

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -125,6 +125,21 @@ template<class ActionT, class NodeT>
 BtActionServer<ActionT, NodeT>::~BtActionServer()
 {}
 
+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);
+  }
+}
+
 template<class ActionT, class NodeT>
 bool BtActionServer<ActionT, NodeT>::on_configure()
 {
@@ -137,15 +152,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();
+  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);