allow for remapping of the server name for the follow path or compute… (#1365)

* allow for remapping of the server name for the follow path or compute control effort for remote or multiple servers

* adding reference to server name remap in bt action node

nav2_behavior_tree/README.md

@@ -9,7 +9,7 @@ This module is used by the nav2_bt_navigator to implement a ROS2 node that execu
 
 ## The bt_action_node Template and the Behavior Tree Engine
 
-The [bt_action_node template](include/nav2_behavior_tree/bt_action_node.hpp) allows one to easily integrate a ROS2 action into a BehaviorTree. To do so, one derives from the BTActionNode template, providing the action message type. For example,
+The [bt_action_node template](include/nav2_behavior_tree/bt_action_node.hpp) allows one to easily integrate a ROS2 action into a BehaviorTree. To do so, one derives from the BtActionNode template, providing the action message type. For example,
 
 ```C++
 #include "nav2_msgs/action/follow_path.hpp"
@@ -66,8 +66,8 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
 | BT Node   |      Type      |  Description |
 |----------|:-------------|------|
 | Backup |  Action | Invokes the BackUp ROS2 action server, which causes the robot to back up to a specific pose. This is used in nav2 Behavior Trees as a recovery behavior. The nav2_recoveries module implements the BackUp action server. |
-| ComputePathToPose |    Action   | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_planner module. |
-| FollowPath | Action |Invokes the FollowPath ROS2 action server, which is implemented by the controller plugin modules loaded. |
+| ComputePathToPose |    Action   | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_planner module. The server address can be remapped using the `server_name` input port. |
+| FollowPath | Action |Invokes the FollowPath ROS2 action server, which is implemented by the controller plugin modules loaded. The server address can be remapped using the `server_name` input port. |
 | GoalReached | Condition | Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, otherwise it returns FAILURE. |
 | IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. |
 | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. |

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -46,12 +46,7 @@ public:
       config().blackboard->get<std::chrono::milliseconds>("server_timeout");
     getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
 
-    // Now that we have the ROS node to use, create the action client for this BT action
-    action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name_);
-
-    // Make sure the server is actually there before continuing
-    RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" action server", action_name_.c_str());
-    action_client_->wait_for_action_server();
+    createActionClient(action_name_);
 
     // Give the derive class a chance to do any initialization
     RCLCPP_INFO(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str());
@@ -63,6 +58,17 @@ public:
   {
   }
 
+  // Create instance of an action server
+  void createActionClient(const std::string & action_name)
+  {
+    // Now that we have the ROS node to use, create the action client for this BT action
+    action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name);
+
+    // Make sure the server is actually there before continuing
+    RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
+    action_client_->wait_for_action_server();
+  }
+
   // Any subclass of BtActionNode that accepts parameters must provide a providedPorts method
   // and call providedBasicPorts in it.
   static BT::PortsList providedBasicPorts(BT::PortsList addition)

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -34,6 +34,11 @@ public:
     const BT::NodeConfiguration & conf)
   : BtActionNode<nav2_msgs::action::ComputePathToPose>(xml_tag_name, action_name, conf)
   {
+    std::string remapped_action_name;
+    if (getInput("server_name", remapped_action_name)) {
+      action_client_.reset();
+      createActionClient(remapped_action_name);
+    }
   }
 
   void on_tick() override
@@ -58,7 +63,8 @@ public:
     return providedBasicPorts({
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
-        BT::InputPort<std::string>("planner_id", "")
+        BT::InputPort<std::string>("planner_id", ""),
+        BT::InputPort<std::string>("server_name", "")
       });
   }
 

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -33,6 +33,11 @@ public:
     const BT::NodeConfiguration & conf)
   : BtActionNode<nav2_msgs::action::FollowPath>(xml_tag_name, action_name, conf)
   {
+    std::string remapped_action_name;
+    if (getInput("server_name", remapped_action_name)) {
+      action_client_.reset();
+      createActionClient(remapped_action_name);
+    }
     config().blackboard->set("path_updated", false);
   }
 
@@ -61,6 +66,7 @@ public:
     return providedBasicPorts({
         BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
         BT::InputPort<std::string>("controller_id", ""),
+        BT::InputPort<std::string>("server_name", "")
       });
   }
 };