【Nav2中文网】九、Nav2行为树（二）详细的行为树演练

最新推荐文章于 2024-01-26 13:14:57 发布

鱼香ROS

最新推荐文章于 2024-01-26 13:14:57 发布

阅读量893

点赞数

分类专栏： Nav2中文网文章标签： nav nav2

本文链接：https://blog.csdn.net/qq_27865227/article/details/124376569

版权

Nav2中文网专栏收录该内容

121 篇文章 57 订阅

订阅专栏

本教程来自：Nav2中文网
Nav2交流社区：https://fishros.org.cn/forum
ROS2/Nav2千人交流群：(QQ)139707339
更多精彩教程请关注微信公众号：鱼香ROS
欢迎添加机器人小伊微信，解锁机器人学习特殊服务

详细的行为树演练

概述

本文作为Nav2中主要行为树 (BT) 的参考指南。 [校准@123—向佐转]

在 nav2_bt_navigator/behavior_trees 中提供了行为树示例，但是这些示例必须基于机器人的应用进行重新配置。以下文档将详细介绍当前主要默认的BT navigate_to_pose_w_replanning_and_recovery.xml 。 [校准@123—向佐转]

前提条件

在继续之前，请熟悉行为树的概念 [校准@123—向佐转]
- 阅读导航相关概念介绍的简短解释 [校准@mzebra]
- 阅读 BehaviorTree CPP V3 网站上的一般教程和指南 (非Nav2专用)。具体来说 BehaviorTree CPP V3 网站上的 “Learn the Basics” 部分解释了本指南基础部分所使用的一般节点类型的介绍。 [校准@mzebra]
熟悉 Nav2 specific BT nodes [校准@123—向佐转]

通过重新规划和恢复导航到某个位姿

下节将详细描述目前在Nav2、 navigate_to_pose_w_replanning_and_recovery.xml 中使用的主行为树和默认行为树的概念。此行为树以1Hz定期重新规划全局路径，并且它还具有恢复操作。 [校准@123—向佐转]

行为树（BT）主要在XML中定义。上面显示的树在 XML 中表示如下。 [校准@123—向佐转]

