动态物体跟踪
概述
本教程展示了如何使用 Nav2 执行从 A 点到 B 点以外的其他任务。在本例中，我们将使用 Nav2 无限期地跟踪远处的移动物体。
此任务在跟踪人或另一个机器人等情况下很有用。以下是可以使用此功能创建的一些应用程序示例视频。“Carry My Luggage”RoboCup @ Home 测试，其中 CATIE Robotics 团队成功执行了测试，以及这个真实（未来）世界的应用程序：
此任务的要求如下：
更改仅限于用于导航的行为树。此行为树可在需要时在
NavigateToPose操作中选择，也可以是默认行为树。它由运行时可配置的插件组成。规划器和控制器的配置不会被修改。
该操作将无限期运行，直到发起者取消它为止。
检测要跟随的动态物体（如人）不在本教程的讨论范围内。如下图所示，您的应用程序应为感兴趣的物体提供检测器，将初始姿势发送到 NavigateToPose 操作，并在任务期间根据主题更新它。存在许多不同类型的检测器，您可以利用它们来实现此应用程序：
教程步骤
0- 创建行为树
我们从这个简单的行为树开始。这个行为树以 1 hz 的频率重新规划一条新路径，并将该路径传递给控制器进行跟踪：
<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>
现在，您可以保存此行为树并将其用于我们的导航任务。
作为参考，此确切的行为树 `made available<https://github.com/ros-planning/navigation2/blob/main/nav2_bt_navigator/behavior_trees/follow_point.xml>”给您，包含在“nav2_bt_navigator`包中。
1- 设置Rviz点击点
我们将使用 RViz 而不是完整的应用程序，这样您就可以在家进行测试，而无需找到检测器即可开始使用。我们将使用工具栏上的 点击点 按钮来替代对象检测，以向 Nav2 提供目标更新。此按钮允许您在主题 /clicked_point 中发布坐标。需要使用程序 clicked_point_to_pose 从此 `repo <https://github.com/fmrico/nav2_test_utils>”将此点发送到行为树。在您的工作区中克隆此 repo，构建并在终端中输入`_.
ros2 run nav2_test_utils clicked_point_to_pose
或者，您可以将 rviz 配置文件中的该主题重新映射到 goal_updates 。