From c12dffb682dd725fff5e1ecb1c17404906dd626e Mon Sep 17 00:00:00 2001
From: Steven Macenski <stevenmacenski@gmail.com>
Date: Thu, 5 Dec 2019 13:51:36 -0800
Subject: [PATCH] =?UTF-8?q?allow=20for=20remapping=20of=20the=20server=20n?=
=?UTF-8?q?ame=20for=20the=20follow=20path=20or=20compute=E2=80=A6=20(#136?=
=?UTF-8?q?5)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
* 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 | 6 +++---
.../nav2_behavior_tree/bt_action_node.hpp | 18 ++++++++++++------
.../action/compute_path_to_pose_action.cpp | 8 +++++++-
.../plugins/action/follow_path_action.cpp | 6 ++++++
4 files changed, 28 insertions(+), 10 deletions(-)
diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md
index 2d7e3971..21daa188 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 89818b80..de0ecfbc 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 77f4109d..cd4a8166 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 3809aab6..e22f94fc 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", "")
});
}
};
--
GitLab