动态对象跟随
概述
本教程演示了如何在Nav2中执行与从点A到点B的不同任务。在本例中，我们将使用Nav2来无限跟随一个移动对象的距离。
这个任务在追踪一个人或另一个机器人的情况下非常有用。以下是一些可以使用这种能力创建的应用程序的示例视频。Carry My Luggage RoboCup @ Home 测试，其中 CATIE Robotics 团队成功进行了测试，这是一个真实（未来）世界的应用程序：
该任务的要求如下：
更改限于用于导航的行为树。此行为树可以在需要时在``NavigateToPose``动作中选择，或者它可以是默认的行为树。它由运行时可配置的插件组成。
规划器和控制器的配置不会被修改。
该动作将一直运行，直到由启动它的人取消为止。
检测要跟随的动态物体（例如人）超出了本教程的范围。如下图所示，您的应用程序应提供一个用于检测感兴趣物体的探测器，将初始姿态发送到“NavigateToPose”动作，并在任务的整个过程中通过话题进行更新。有许多不同类型的探测器可供您在此应用程序中使用：
教程步骤
0- 创建行为树
让我们从这个简单的行为树开始。该行为树以1赫兹的频率重新规划新路径，并将该路径传递给控制器进行跟随：
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
</RateController>
<FollowPath path="{path}" controller_id="FollowPath"/>
</PipelineSequence>
</BehaviorTree>
</root>
首先，让我们让这个行为一直运行，直到出现故障。为此，我们将使用``KeepRunningUntilFailure``控制节点。
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
</RateController>
<KeepRunningUntilFailure>
<FollowPath path="{path}" controller_id="FollowPath"/>
</KeepRunningUntilFailure>
</PipelineSequence>
</BehaviorTree>
</root>
然后，我们将使用修饰器“GoalUpdater”来接受我们要跟随的动态物体姿态的更新。该节点以当前目标作为输入，并订阅主题“/goal_update”。如果在该主题上接收到新目标，它将将新目标设置为“updated_goal”。
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
<ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="GridBased"/>
</GoalUpdater>
</RateController>
<KeepRunningUntilFailure>
<FollowPath path="{path}" controller_id="FollowPath"/>
</KeepRunningUntilFailure>
</PipelineSequence>
</BehaviorTree>
</root>
为了保持与目标的一定距离，我们将使用动作节点``TruncatePath``。该节点会修改路径，使其变短，以便我们不会试图进入感兴趣的对象。我们可以使用输入端口``distance``来设置到目标的期望距离。
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<Sequence>
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
<ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="GridBased"/>
</GoalUpdater>
<TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
</Sequence>
</RateController>
<KeepRunningUntilFailure>
<FollowPath path="{truncated_path}" controller_id="FollowPath"/>
</KeepRunningUntilFailure>
</PipelineSequence>
</BehaviorTree>
</root>
现在，您可以保存这个行为树并在我们的导航任务中使用它。
供您使用的确切行为树可以在``nav2_bt_navigator``包中找到，链接在`此处 <https://github.com/ros-planning/navigation2/blob/main/nav2_bt_navigator/behavior_trees/follow_point.xml>`_。
1- 设置 Rviz 点击点
我们将使用RViz而不是完整的应用程序，这样您就可以在家中进行测试，而无需找到一个探测器来开始。我们将使用工具栏上的“clicked point”按钮，用来替代对象检测，以向Nav2提供目标更新。该按钮允许您在主题``/clicked_point``中发布坐标。这个点需要通过程序``clicked_point_to_pose``发送到行为树，从`此存储库 <https://github.com/fmrico/nav2_test_utils>`_中获取。在您的工作空间中克隆此存储库，构建并在终端中输入命令。
ros2 run nav2_test_utils clicked_point_to_pose
可选地，您可以在rviz配置文件中重新映射此主题为``goal_updates``。