@小伊 导航的点都是可以正常到达的，到达第一个点之后控制服务已经打印到达目标点，然后就是返回值给行为树，行为树再返回值给waypoint_follower.cpp，waypoint_follower.cpp收到成功之后才会发送下一个点。
bt_action_node.hpp行为树节点代码如下：
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
#include <memory>
#include <string>
#include <chrono>
#include "behaviortree_cpp_v3/action_node.h"
#include "nav2_util/node_utils.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_behavior_tree/bt_conversions.hpp"
namespace nav2_behavior_tree
{
using namespace std::chrono_literals; // NOLINT
/**
* @brief Abstract class representing an action based BT node
* @tparam ActionT Type of action
*/
template<class ActionT>
class BtActionNode : public BT::ActionNodeBase
{
public:
/**
* @brief A nav2_behavior_tree::BtActionNode constructor
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
BtActionNode(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name), should_send_goal_(true)
{
node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false);
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
// Get the required items from the blackboard
auto bt_loop_duration =
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
server_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
wait_for_service_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
// timeout should be less than bt_loop_duration to be able to finish the current tick
max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);
// Initialize the input and output messages
goal_ = typename ActionT::Goal();
result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
std::string remapped_action_name;
if (getInput("server_name", remapped_action_name)) {
action_name_ = remapped_action_name;
}
createActionClient(action_name_);
// Give the derive class a chance to do any initialization
RCLCPP_DEBUG(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str());
}
BtActionNode() = delete;
virtual ~BtActionNode()
{
}
/**
* @brief Create instance of an action client
* @param action_name Action name to create client for
*/
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, callback_group_);
// Make sure the server is actually there before continuing
RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
RCLCPP_ERROR(
node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
action_name.c_str(),
wait_for_service_timeout_.count() / 1000.0);
throw std::runtime_error(
std::string("Action server ") + action_name +
std::string(" not available"));
}
}
/**
* @brief Any subclass of BtActionNode that accepts parameters must provide a
* providedPorts method and call providedBasicPorts in it.
* @param addition Additional ports to add to BT port list
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedBasicPorts(BT::PortsList addition)
{
BT::PortsList basic = {
BT::InputPort<std::string>("server_name", "Action server name"),
BT::InputPort<std::chrono::milliseconds>("server_timeout")
};
basic.insert(addition.begin(), addition.end());
return basic;
}
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts()
{
return providedBasicPorts({});
}
// Derived classes can override any of the following methods to hook into the
// processing for the action: on_tick, on_wait_for_result, and on_success
/**
* @brief Function to perform some user-defined operation on tick
* Could do dynamic checks, such as getting updates to values on the blackboard
*/
virtual void on_tick()
{
}
/**
* @brief Function to perform some user-defined operation after a timeout
* waiting for a result that hasn't been received yet. Also provides access to
* the latest feedback message from the action server. Feedback will be nullptr
* in subsequent calls to this function if no new feedback is received while waiting for a result.
* @param feedback shared_ptr to latest feedback message, nullptr if no new feedback was received
*/
virtual void on_wait_for_result(std::shared_ptr<const typename ActionT::Feedback>/*feedback*/)
{
}
/**
* @brief Function to perform some user-defined operation upon successful
* completion of the action. Could put a value on the blackboard.
* @return BT::NodeStatus Returns SUCCESS by default, user may override return another value
*/
virtual BT::NodeStatus on_success()
{
return BT::NodeStatus::SUCCESS;
}
/**
* @brief Function to perform some user-defined operation when the action is aborted.
* @return BT::NodeStatus Returns FAILURE by default, user may override return another value
*/
virtual BT::NodeStatus on_aborted()
{
return BT::NodeStatus::FAILURE;
}
/**
* @brief Function to perform some user-defined operation when the action is cancelled.
* @return BT::NodeStatus Returns SUCCESS by default, user may override return another value
*/
virtual BT::NodeStatus on_cancelled()
{
return BT::NodeStatus::SUCCESS;
}
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick() override
{
// first step to be done only at the beginning of the Action
if (status() == BT::NodeStatus::IDLE) {
// setting the status to RUNNING to notify the BT Loggers (if any)
setStatus(BT::NodeStatus::RUNNING);
// reset the flag to send the goal or not, allowing the user the option to set it in on_tick
should_send_goal_ = true;
// user defined callback, may modify "should_send_goal_".
on_tick();
if (!should_send_goal_) {
return BT::NodeStatus::FAILURE;
}
send_new_goal();
}
try {
// if new goal was sent and action server has not yet responded
// check the future goal handle
if (future_goal_handle_) {
auto elapsed = (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
if (!is_future_goal_handle_complete(elapsed)) {
// return RUNNING if there is still some time before timeout happens
if (elapsed < server_timeout_) {
return BT::NodeStatus::RUNNING;
}
// if server has taken more time than the specified timeout value return FAILURE
RCLCPP_WARN(
node_->get_logger(),
"Timed out while waiting for action server to acknowledge goal request for %s",
action_name_.c_str());
future_goal_handle_.reset();
return BT::NodeStatus::FAILURE;
}
}
// The following code corresponds to the "RUNNING" loop
if (rclcpp::ok() && !goal_result_available_) {
// user defined callback. May modify the value of "goal_updated_"
on_wait_for_result(feedback_);
// reset feedback to avoid stale information
feedback_.reset();
auto goal_status = goal_handle_->get_status();
if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
{
goal_updated_ = false;
send_new_goal();
auto elapsed = (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
if (!is_future_goal_handle_complete(elapsed)) {
if (elapsed < server_timeout_) {
return BT::NodeStatus::RUNNING;
}
RCLCPP_WARN(
node_->get_logger(),
"Timed out while waiting for action server to acknowledge goal request for %s",
action_name_.c_str());
future_goal_handle_.reset();
return BT::NodeStatus::FAILURE;
}
}
callback_group_executor_.spin_some();
// check if, after invoking spin_some(), we finally received the result
if (!goal_result_available_) {
// Yield this Action, returning RUNNING
return BT::NodeStatus::RUNNING;
}
}
} catch (const std::runtime_error & e) {
if (e.what() == std::string("send_goal failed") ||
e.what() == std::string("Goal was rejected by the action server"))
{
// Action related failure that should not fail the tree, but the node
return BT::NodeStatus::FAILURE;
} else {
// Internal exception to propagate to the tree
throw e;
}
}
BT::NodeStatus status;
switch (result_.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_ERROR(node_->get_logger(),"rclcpp_action::ResultCode::SUCCEEDED");
status = on_success();
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(node_->get_logger(),"rclcpp_action::ResultCode::ABORTED");
status = on_aborted();
break;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(node_->get_logger(),"rclcpp_action::ResultCode::CANCELED");
status = on_cancelled();
break;
default:
throw std::logic_error("BtActionNode::Tick: invalid status value");
}
goal_handle_.reset();
return status;
}
/**
* @brief The other (optional) override required by a BT action. In this case, we
* make sure to cancel the ROS2 action if it is still running.
*/
void halt() override
{
if (should_cancel_goal()) {
auto future_result = action_client_->async_get_result(goal_handle_);
auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(
node_->get_logger(),
"Failed to cancel action server for %s", action_name_.c_str());
}
if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(
node_->get_logger(),
"Failed to get result for %s in node halt!", action_name_.c_str());
}
on_cancelled();
}
setStatus(BT::NodeStatus::IDLE);
}
protected:
/**
* @brief Function to check if current goal should be cancelled
* @return bool True if current goal should be cancelled, false otherwise
*/
bool should_cancel_goal()
{
// Shut the node down if it is currently running
if (status() != BT::NodeStatus::RUNNING) {
return false;
}
// No need to cancel the goal if goal handle is invalid
if (!goal_handle_) {
return false;
}
callback_group_executor_.spin_some();
auto status = goal_handle_->get_status();
// Check if the goal is still executing
return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
status == action_msgs::msg::GoalStatus::STATUS_EXECUTING;
}
/**
* @brief Function to send new goal to action server
*/
void send_new_goal()
{
goal_result_available_ = false;
auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
send_goal_options.result_callback =
[this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
if (future_goal_handle_) {
RCLCPP_DEBUG(
node_->get_logger(),
"Goal result for %s available, but it hasn't received the goal response yet. "
"It's probably a goal result for the last goal request", action_name_.c_str());
return;
}
// TODO(#1652): a work around until rcl_action interface is updated
// if goal ids are not matched, the older goal call this callback so ignore the result
// if matched, it must be processed (including aborted)
if (this->goal_handle_->get_goal_id() == result.goal_id) {
goal_result_available_ = true;
result_ = result;
}
};
send_goal_options.feedback_callback =
[this](typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
const std::shared_ptr<const typename ActionT::Feedback> feedback) {
feedback_ = feedback;
};
future_goal_handle_ = std::make_shared<
std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>(
action_client_->async_send_goal(goal_, send_goal_options));
time_goal_sent_ = node_->now();
}
/**
* @brief Function to check if the action server acknowledged a new goal
* @param elapsed Duration since the last goal was sent and future goal handle has not completed.
* After waiting for the future to complete, this value is incremented with the timeout value.
* @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise
*/
bool is_future_goal_handle_complete(std::chrono::milliseconds & elapsed)
{
auto remaining = server_timeout_ - elapsed;
// server has already timed out, no need to sleep
if (remaining <= std::chrono::milliseconds(0)) {
future_goal_handle_.reset();
return false;
}
auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;
auto result =
callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout);
elapsed += timeout;
if (result == rclcpp::FutureReturnCode::INTERRUPTED) {
future_goal_handle_.reset();
throw std::runtime_error("send_goal failed");
}
if (result == rclcpp::FutureReturnCode::SUCCESS) {
goal_handle_ = future_goal_handle_->get();
future_goal_handle_.reset();
if (!goal_handle_) {
throw std::runtime_error("Goal was rejected by the action server");
}
return true;
}
return false;
}
/**
* @brief Function to increment recovery count on blackboard if this node wraps a recovery
*/
void increment_recovery_count()
{
int recovery_count = 0;
config().blackboard->template get<int>("number_recoveries", recovery_count); // NOLINT
recovery_count += 1;
config().blackboard->template set<int>("number_recoveries", recovery_count); // NOLINT
}
std::string action_name_;
typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
// All ROS2 actions have a goal and a result
typename ActionT::Goal goal_;
bool goal_updated_{false};
bool goal_result_available_{false};
typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle_;
typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult result_;
// To handle feedback from action server
std::shared_ptr<const typename ActionT::Feedback> feedback_;
// The node that will be used for any ROS operations
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
// The timeout value while waiting for response from a server when a
// new action goal is sent or canceled
std::chrono::milliseconds server_timeout_;
// The timeout value for BT loop execution
std::chrono::milliseconds max_timeout_;
// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;
// To track the action server acknowledgement when a new goal is sent
std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
future_goal_handle_;
rclcpp::Time time_goal_sent_;
// Can be set in on_tick or on_wait_for_result to indicate if a goal should be sent.
bool should_send_goal_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
navigate_to_pose.cpp代码如下：
// Copyright (c) 2021 Samsung Research
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <vector>
#include <string>
#include <set>
#include <memory>
#include <limits>
#include "nav2_bt_navigator/navigators/navigate_to_pose.hpp"
namespace nav2_bt_navigator
{
bool
NavigateToPoseNavigator::configure(
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother)
{
start_time_ = rclcpp::Time(0);
auto node = parent_node.lock();
if (!node->has_parameter("goal_blackboard_id")) {
node->declare_parameter("goal_blackboard_id", std::string("goal"));
}
goal_blackboard_id_ = node->get_parameter("goal_blackboard_id").as_string();
if (!node->has_parameter("path_blackboard_id")) {
node->declare_parameter("path_blackboard_id", std::string("path"));
}
path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string();
// Odometry smoother object for getting current speed
odom_smoother_ = odom_smoother;
self_client_ = rclcpp_action::create_client<ActionT>(node, getName());
goal_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(
"goal_pose",
rclcpp::SystemDefaultsQoS(),
std::bind(&NavigateToPoseNavigator::onGoalPoseReceived, this, std::placeholders::_1));
return true;
}
std::string
NavigateToPoseNavigator::getDefaultBTFilepath(
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node)
{
std::string default_bt_xml_filename;
auto node = parent_node.lock();
if (!node->has_parameter("default_nav_to_pose_bt_xml")) {
std::string pkg_share_dir =
ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
node->declare_parameter<std::string>(
"default_nav_to_pose_bt_xml",
pkg_share_dir +
"/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml");
}
node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename);
return default_bt_xml_filename;
}
bool
NavigateToPoseNavigator::cleanup()
{
goal_sub_.reset();
self_client_.reset();
return true;
}
bool
NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
{
auto bt_xml_filename = goal->behavior_tree;
if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
RCLCPP_ERROR(
logger_, "BT file not found: %s. Navigation canceled.",
bt_xml_filename.c_str());
return false;
}
initializeGoalPose(goal);
return true;
}
void
NavigateToPoseNavigator::goalCompleted(
typename ActionT::Result::SharedPtr /*result*/,
const nav2_behavior_tree::BtStatus /*final_bt_status*/)
{
}
void
NavigateToPoseNavigator::onLoop()
{
// action server feedback (pose, duration of task,
// number of recoveries, and distance remaining to goal)
auto feedback_msg = std::make_shared<ActionT::Feedback>();
geometry_msgs::msg::PoseStamped current_pose;
nav2_util::getCurrentPose(
current_pose, *feedback_utils_.tf,
feedback_utils_.global_frame, feedback_utils_.robot_frame,
feedback_utils_.transform_tolerance);
auto blackboard = bt_action_server_->getBlackboard();
try {
// Get current path points
nav_msgs::msg::Path current_path;
blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path);
// Find the closest pose to current pose on global path
auto find_closest_pose_idx =
[¤t_pose, ¤t_path]() {
size_t closest_pose_idx = 0;
double curr_min_dist = std::numeric_limits<double>::max();
for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
double curr_dist = nav2_util::geometry_utils::euclidean_distance(
current_pose, current_path.poses[curr_idx]);
if (curr_dist < curr_min_dist) {
curr_min_dist = curr_dist;
closest_pose_idx = curr_idx;
}
}
return closest_pose_idx;
};
// Calculate distance on the path
double distance_remaining =
nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx());
// Default value for time remaining
rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0);
// Get current speed
geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist();
double current_linear_speed = std::hypot(current_odom.linear.x, current_odom.linear.y);
// Calculate estimated time taken to goal if speed is higher than 1cm/s
// and at least 10cm to go
if ((std::abs(current_linear_speed) > 0.01) && (distance_remaining > 0.1)) {
estimated_time_remaining =
rclcpp::Duration::from_seconds(distance_remaining / std::abs(current_linear_speed));
}
feedback_msg->distance_remaining = distance_remaining;
feedback_msg->estimated_time_remaining = estimated_time_remaining;
} catch (...) {
// Ignore
}
int recovery_count = 0;
blackboard->get<int>("number_recoveries", recovery_count);
feedback_msg->number_of_recoveries = recovery_count;
feedback_msg->current_pose = current_pose;
feedback_msg->navigation_time = clock_->now() - start_time_;
bt_action_server_->publishFeedback(feedback_msg);
}
void
NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
{
RCLCPP_INFO(logger_, "Received goal preemption request");
if (goal->behavior_tree == bt_action_server_->getCurrentBTFilename() ||
(goal->behavior_tree.empty() &&
bt_action_server_->getCurrentBTFilename() == bt_action_server_->getDefaultBTFilename()))
{
// if pending goal requests the same BT as the current goal, accept the pending goal
// if pending goal has an empty behavior_tree field, it requests the default BT file
// accept the pending goal if the current goal is running the default BT file
initializeGoalPose(bt_action_server_->acceptPendingGoal());
} else {
RCLCPP_WARN(
logger_,
"Preemption request was rejected since the requested BT XML file is not the same "
"as the one that the current goal is executing. Preemption with a new BT is invalid "
"since it would require cancellation of the previous goal instead of true preemption."
"\nCancel the current goal and send a new action request if you want to use a "
"different BT XML file. For now, continuing to track the last goal until completion.");
bt_action_server_->terminatePendingGoal();
}
}
void
NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
{
geometry_msgs::msg::PoseStamped current_pose;
nav2_util::getCurrentPose(
current_pose, *feedback_utils_.tf,
feedback_utils_.global_frame, feedback_utils_.robot_frame,
feedback_utils_.transform_tolerance);
RCLCPP_INFO(
logger_, "Begin navigating from current location (%.2f, %.2f) to (%.2f, %.2f)",
current_pose.pose.position.x, current_pose.pose.position.y,
goal->pose.pose.position.x, goal->pose.pose.position.y);
// Reset state for new action feedback
start_time_ = clock_->now();
auto blackboard = bt_action_server_->getBlackboard();
blackboard->set<int>("number_recoveries", 0); // NOLINT
// Update the goal pose on the blackboard
blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal->pose);
}
void
NavigateToPoseNavigator::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose)
{
ActionT::Goal goal;
goal.pose = *pose;
self_client_->async_send_goal(goal);
}
} // namespace nav2_bt_navigator
