diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 2d7e3971aefd017e0f26c5aecf56378bdcd379c2..21daa18882f4def7fa1a9174d9992ae4d98ba8ba 100644 --- a/nav2_behavior_tree/README.md +++ b/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. | diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 89818b807f4ee6ab29f896dbfae219de6a8c0a9b..de0ecfbc2659ec8557ae398fc6a11287e312f061 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/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) diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index 77f4109d684472a685b2f4e8a40008690e14b793..cd4a816614dff2767652c79362ba3a92e79c4ad0 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/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", "") }); } diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 3809aab64e135ec96aa0e29edf1de88066ecdb89..e22f94fcee72faa307fb1c174c03c7ab57deb936 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/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", "") }); } };