From 7f5dd20914d8fe5036ecf29ea01f57073b45d1ac Mon Sep 17 00:00:00 2001
From: "Carlos A. Orduno" <39749557+orduno@users.noreply.github.com>
Date: Thu, 6 Jun 2019 15:13:53 -0700
Subject: [PATCH] Enable navigation goal preemption (#802)
---
.../nav2_bt_navigator/bt_navigator.hpp | 3 ++
nav2_bt_navigator/src/bt_navigator.cpp | 48 +++++++++++++------
.../nav2_tasks/behavior_tree_engine.hpp | 1 +
.../include/nav2_tasks/bt_action_node.hpp | 2 +-
nav2_tasks/src/behavior_tree_engine.cpp | 3 ++
.../nav2_util/simple_action_server.hpp | 16 +++++--
6 files changed, 52 insertions(+), 21 deletions(-)
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
index 10e449dd..0c3e849a 100644
--- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
@@ -54,6 +54,9 @@ protected:
// The action server callback
void navigateToPose(const std::shared_ptr<GoalHandle> goal_handle);
+ // Goal pose initialization on the blackboard
+ void initializeGoalPose(std::shared_ptr<GoalHandle> goal_handle);
+
// A subscription and callback to handle the topic-based goal published from rviz
void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose);
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index 6519dd22..ec97a08d 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -51,9 +51,10 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
self_client_ = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(
client_node_, "NavigateToPose");
- goal_sub_ = rclcpp_node_->create_subscription<geometry_msgs::msg::PoseStamped>("/move_base_simple/goal",
- rclcpp::SystemDefaultsQoS(),
- std::bind(&BtNavigator::onGoalPoseReceived, this, std::placeholders::_1));
+ goal_sub_ = rclcpp_node_->create_subscription<geometry_msgs::msg::PoseStamped>(
+ "/move_base_simple/goal",
+ rclcpp::SystemDefaultsQoS(),
+ std::bind(&BtNavigator::onGoalPoseReceived, this, std::placeholders::_1));
// Create an action server that we implement with our navigateToPose method
action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "NavigateToPose",
@@ -159,34 +160,39 @@ BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
void
BtNavigator::navigateToPose(const std::shared_ptr<GoalHandle> goal_handle)
{
- // Initialize the goal and result
- auto goal = goal_handle->get_goal();
+ // The action may be pre-empted, so keep a pointer to the current goal handle
+ std::shared_ptr<GoalHandle> current_goal_handle = goal_handle;
auto result = std::make_shared<nav2_msgs::action::NavigateToPose::Result>();
- RCLCPP_INFO(get_logger(), "Begin navigating from current location to (%.2f, %.2f)",
- goal->pose.pose.position.x, goal->pose.pose.position.y);
-
- // Update the goal pose on the blackboard
- *(blackboard_->get<geometry_msgs::msg::PoseStamped::SharedPtr>("goal")) = goal->pose;
+ initializeGoalPose(current_goal_handle);
// Execute the BT that was previously created in the configure step
- auto is_canceling = [goal_handle]() -> bool {return goal_handle->is_canceling();};
- nav2_tasks::BtStatus rc = bt_->run(tree_, is_canceling);
+ auto is_canceling = [¤t_goal_handle]() -> bool
+ {return current_goal_handle->is_canceling();};
+
+ auto on_loop = [this, ¤t_goal_handle] {
+ if (action_server_->preempt_requested()) {
+ current_goal_handle = action_server_->get_updated_goal_handle();
+ initializeGoalPose(current_goal_handle);
+ }
+ };
+
+ nav2_tasks::BtStatus rc = bt_->run(tree_, on_loop, is_canceling);
switch (rc) {
case nav2_tasks::BtStatus::SUCCEEDED:
RCLCPP_INFO(get_logger(), "Navigation succeeded");
- goal_handle->succeed(result);
+ current_goal_handle->succeed(result);
break;
case nav2_tasks::BtStatus::FAILED:
RCLCPP_ERROR(get_logger(), "Navigation failed");
- goal_handle->abort(result);
+ current_goal_handle->abort(result);
break;
case nav2_tasks::BtStatus::CANCELED:
RCLCPP_INFO(get_logger(), "Navigation canceled");
- goal_handle->canceled(result);
+ current_goal_handle->canceled(result);
// Reset the BT so that it can be run again in the future
bt_->resetTree(tree_->root_node);
break;
@@ -196,6 +202,18 @@ BtNavigator::navigateToPose(const std::shared_ptr<GoalHandle> goal_handle)
}
}
+void
+BtNavigator::initializeGoalPose(std::shared_ptr<GoalHandle> goal_handle)
+{
+ auto goal = goal_handle->get_goal();
+
+ RCLCPP_INFO(get_logger(), "Begin navigating from current location to (%.2f, %.2f)",
+ goal->pose.pose.position.x, goal->pose.pose.position.y);
+
+ // Update the goal pose on the blackboard
+ *(blackboard_->get<geometry_msgs::msg::PoseStamped::SharedPtr>("goal")) = goal->pose;
+}
+
void
BtNavigator::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose)
{
diff --git a/nav2_tasks/include/nav2_tasks/behavior_tree_engine.hpp b/nav2_tasks/include/nav2_tasks/behavior_tree_engine.hpp
index c399183e..3c851159 100644
--- a/nav2_tasks/include/nav2_tasks/behavior_tree_engine.hpp
+++ b/nav2_tasks/include/nav2_tasks/behavior_tree_engine.hpp
@@ -42,6 +42,7 @@ public:
BtStatus run(
std::unique_ptr<BT::Tree> & tree,
+ std::function<void()> onLoop,
std::function<bool()> cancelRequested,
std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));
diff --git a/nav2_tasks/include/nav2_tasks/bt_action_node.hpp b/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
index 54565b95..1e7ddaf8 100644
--- a/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
+++ b/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
@@ -136,7 +136,7 @@ new_goal_received:
// We can handle a new goal if we're still executing
auto status = goal_handle_->get_status();
- if (goal_updated_ && (status == action_msgs::msg::GoalStatus::STATUS_EXECUTING)) {
+ if (goal_updated_ && (status == action_msgs::msg::GoalStatus::STATUS_EXECUTING)) {
goal_updated_ = false;
goto new_goal_received;
}
diff --git a/nav2_tasks/src/behavior_tree_engine.cpp b/nav2_tasks/src/behavior_tree_engine.cpp
index 7c53cf8e..dc268e03 100644
--- a/nav2_tasks/src/behavior_tree_engine.cpp
+++ b/nav2_tasks/src/behavior_tree_engine.cpp
@@ -62,6 +62,7 @@ BehaviorTreeEngine::run(
BtStatus
BehaviorTreeEngine::run(
std::unique_ptr<BT::Tree> & tree,
+ std::function<void()> onLoop,
std::function<bool()> cancelRequested,
std::chrono::milliseconds loopTimeout)
{
@@ -70,6 +71,8 @@ BehaviorTreeEngine::run(
// Loop until something happens with ROS or the node completes w/ success or failure
while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
+ onLoop();
+
result = tree->root_node->executeTick();
// Check if we've received a cancel message
diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp
index 6d799280..ffa5b549 100644
--- a/nav2_util/include/nav2_util/simple_action_server.hpp
+++ b/nav2_util/include/nav2_util/simple_action_server.hpp
@@ -56,12 +56,12 @@ public:
// If we're currently working on a task, set a flag so that the
// action server can grab the pre-empting request in its loop
- if (received_handle_ != nullptr && received_handle_->is_active()) {
+ if (current_handle_ != nullptr && current_handle_->is_active()) {
preempt_requested_ = true;
- received_handle_ = handle;
+ new_handle_ = handle;
} else {
// Otherwise, safe to start a new task
- received_handle_ = handle;
+ current_handle_ = handle;
std::thread{execute_callback_, handle}.detach();
}
};
@@ -84,8 +84,13 @@ public:
get_updated_goal_handle()
{
std::lock_guard<std::mutex> lock(update_mutex_);
+
+ current_handle_->abort(std::make_shared<typename ActionT::Result>());
+ current_handle_ = new_handle_;
+ new_handle_.reset();
preempt_requested_ = false;
- return received_handle_;
+
+ return current_handle_;
}
protected:
@@ -96,7 +101,8 @@ protected:
std::mutex update_mutex_;
bool preempt_requested_{false};
- std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> received_handle_;
+ std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> current_handle_;
+ std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> new_handle_;
typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
};
--
GitLab