<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 称为是 [](http://dev.nav2.fishros.com/doc/behavior_trees/overview/detailed_behavior_tree_walkthrough.html#id1)Recovery的子树。这可以用以下方式表示: [校准@123—向佐转]

这个 Navigation 子树实际上主要包含导航行为: [校准@123—向佐转]

calculating a path
路径跟随 [校准@123—向佐转]
上述每种主要导航行为的恢复行为 [校准@123—向佐转]

这个 Recovery 子树包括系统故障恢复行为或者项目内部不易处理的恢复行为。 [校准@123—向佐转]

整个行为树希望大部分时间花在“Navigation”子树中。如果 Navigation 子树中的两个主要行为有一个失败 (路径计算或路径跟随)，将尝试恢复整个系统。 [校准@123—向佐转]

如果整个系统恢复仍然不够， Navigation 子树将返回 FAILURE 。系统将移动到 Recovery 子树，然后尝试清除所有系统级导航故障。 [校准@123—向佐转]

这种情况发生，直到原系统 RecoveryNode 被number_of_retries 超过 (默认情况下为6)。 [校准@123—向佐转]

<RecoveryNode number_of_retries="6" name="NavigateRecovery">

导航子树

既然我们已经讨论了 Navigation 子树和 Recovery 子树之间的控制流，下面让我们关注导航子树。 [校准@123—向佐转]

该子树的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 有两个主要作用。如果这两个操作有一个失败，他们将尝试根据上下文清除失败。树的关键只能用一个父节点和两个子节点来表示，如下所示: [校准@123—向佐转]

父节点PipelineSequence 允许 ComputePathToPose 被勾选，并且一旦被勾选， FollowPath 将被勾选。当 FollowPath 子树被勾选时， ComputePathToPose 子树也将被勾选。这允许在机器人四处移动时重新计算路径。 [校准@123—向佐转]

ComputePathToPose 和 FollowPath 都遵循相同的总体结构。 [校准@123—向佐转]

执行动作 [校准@123—向佐转]
如果动作执行失败，请尝试查看我们是否可以上下文恢复 [校准@123—向佐转]

The below is the ComputePathToPose subtree:

父级 RecoveryNode 控制动作和上下文恢复子树之间的流程。 ComputePathToPose 和 FollowPath 的上下文恢复包括检查目标是否已经更新，和清除相关的成本图。 [校准@123—向佐转]

如果应用程序在进入系统之前可以接受较多的上下文恢复尝试，您可以更改父“RecoveryNode”控制节点中的“number_of_retries”参数。 [校准@123—向佐转]

在BT子树ComputePathToPose 和 [](http://dev.nav2.fishros.com/doc/behavior_trees/overview/detailed_behavior_tree_walkthrough.html#id1)FollowPath中的唯一区别概述如下: [校准@123—向佐转]

子树中的动作节点: [校准@123—向佐转]* ComputePathToPose 子树以ComputePathToPose 动作为中心。 [校准@123—向佐转]
- FollowPath 子树以 FollowPath 动作为中心。 [校准@123—向佐转]
修饰ComputePathToPose子树的RateController [校准@123—向佐转]RateController``控制``ComputePathToPose``子树以保证规划在指定的频率。BT树的默认频率是1 hz。这样做是为了防止BT以树更新速率 (100Hz) 用太多无用的请求浪费规划服务器资源。根据应用和计算路径的计算成本，可以考虑将该频率更改为更高或更低的频率。还有其他装饰可以用来代替这个 ``RateController 。如果合适，考虑使用 SpeedController 或 DistanceController 装饰器。 [校准@123—向佐转]
在上下文恢复中清除costmap: [校准@小鱼]* 这个 ComputePathToPose 子树清除全球成本图。全球成本图是planner背景下的相关成本图 [待校准@144]
- 这个 FollowPath 子树清除了当地的成本图。本地costmap是控制者上下文中的相关costmap [待校准@145]

恢复（Recovery）子树

这个 Recovery 子树是Nav2默认 navigate_to_pose_w_replanning_and_recovery.xml 树的第二大 “half” 。简而言之，当 Navigation 子树返回 FAILURE 控制系统级别的恢复时，就会触发这个子树 (在这种情况下， Navigation 子树中的上下文恢复是不够的)。 [待校准@147]

以及XML片段: [待校准@148]

<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>

最顶级的父级， ReactiveFallback 控制系统范围内其余恢复之间的流程，并异步检查是否收到新目标。如果在任何时候目标被更新，这个子树将停止所有的孩子并返回 SUCCESS 。这允许对新目标的快速反应和抢占当前执行恢复。对于 Navigation 子树的上下文恢复部分来说，这应该是熟悉的。这是处理这种情况的常见BT模式，“除非发生’this condition’，否则请采取行动”。 [待校准@149]

这些条件节点非常强大，通常与 ReactiveFallback 配对。可以很容易地想象用 isBatteryLow 条件将整个 navigate_to_pose_w_replanning_and_recovery 树包裹在 ReactiveFallback 中 – 这意味着 navigate_to_pose_w_replanning_and_recovery 树将执行 * 除非 * 电池电量变低 (然后整个不同的子树对接充电)。 [待校准@150]

如果目标从未更新，行为树将继续到 RoundRobin 节点。以下是BT中默认的四个系统级恢复: [待校准@151]

清除两个成本图 (本地和全局) 的序列 [待校准@152]
Spin recovery action
Wait recovery action
BackUp recovery action

在 SUCCESS 的四个孩子父 RoundRobin ，机器人将尝试renavigate在 Navigation 子树。如果重新隔离不成功， RoundRobin 的下一个孩子将被勾选。 [待校准@156]

例如，假设机器人被卡住， Navigation 子树返回 FAILURE 😦 为了这个例子，让我们假设目标从未更新)。 [待校准@157]

尝试使用 Recovery 子树中的代价地图清除序列，并返回 SUCCESS 。机器人现在再次移动到 Navigation 子树 [待校准@158]
让我们假设清除两个成本图是不够的，并且 Navigation 子树再次返回 FAILURE 。机器人现在滴答 Recovery 子树 [待校准@159]
在 Recovery 子树中， Spin 的作用将被勾选。如果这返回 SUCCESS ，那么机器人将返回到主 Navigation 子树 * 但是 * 让我们假设 Spin 的恢复返回 FAILURE 。在这种情况下，树将 * 保留 * 在 Recovery 子树中 [待校准@160]
假设下一个恢复行动， Wait 返回 SUCCESS 。然后机器人将移动到 Navigation 子树 [待校准@161]
假设 Navigation 子树返回 FAILURE (清除成本图，尝试旋转，等待仍然不足以恢复系统。机器人将移动到 Recovery 子树上并尝试 BackUp 行动。假设机器人尝试了 BackUp 动作，并且能够成功完成动作。 BackUp 动作节点返回 SUCCESS ，所以现在我们再次进入导航子树。 [待校准@162]
在这个假设的场景中，让我们假设 BackUp 的动作允许机器人在 Navigation 子树中成功导航，并且机器人到达目标。在这种情况下，整个英国电信仍将返回 SUCCESS 。 [待校准@163]

如果 BackUp 的作用不足以让机器人不卡住，上述逻辑将无限期地继续下去，直到 Navigate 子树和 Recovery 子树的母树中的 number_of_retries 被超越，或者如果 Recovery 子树中的所有系统范围的恢复都返回 FAILURE (这不太可能，并且可能指向其他一些系统故障)。 [待校准@164]

本文主要校准贡献：向佐转
本文遵循知识共享协议，禁止未授权商用转载

鱼香ROS

关注

0
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
【Nav2中文网】九、Nav2行为树（二）详细的行为树演练

本教程来自：Nav2中文网Nav2交流社区：https://fishros.org.cn/forumROS2/Nav2千人交流群：(QQ)139707339更多精彩教程请关注微信公众号：鱼香ROS欢迎添加机器人小伊微信，解锁机器人学习特殊服务详细的行为树演练概述必要条件Navigate To Pose With Replanning and RecoveryNavigation SubtreeRecovery Subtree概述本文作为Nav2中主要行为树 (BT) 的参.
复制链接

扫一扫