使用一致重新规划和如果路径无效则导航到姿态
这个行为树实现了一个更成熟版本的行为树,可以在:ref:`behavior_trees`中进行导航。它在自由空间中从起点导航到单个目标点。它既包含了在特定子上下文中使用自定义恢复操作,也包含了一个用于系统级故障的全局恢复子树。它还提供了用户在返回失败状态之前多次重试任务的机会。
ComputePathToPose
和 FollowPath
的BT节点也指定了它们要使用的算法。按照惯例,我们按算法的风格命名它们(例如,不是 DWB
而是 FollowPath
),这样行为树或应用程序开发人员就不需要关注技术细节。他们只想使用一个路径跟踪控制器。
在这个行为树中,我们尝试在返回任务失败之前重试整个导航任务 6 次。这使得导航系统有充分的机会尝试从故障条件中恢复或等待瞬时问题解决,例如人员拥挤或临时传感器故障。
在正常执行中,重新规划可以由无效的先前路径、新目标或者如果10秒钟内未创建新路径来触发。如果规划器或者控制器失败,它将在其子树中触发具有上下文意识的恢复行为。目前,如果规划器失败,恢复行为将清除全局代价地图,如果控制器失败,恢复行为将清除局部代价地图。可以在这些子树中添加其他上下文特定的恢复行为。
如果这些上下文恢复失败,则行为树进入恢复子树。这个子树专门用于解决系统级故障,例如机器人被卡住或处于糟糕位置的问题。该子树有一个``GoalUpdated``行为树节点,它在每次迭代中进行刻度,以确保对新目标的响应性。接下来,恢复子树将尝试以下恢复操作:清除代价地图、旋转、等待和后退。在子树中的每个恢复操作之后,将重新尝试主导航子树。如果继续失败,则刻度恢复子树中的下一个恢复操作。
虽然此行为树没有使用,但是``PlannerSelector``、ControllerSelector``和``GoalCheckerSelector``行为树节点也可能很有帮助。与硬编码要使用的算法(``GridBased``和``FollowPath
)不同,这些行为树节点将允许用户通过ROS主题动态更改导航系统中使用的算法。相反,可能更明智的做法是使用条件节点在最有用和独特的情况下创建具有指定算法的不同子树上下文。然而,选择节点可以是一种有用的方式,通过外部应用程序而不是通过内部行为树控制流逻辑来更改算法。最好通过行为树方法来实现更改,但我们理解许多专业用户有外部应用程序以动态更改其导航器的设置。
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="2.0">
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<Fallback>
<ReactiveSequence>
<Inverter>
<PathExpiringTimer seconds="10" path="{path}"/>
</Inverter>
<Inverter>
<GlobalUpdatedGoal/>
</Inverter>
<IsPathValid path="{path}"/>
</ReactiveSequence>
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
</Fallback>
<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>