0. 简介
相比于ROS1的navigation而言，ROS2的navigation2已经将move_base 已被分割成多个组件。navigation 2不是一个单一的单体状态机，而是利用行动服务器（action server）和ROS 2的低延迟、可靠的通信来分离概念。行为树被用来协调这些任务。这允许Navigation 2具有高度可配置的导航行为，而无需通过在行为树xml文件中重新排列任务进行编程。
nav2_bt_navigator取代了顶层的move_base，用一个Action接口来完成基于树的动作模型的导航任务。它使用 Behavior Trees，使其有可能拥有更复杂的状态机，并作为额外的Action Servers加入恢复行为。
#==============控制器及其实现相关功能包======================#nav2_controller ｜ 控制器nav2_dwb_controller | DWB控制器，Nav2控制器的一个实现nav2_regulated_pure_pursuit_controller | 纯追踪控制器，Nav2控制器的一个实现#==============规划器及其实现相关功能包======================#nav2_planner | Nav2规划器nav2_navfn_planner ｜ navfn规划器，Nav2规划器的一个实现smac_planner | smac规划器，Nav2规划器的一个实现#=====================恢复器==============================#nav2_recoveries | Nav2恢复器#=====================行为树节点及其定义====================#nav2_bt_navigator | 导航行为树nav2_behavior_tree | 行为树节点插件定义#=====================地图和定位===========================#nav2_map_server ｜ 地图服务器nav2_costmap_2d ｜ 2D代价地图nav2_voxel_grid | 体素栅格nav2_amcl | 自适应蒙特卡洛定位。 状态估计，输入地图、激光、里程计数据，输出机器人map和odom之间的位资关系。#=====================通用插件系统管理等====================#nav2_bringup | 启动入口nav2_common ｜ 公共功能包nav2_msgs ｜ 通信相关消息定义nav2_util | 常用工具nav2_lifecycle_manager |节点生命周期管理器nav2_rviz_plugins | RVIZ插件#=====================核心定义============================#nav2_core ｜ Nav2核心包navigation2 | nav2导航汇总配置#=====================应用================================#nav2_waypoint_follower | 路点跟踪#=====================测试=================================#nav2_system_tests | 系统测试
规划、恢复和控制器服务器也是BT导航器可以调用的行动服务器，以进行计算。所有3个服务器都可以托管许多算法的插件，并单独从导航行为树中调用特定行为。提供的默认插件是从ROS 1移植过来的，即：DWB、NavFn，以及类似的恢复，如旋转和清除成本地图。
Nav2的中文网站在：
http://dev.nav2.fishros.com/doc/behavior_trees/overview/detailed_behavior_tree_walkthrough.html#navigate-to-pose-with-replanning-and-recovery
对于Nav2使用行为树（BT，Behavior Trees）调用模块化服务器来完成一个动作。动作可以是计算路径、控制工作（control efforts）、恢复或其他与导航相关的动作。这些动作都是通过动作服务器与行为树（BT）进行通信的独立节点，如下图所示：
1. Nav2实现
navigation 2导航任务的逻辑是通过package “nav2_bt_navigator”控制和维护的，包括路径计算，脱困恢复等，该package基于behavior tree实现对导航任务的逻辑状态维护。通过查看其默认调用的bt， “navigate_to_pose_w_replanning_and_recovery.xml”，可以发现在顺序执行的control node “NavigateWithReplanning”节点下的两个action nodes即为接收导航目标，计算路径，并开始遵循路径运行的导航过程
由于该过程由顺序执行的control node控制，即PipelineSequence，所以完全可以在路径计算和遵循路径两个节点中间加入原地旋转调整导航目标航向角的过程，如下图：
利用behavior tree的可视化工具Groot可以实现对bt的预览和编辑。Groot的使用超出本内容范围，不在此累述。在nav2_bt_navigator的behavior tree中，所有的action node即上图中蓝色节点，为该package的plugin，因此，希望能实现面向导航目标的原地旋转功能，即是创建一个能够被nav2_bt_navigator加载的自定义plugin。
2. 行为树含义
行为树（BT）主要在XML中定义。上面显示的树在 XML 中表示如下：
<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"/><ReactiveFallback name="ComputePathToPoseRecoveryFallback"><GoalUpdated/><ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/></ReactiveFallback></RecoveryNode></RateController><RecoveryNode number_of_retries="1" name="FollowPath"><FollowPath path="{path}" controller_id="FollowPath"/><ReactiveFallback name="FollowPathRecoveryFallback"><GoalUpdated/><ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/></ReactiveFallback></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.15" backup_speed="0.025"/></RoundRobin></ReactiveFallback></RecoveryNode></BehaviorTree></root>
这可能有点难以接受，但是这个树可以被分成两个更小的子树，我们可以一次关注一个。这些小的子树是最上面的 RecoveryNode 的子树。从现在开始， NavigateWithReplanning 称为是 Navigation 的子树，RecoveryFallback 称为是 Recovery的子树。这可以用以下方式表示:
这个 Navigation 子树实际上主要包含导航行为:
calculating a path
路径跟随
上述每种主要导航行为的恢复行为
这个 Recovery 子树包括系统故障恢复行为或者项目内部不易处理的恢复行为。整个行为树希望大部分时间花在“Navigation”子树中。如果 Navigation 子树中的两个主要行为有一个失败 (路径计算或路径跟随)，将尝试恢复整个系统。如果整个系统恢复仍然不够， Navigation 子树将返回 FAILURE 。系统将移动到 Recovery 子树，然后尝试清除所有系统级导航故障。
2.1 导航子树
既然我们已经讨论了 Navigation 子树和 Recovery 子树之间的控制流，下面让我们关注导航子树。
该子树的XML如下所示:
<PipelineSequence name="NavigateWithReplanning"><RateController hz="1.0"><RecoveryNode number_of_retries="1" name="ComputePathToPose"><ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/><ReactiveFallback name="ComputePathToPoseRecoveryFallback"><GoalUpdated/><ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/></ReactiveFallback></RecoveryNode></RateController><RecoveryNode number_of_retries="1" name="FollowPath"><FollowPath path="{path}" controller_id="FollowPath"/><ReactiveFallback name="FollowPathRecoveryFallback"><GoalUpdated/><ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/></ReactiveFallback></RecoveryNode></PipelineSequence>
这个树 ComputePathToPose 和 FollowPath 有两个主要作用。如果这两个操作有一个失败，他们将尝试根据上下文清除失败。树的关键只能用一个父节点和两个子节点来表示。
父节点PipelineSequence 允许 ComputePathToPose 被勾选，并且一旦被勾选， FollowPath 将被勾选。当 FollowPath 子树被勾选时， ComputePathToPose 子树也将被勾选。这允许在机器人四处移动时重新计算路径。
3. 如何自定义多控制器切换
其实本质上就是对行为树增加插件以供切换。《实现navigation2导航多控制器实时切换功能》（https://blog.csdn.net/lgh1231/article/details/136059494?spm=1001.2014.3001.5502）一文详细介绍了如何切换控制器，大致分为以下几步：
1、在controller server增加参数配置
2、行为树文件配置
这一部分主要讲到替换为controller_selector_node节点然后自动接受插件。
4. 自定义插件
4.1 创建一个新的规划器插件
我们将创建一个简单的直线规划器。本教程中带注释代码可以在 navigation_tutorials（https://github.com/ros-planning/navigation2_tutorials）仓库中的 nav2_straightline_planner 找到。这个软件包可以作为编写规划器插件的参考。
我们的示例插件继承自基类 nav2_core::GlobalPlanner。基类提供了5个纯虚方法来实现规划器插件。该插件将被规划器服务器用于计算路径。让我们更深入地了解编写规划器插件所需的方法。
在本教程中，我们将使用StraightLine::configure()和StraightLine::createPlan()方法来创建直线规划器。
在规划器中，configure()方法必须从ROS参数中设置成员变量，并进行任何所需的初始化。
node_ = parent;tf_ = tf;name_ = name;costmap_ = costmap_ros->getCostmap();global_frame_ = costmap_ros->getGlobalFrameID();// Parameter initializationnav2_util::declare_parameter_if_not_declared(node_, name_ + ".interpolation_resolution", rclcpp::ParameterValue(0.1));node_->get_parameter(name_ + ".interpolation_resolution", interpolation_resolution_);
这里，name_ + ".interpolation_resolution" 是获取特定于我们规划器的 ROS 参数interpolation_resolution。Nav2 允许加载多个插件，并为了保持组织结构的整洁性，每个插件都映射到某个 ID/ 名称。现在，如果我们想要检索特定插件的参数，我们使用.，就像上面的代码片段中所做的那样。例如，我们的示例规划器映射到名称“GridBased”，要检索特定于 “GridBased” 的interpolation_resolution参数，我们使用GridBased.interpolation_resolution 。换句话说，GridBased 被用作插件特定参数的命名空间。在讨论参数文件（或参数文件）时，我们将更详细地介绍这一点。
在 createPlan() 方法中，我们需要根据给定的起始和目标姿态创建路径。使用起始姿态和目标姿态调用StraightLine::createPlan()来解决全局路径规划问题。如果成功，它将将路径转换为nav_msgs::msg::Path并返回给规划器服务器。下面的注释显示了该方法的实现。
nav_msgs::msg::Path global_path;// Checking if the goal and start state is in the global frameif (start.header.frame_id != global_frame_) {RCLCPP_ERROR(node_->get_logger(), "Planner will only except start position from %s frame",global_frame_.c_str());return global_path;}if (goal.header.frame_id != global_frame_) {RCLCPP_INFO(node_->get_logger(), "Planner will only except goal position from %s frame",global_frame_.c_str());return global_path;}global_path.poses.clear();global_path.header.stamp = node_->now();global_path.header.frame_id = global_frame_;// calculating the number of loops for current value of interpolation_resolution_int total_number_of_loop = std::hypot(goal.pose.position.x - start.pose.position.x,goal.pose.position.y - start.pose.position.y) /interpolation_resolution_;double x_increment = (goal.pose.position.x - start.pose.position.x) / total_number_of_loop;double y_increment = (goal.pose.position.y - start.pose.position.y) / total_number_of_loop;for (int i = 0; i < total_number_of_loop; ++i) {geometry_msgs::msg::PoseStamped pose;pose.pose.position.x = start.pose.position.x + x_increment * i;pose.pose.position.y = start.pose.position.y + y_increment * i;pose.pose.position.z = 0.0;pose.pose.orientation.x = 0.0;pose.pose.orientation.y = 0.0;pose.pose.orientation.z = 0.0;pose.pose.orientation.w = 1.0;pose.header.stamp = node_->now();pose.header.frame_id = global_frame_;global_path.poses.push_back(pose);}global_path.poses.push_back(goal);return global_path;
剩余的方法虽然没有被使用，但是必须重写。根据规则，我们确实重写了所有这些方法，但是将它们留空。
4.2 导出规划器插件
现在我们已经创建了自定义规划器，我们需要导出我们的规划器插件，以便它对规划器服务器可见。插件在运行时加载，如果它们不可见，那么我们的规划器服务器将无法加载它。在ROS 2中，导出和加载插件是由pluginlib处理的。
回到我们的教程，nav2_straightline_planner::StraightLine类以动态方式加载为我们的基类nav2_core::GlobalPlanner。
4.2.1 要导出规划器，我们需要提供两行代码：
PLUGINLIB_EXPORT_CLASS(nav2_straightline_planner::StraightLine, nav2_core::GlobalPlanner)
将这两行代码放在文件末尾是一个良好的实践，但从技术上讲，你也可以将其放在文件的顶部。
4.2.2 接下来的步骤是在包的根目录中创建插件的描述文件。例如，在我们的教程包中创建global_planner_plugin.xml文件。该文件包含以下信息：
library path: 插件库的名称和位置。
class name: 类的名称。
class type: 类的类型。
base class: 基类的名称。
description: 插件的描述。
<library path="nav2_straightline_planner_plugin"><class name="nav2_straightline_planner/StraightLine" type="nav2_straightline_planner::StraightLine" base_class_type="nav2_core::GlobalPlanner"><description>This is an example plugin which produces straight path.</description></class></library>
4.2.3 接下来的步骤是使用 CMakeLists.txt 导出插件，通过使用 cmake 函数pluginlib_export_plugin_description_file()。这个函数将插件描述文件安装到 share目录，并设置ament索引以使其可被发现。
pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml)4.2.4 插件描述文件也应该添加到package.xml文件中。
<export><build_type>ament_cmake</build_type><nav2_core plugin="${prefix}/global_planner_plugin.xml" /></export>
4.2.5 编译后，插件应该已注册。接下来，我们将使用这个插件
4.3 通过参数文件传递插件名称。
为了启用插件，我们需要修改nav2_params.yaml文件，如下所示，替换以下参数：
备注：
从Galactic版本开始，plugin_names和plugin_types已被替换为一个plugins字符串向量用于插件名称。类型现在在plugin_name命名空间中的plugin字段中定义（例如，plugin: MyPlugin::Plugin）。代码块中的内联注释将帮助指导你进行操作。
planner_server:ros__parameters:plugins: ["GridBased"]use_sim_time: TrueGridBased:plugin: "nav2_navfn_planner/NavfnPlanner" # For Foxy and latertolerance: 2.0use_astar: falseallow_unknown: true
with
planner_server:ros__parameters:plugins: ["GridBased"]use_sim_time: TrueGridBased:plugin: "nav2_straightline_planner/StraightLine"interpolation_resolution: 0.1
在上面的代码片段中，你可以观察到我们将nav2_straightline_planner/StraightLine规划器映射到其id GridBased。为了传递插件特定的参数，我们使用了.的格式。
4.4 运行 StraightLine 插件
使用启用了Navigation2的Turtlebot3仿真。如何制作的详细说明已在“入门指南”中编写。以下是一个快捷命令：
ros2 launch nav2_bringup tb3_simulation_launch.py params_file:=/path/to/your_params_file.yaml然后进入RViz，点击顶部的“2D姿态估计”按钮，并在地图上指向位置，就像在“入门指南”中描述的那样。机器人将在地图上进行定位，然后点击“Navigation2目标”，并单击您希望规划器考虑的目标姿态。之后，规划器将规划路径，机器人将开始朝着目标移动
5. plugin使用
5.1 利用ros2 lifecycle command line interface验证
在终端中先运行btnav_action_server，然后打开一个新终端，输入lifecycle command line interface验证action server。查看btnav_action_server当前可用状态，在终端输入命令：
ros2 lifecycle list btnav_action_server将显示自定义action server当前可用的状态，比如当前可以transit的两个状态“configure”和“shutdown”，如下图：
5.2 查看btnav_action_server当前状态：
在终端输入命令：
ros2 lifecycle get /btnav_action_server将显示自定义action server当前的状态，比如当前状态“unconfigured”，如下图：
5.3 查看状态迁移的ID：
在终端输入命令：
ros2 interface show lifecycle_msgs/msg/Transition将显示自定义action server可迁移状态的ID，如下图：
5.4 执行configure：
在终端输入命令：
ros2 lifecycle set /btnav_action_server configure将显示状态迁移成功，同时，运行action server终端显示创建并正在配置自定义的插件“spinGO”，如下图：
5.5 激活插件：
通过上述查看到的可迁移状态ID，比如“activate=3”，尝试切换状态到active，在终端输入命令：
ros2 service call /btnav_action_server/change_state lifecycle_msgs/srv/ChangeState "{transition: {id: 3}}"将显示状态迁移成功，同时，运行action server终端显示正在激活自定义的插件“spinGO”，如下图：
6. 参考链接
https://blog.csdn.net/qq_19598969/article/details/124159293
https://blog.csdn.net/weixin_45710350/article/details/109543288
https://blog.csdn.net/lgh1231/article/details/136059494
https://blog.csdn.net/m0_60346726/article/details/134087938
https://blog.51cto.com/u_15127514/4321104
https://zhuanlan.zhihu.com/p/662142400
招募要求
完成符合要求的机器人相关视频制作
总时长需达到 3小时以上
视频内容需为精品课程，确保高质量和专业性
讲师奖励
享受课程收入分成
赠送 2门 古月学院在售精品课程（训练营除外）
联系我们
添加工作人员微信：GYH-xiaogu
点击“阅读原文”查看详情
