ROS2入门到精通—— 1-7 ROS2实战：行为树详解

1 行为树简介

在这里插入图片描述

行为树：一种用于描述和执行复杂系统中行为的图形化结构，Nav2中，行为树用于定义机器人导航的决策和行为
（1）一个名为 "tick "的信号被发送到树的根部，并在树中传播，直到它到达叶子节点
（2）任何收到tick信号的TreeNode都会执行其回调函数。这个回调函数必须有如下返回值：SUCCESS、FAILURE、RUNNING

行为树由节点组成，节点分为控制节点和执行节点

控制节点：用于组织和控制叶子节点的执行逻辑

顺序、备选、并行、装饰

执行节点（叶子节点）：代表具体的行为或任务

执行节点，条件节点

在这里插入图片描述父节点可以触发子节点，称为依次tick，子节点会返回执行状态，包括：

成功(success)
失败(failure)
运行中(running)

1.1 控制节点

(1）顺序节点：顺序执行所有子节点，如果一个子节点失败，则停止执行并返回失败；所有节点执行成功，返回成功
在这里插入图片描述
（2）备选节点：顺序执行所有子节点，如果一个子节点成功，则停止执行返回成功；如果失败，接着往下执行

（3）并行节点：并行执行所有孩子节点。直到至少M个孩子（M的值在1到N之间）节点返回成功状态或所有节点返回失败状态
在这里插入图片描述
（4）装饰节点：修改、装饰其子节点的执行。例如，可以有一个重试装饰器，使其子节点在失败时重试

黑板：一种数据存储和共享机制，用于行为树的不同部分之间传递信息和维护状态

1.2 执行节点

（1）任务节点（Action Node）：执行具体的任务或行为，例如移动到目标位置、执行某个动作

（2）条件节点（Condition Node）：根据某些条件来决定是否执行其子节点，如果条件满足，执行成功子节点，否则，执行失败节点
在这里插入图片描述

1.3 行为树实例

使用行为树控制机器人同时寻找苹果和橘子
在这里插入图片描述上面如果节点多了，不方便构造行为树，使用黑板和装饰节点简化行为树
行为树编辑工具Groot：用于ROS的图形化工具，用于可视化和管理机器人运行的状态、节点通信和数据流

相关安装自行搜索
BehaviorTree.CPP

2 Nav2独有的控制节点

(1) 流水线节点
在这里插入图片描述 (2) 恢复节点

(3) RoundRobin

2 Nav2行为树导航服务

在这里插入图片描述 BT Navigator Server导航的主要逻辑就是通过这个驱动的，配合行为树插件。外面给它发一个目标点，走点请求调用行为树，然后再调用全局规划，局部规划，最后再到这个底盘，让小车移动起来

在这里插入图片描述
行为树可以解决的问题：
● 更高效的展示逻辑流程
● 更快速的逻辑修改
● 完善运行监控系统
● 日志系统（能够解决逻辑Bug）

默认导航到点行为树分析
在这里插入图片描述

<!--This Behavior Tree replans the global path periodically at 1 Hz and it also hasrecovery actions specific to planning / control as well as general system issues.This will be continuous if a kinematically valid planner is selected.该行为树以1Hz周期性地重新规划全局路径，并且它还具有特定于计划/控制的恢复操作以及一般系统问题。如果选择了运动学有效的规划器，这将是连续
-->
<root main_tree_to_execute="MainTree"><BehaviorTree ID="MainTree"><RecoveryNode number_of_retries="6" name="NavigateRecovery"><PipelineSequence name="NavigateWithReplanning"><RateController hz="1.0"><RecoveryNode number_of_retries="1" name="ComputePathToPose"><ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/><ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/></RecoveryNode></RateController><RecoveryNode number_of_retries="1" name="FollowPath"><FollowPath path="{path}" controller_id="FollowPath"/><ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/></RecoveryNode></PipelineSequence><ReactiveFallback name="RecoveryFallback"><GoalUpdated/><RoundRobin name="RecoveryActions"><Sequence name="ClearingActions"><ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/><ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/></Sequence><Spin spin_dist="1.57"/><Wait wait_duration="5"/><BackUp backup_dist="0.30" backup_speed="0.05"/></RoundRobin></ReactiveFallback></RecoveryNode></BehaviorTree>
</root>

在自由空间中从一个起点导航到一个单点目标。既包含在特定子上下文中使用自定义恢复，也包含用于系统级故障的全局恢复子树。它还为用户提供了在返回失败状态之前多次重试任务的机会。

这个ComputePathToPose和FollowPath BT节点都指定了它们要使用的算法。按照惯例，我们用它们的算法风格来命名它们 (例如，不是 DWB ，而是 FollowPath )，在此行为树中，我们尝试重试整个导航任务6次，然后将任务失败状态返回给调用方。这使得导航系统有足够的机会尝试从故障情况中恢复或等待瞬态问题通过

在名义上的执行中，这将每秒重新规划路径并将该路径传递给控制器，类似Nav2行为树中的行为树。然而，这一次，如果规划器失败了，它将在其子树中触发上下文感知的恢复，从而清除全局成本地图。可以在此处添加其他恢复以进行其他特定于上下文的恢复，例如尝试其他算法，虽然这个行为树没有利用它，但是 PlannerSelector 、 ControllerSelector 和 GoalCheckerSelector 行为树节点也是有帮助的。这些行为树节点将允许用户通过ROS话题动态地改变导航系统中使用的算法，而不是对要使用的算法 ( GridBased 和 FollowPath ) 进行硬编码。相反，在最有用和最独特的情况下，使用具有指定算法的条件节点创建不同的子树上下文可能是可取的。然而，选择器节点可以成为一种有用的方式，从外部应用程序而不是通过内部行为树控制流逻辑来改变算法。最好通过行为树方法来实现更改，但是我们知道许多专业用户都有外部应用程序来动态更改其导航器的设置。

3 源码解析

move_base 已被分割成多个组件。navigation 2不是一个单一的单体状态机，而是利用行动服务器(action server)和ROS 2的低延迟、可靠的通信来分离概念。

行为树被用来协调这些任务。这允许Navigation 2具有高度可配置的导航行为，而无需通过在行为树xml文件中重新排列任务进行编程。nav2_bt_navigator取代了顶层的move_base，用一个Action接口来完成基于树的动作模型的导航任务。它使用Behavior Trees，使其有可能拥有更复杂的状态机，并作为额外的Action Servers加入恢复行为。

BT导航器（行为树导航器）模块实现NavigateToPose和NavigateThroughPoses任务界面。它是一种基于行为树的导航实现，旨在实现导航任务的灵活性，并提供一种轻松指定复杂机器人行为的方法。
在这里插入图片描述 bt_navigator.cpp

#include "nav2_bt_navigator/bt_navigator.hpp"#include <memory>
#include <string>
#include <utility>
#include <set>
#include <limits>
#include <vector>#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_behavior_tree/bt_conversions.hpp"namespace nav2_bt_navigator
{
// 行为树构造函数
BtNavigator::BtNavigator(const rclcpp::NodeOptions & options)
: nav2_util::LifecycleNode("bt_navigator", "", options)
{RCLCPP_INFO(get_logger(), "Creating");// 插件const std::vector<std::string> plugin_libs = {"nav2_compute_path_to_pose_action_bt_node",       // 计算路径到位姿"nav2_compute_path_through_poses_action_bt_node", // 计算路径中位姿"nav2_smooth_path_action_bt_node",                // 平滑路径"nav2_follow_path_action_bt_node",                // follow path"nav2_spin_action_bt_node",                       // 旋转"nav2_wait_action_bt_node",                       // 等待"nav2_assisted_teleop_action_bt_node",            // 辅助遥控"nav2_back_up_action_bt_node",                    // 往后退"nav2_drive_on_heading_bt_node",                  // 航向行驶"nav2_clear_costmap_service_bt_node",             // 清除代价地图服务"nav2_is_stuck_condition_bt_node",                // 卡住条件"nav2_goal_reached_condition_bt_node",            // 达到目标条件"nav2_initial_pose_received_condition_bt_node",   // 初始位姿接收条件"nav2_goal_updated_condition_bt_node",            // 目标更新条件"nav2_globally_updated_goal_condition_bt_node",   // nav2全局更新目标条件"nav2_is_path_valid_condition_bt_node",           // 路径是否符合条件"nav2_reinitialize_global_localization_service_bt_node", // 重新初始化全局定位服务"nav2_rate_controller_bt_node",                   // rate controller"nav2_distance_controller_bt_node",               // 距离控制器"nav2_speed_controller_bt_node",                  // 速度控制器"nav2_truncate_path_action_bt_node",              // 截断路径操作"nav2_truncate_path_local_action_bt_node",        // 截断局部路径"nav2_goal_updater_node_bt_node",                 // 目标更新节点"nav2_recovery_node_bt_node",                     // 恢复节点"nav2_pipeline_sequence_bt_node",                 // 管道序列"nav2_round_robin_node_bt_node",                  // round_robin节点"nav2_transform_available_condition_bt_node",     // 变换可用条件"nav2_time_expired_condition_bt_node",            // 超时条件"nav2_path_expiring_timer_condition",             // 路径过期计时器条件"nav2_distance_traveled_condition_bt_node",       // 行驶距离条件"nav2_single_trigger_bt_node",                    // 单触发器"nav2_goal_updated_controller_bt_node",           // 目标更新控制器"nav2_is_battery_low_condition_bt_node",          // 电池电量是否不足"nav2_navigate_through_poses_action_bt_node",     // navigate through poses"nav2_navigate_to_pose_action_bt_node",           // navigate to pose"nav2_remove_passed_goals_action_bt_node",        // 删除已通过的目标"nav2_planner_selector_bt_node",                  // 规划器选择器"nav2_controller_selector_bt_node",               // 控制器选择器"nav2_goal_checker_selector_bt_node",             // 目标检查选择器"nav2_controller_cancel_bt_node",                 // 控制器取消"nav2_path_longer_on_approach_bt_node",           // 接近路径"nav2_wait_cancel_bt_node",                       // 等待取消"nav2_spin_cancel_bt_node",                       // 等待旋转"nav2_assisted_teleop_cancel_bt_node",            // 辅助遥控取消"nav2_back_up_cancel_bt_node",                    // 向后退取消"nav2_back_up_cancel_bt_node",                    // 向后退取消"nav2_drive_on_heading_cancel_bt_node"            // 取消航向行驶};declare_parameter("plugin_lib_names", plugin_libs);              // 转换容差插件库名称declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); // 转换容差 0.1declare_parameter("global_frame", std::string("map"));           // 全局帧 mapdeclare_parameter("robot_base_frame", std::string("base_link")); // 机器人参考帧：base_linkdeclare_parameter("odom_topic", std::string("odom"));            // 里程计话题：odom
}
// 析构函数
BtNavigator::~BtNavigator()
{
}
// 配置变量回调函数
nav2_util::CallbackReturn
BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
{RCLCPP_INFO(get_logger(), "Configuring");// 创建一个tf2_ros::Buffer对象，用于管理TF变换tf_ = std::make_shared<tf2_ros::Buffer>(get_clock());auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(get_node_base_interface(), get_node_timers_interface());tf_->setCreateTimerInterface(timer_interface);tf_->setUsingDedicatedThread(true);// 创建一个tf2_ros::TransformListener对象，用于监听TF变换tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, this, false);// 设置全局框架、机器人框架、变换容差和odom主题global_frame_ = get_parameter("global_frame").as_string();robot_frame_ = get_parameter("robot_base_frame").as_string();transform_tolerance_ = get_parameter("transform_tolerance").as_double();odom_topic_ = get_parameter("odom_topic").as_string();// Libraries to pull plugins (BT Nodes) from// 获取插件库名称auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array();// 创建导航到姿态导航器和导航过姿态导航器pose_navigator_ = std::make_unique<nav2_bt_navigator::NavigateToPoseNavigator>();poses_navigator_ = std::make_unique<nav2_bt_navigator::NavigateThroughPosesNavigator>();// 创建FeedbackUtils对象，用于处理导航过程中的反馈信息nav2_bt_navigator::FeedbackUtils feedback_utils;feedback_utils.tf = tf_;feedback_utils.global_frame = global_frame_;feedback_utils.robot_frame = robot_frame_;feedback_utils.transform_tolerance = transform_tolerance_;// Odometry smoother object for getting current speed// 创建一个OdomSmoother对象，用于获取当前速度odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(shared_from_this(), 0.3, odom_topic_);// 配置导航到姿态导航器和导航过姿态导航器if (!pose_navigator_->on_configure(shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_)){return nav2_util::CallbackReturn::FAILURE;}if (!poses_navigator_->on_configure(shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_)){return nav2_util::CallbackReturn::FAILURE;}return nav2_util::CallbackReturn::SUCCESS;
}nav2_util::CallbackReturn
BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/)
{RCLCPP_INFO(get_logger(), "Activating");// 激活导航到姿态导航器和导航过姿态导航器if (!poses_navigator_->on_activate() || !pose_navigator_->on_activate()) {return nav2_util::CallbackReturn::FAILURE;}// create bond connection// 创建连接createBond();return nav2_util::CallbackReturn::SUCCESS;
}nav2_util::CallbackReturn
BtNavigator::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
{RCLCPP_INFO(get_logger(), "Deactivating");// 清理导航到姿态导航器和导航过姿态导航器if (!poses_navigator_->on_deactivate() || !pose_navigator_->on_deactivate()) {return nav2_util::CallbackReturn::FAILURE;}// destroy bond connection// 破坏连接destroyBond();return nav2_util::CallbackReturn::SUCCESS;
}nav2_util::CallbackReturn
BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
{RCLCPP_INFO(get_logger(), "Cleaning up");// Reset the listener before the buffer// 重置listener和buffertf_listener_.reset();tf_.reset();if (!poses_navigator_->on_cleanup() || !pose_navigator_->on_cleanup()) {return nav2_util::CallbackReturn::FAILURE;}// 释放资源poses_navigator_.reset();pose_navigator_.reset();RCLCPP_INFO(get_logger(), "Completed Cleaning up");return nav2_util::CallbackReturn::SUCCESS;
}nav2_util::CallbackReturn
BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
{RCLCPP_INFO(get_logger(), "Shutting down");// 这里返回 SUCCESS 表示导航器正在正常关闭return nav2_util::CallbackReturn::SUCCESS;
}}  // namespace nav2_bt_navigator#include "rclcpp_components/register_node_macro.hpp"// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
// 使用class_loader注册组件。
// 这起到了一种入口点的作用，允许组件在其库时被发现
// 正在加载到正在运行的进程中。
RCLCPP_COMPONENTS_REGISTER_NODE(nav2_bt_navigator::BtNavigator)

navigate_to_pose.cpp

#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 speedodom_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;
}
// 获取默认BT文件路径
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 pointsnav_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 pathauto find_closest_pose_idx =[&current_pose, &current_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 goif ((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 fileinitializeGoalPose(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)
{RCLCPP_INFO(logger_, "Begin navigating from current location to (%.2f, %.2f)",goal->pose.pose.position.x, goal->pose.pose.position.y);// Reset state for new action feedbackstart_time_ = clock_->now();auto blackboard = bt_action_server_->getBlackboard();blackboard->set<int>("number_recoveries", 0);  // NOLINT// Update the goal pose on the blackboardblackboard->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