Navigation2 详解与自定义使用

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导航多控制器实时切换功能》一文详细介绍了如何切换控制器，大致分为以下几步：

1. 在controller server增加参数配置

2. 行为树文件配置

这一部分主要讲到替换为controller_selector_node节点然后自动接受插件

4. 自定义插件

4.1 创建一个新的规划器插件

我们将创建一个简单的直线规划器。本教程中带注释代码可以在 navigation_tutorials 仓库中的 nav2_straightline_planner 找到。这个软件包可以作为编写规划器插件的参考。
  我们的示例插件继承自基类 nav2_core::GlobalPlanner。基类提供了5个纯虚方法来实现规划器插件。该插件将被规划器服务器用于计算路径。让我们更深入地了解编写规划器插件所需的方法。

虚方法	方法描述	是否需要重写？
configure()	当规划器服务器进入 on_configure 状态时调用该方法。理想情况下，此方法应执行 ROS 参数的声明和规划器成员变量的初始化。此方法接受4个输入参数：指向父节点的共享指针、规划器名称、tf 缓冲区指针和代价地图的共享指针。	是
activate()	当规划器服务器进入on_activate状态时调用该方法。理想情况下，此方法应实现在规划器进入活动状态之前必要的操作。	是
deactivate()	当规划器服务器进入on_deactivate状态时调用该方法。理想情况下，此方法应实现在规划器进入非活动状态之前必要的操作。	是
cleanup()	当规划器服务器进入on_cleanup状态时调用该方法。理想情况下，此方法应清理为规划器创建的资源。	是
createPlan()	当规划器服务器要求为指定的起点和目标姿态提供全局计划时，调用该方法。该方法返回携带全局计划的 nav_msgs::msg::Path。此方法接受2个输入参数：起始姿态和目标姿态。	是

在本教程中，我们将使用 StraightLine::configure() 和 StraightLine::createPlan() 方法来创建直线规划器。
  在规划器中，configure() 方法必须从ROS参数中设置成员变量，并进行任何所需的初始化。

node_ = parent;
tf_ = tf;
name_ = name;
costmap_ = costmap_ros->getCostmap();
global_frame_ = costmap_ros->getGlobalFrameID();

// Parameter initialization
nav2_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/ 名称。现在，如果我们想要检索特定插件的参数，我们使用<mapped_name_of_plugin>.<name_of_parameter>，就像上面的代码片段中所做的那样。例如，我们的示例规划器映射到名称“GridBased”，要检索特定于 “GridBased” 的 interpolation_resolution 参数，我们使用 GridBased.interpolation_resolution 。换句话说，GridBased 被用作插件特定参数的命名空间。在讨论参数文件（或参数文件）时，我们将更详细地介绍这一点。
  在 createPlan() 方法中，我们需要根据给定的起始和目标姿态创建路径。使用起始姿态和目标姿态调用StraightLine::createPlan()来解决全局路径规划问题。如果成功，它将将路径转换为nav_msgs::msg::Path并返回给规划器服务器。下面的注释显示了该方法的实现。

…详情请参照古月居