ROS2与Navigation2入门教程-动态物体跟随

说明：

介绍如何将Nav2用于除了让机器人从A点移动到B点之外的不同任务中，将Nav2用于无限期地跟随一个移动物体

概述：

这项任务在跟踪诸如人或其他机器人等的情况下很有用。
下面是使用此功能可以创建的一些应用程序的示例视频。
在“携带我的行李（Carry My Luggage）”机器人杯赛RoboCup @ Home测试中，CATIE机器人团队成功执行了该测试，而这个视频展示了这个机器人动态物体跟随真实（未来）世界的应用情况

这项任务的要求如下：

更改仅限于被用来导航的行为树。需要时可以在NavigateToPose动作中选择行为树，也可以是默认的行为树。它由运行时可配置插件组成。
规划器和控制器的配置不会被修改。
该动作会无限期地运行下去，直到被发起者取消为止。
被跟随的动态物体（例如某个人）的检测方法超出了本教程的范围。
如下图所示，应用程序应该提供一个可以检测出待跟随物体的检测器程序，将待跟随物体的初始位置发送给NavigateToPose动作，并在任务持续过程中将该物体的位置更新到某个话题上。
有许多不同类型的物体检测器可以在此应用程序中使用。

请输入图片描述

具体步骤

创建行为树BT
下面将会从这个简单的行为树开始。
此行为树以1hz的频率重新规划一条新路径，并将该路径传递给控制器以进行跟随：

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

然后会使用装饰器（decorator）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软件包中以便让用户可以直接使用。

设置Rviz单击点

这里会使用RViz而不是完整的应用程序，以便可以在家中进行测试，且不需要检测器即可开始使用。
这里会使用工具栏上的“单击点（clicked point）”按钮代替物体检测过程，以向Nav2提供目标位置的更新。
这个按钮允许在话题/clicked_point上发布坐标。
需要用来自于此存储库的clicked_point_to_pose节点程序将这个点发送给行为树。
将该存储库克隆到您的工作空间中（如果是从源代码编译构建安装Nav2的，则默认已经安装了nav2_test_utils软件包及其可执行程序clicked_point_to_pose，因此在这种情况下不需要重新克隆该存储库和重新编译安装），进行软件包构建并在终端中输入以下命令：

ros2 run nav2_test_utils clicked_point_to_pose

另外一种可选的方法就是在rviz配置文件中将这个话题重映射到goal_updates上。
在Nav2仿真中运行“动态物体跟随”节点
在终端中执行以下命令启动Nav2：

ros2 launch nav2_bringup tb3_simulation_launch.py default_bt_xml_filename:=/path/to/bt.xml

打开RViz并在初始化机器人位置后，命令机器人导航到任意位置。
使用按钮“单击点（clicked point）”来模拟对被跟随物体的检测过程，如在本教程开头的视频中所示。
当有一个探测器程序以更高的频率（1 hz、10 hz、100 hz）检测障碍物时，将会看到一个反应更加灵敏的机器人跟随检测到的感兴趣物体！
要查看用“单击点（clicked point）”按钮模拟动态物体跟随的Nav2演示效果，请观看下面这个视频

参考：

https://navigation.ros.org/tutorials/docs/navigation2_dynamic_point_following.html

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号