@小伊 导航的点都是可以正常到达的，到达第一个点之后控制服务已经打印到达目标点，然后就是返回值给行为树，行为树再返回值给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
