跟随动态点

该行为树实现了从起始点开始的导航行为，试图随时间追踪一个动态点。这个“动态点”可以是一个人、另一个机器人、一个虚拟胡萝卜，或者其他任何东西。唯一的要求是你希望跟随的姿态被发布到 GoalUpdater BT 节点中指定的主题中。

在这棵树中，我们像在 导航到姿势 中一样，以1 Hz的频率进行重新规划，使用``ComputePathToPose``节点。然而，这一次在重新规划时，我们根据更新后的目标主题中的最新信息更新``goal``。在规划到这个动态点的路径后，我们使用``TruncatePath``节点从靠近动态点的路径末端删除路径点。这个行为树节点非常有用，因为它可以让机器人始终与障碍物保持至少``distance``的距离，即使它停下来。它还可以平滑处理与尝试在代价地图中的可能占用空间上进行路径规划的任何偏离路径的行为。

在计算和截取到动态点的新路径后，它会再次通过 FollowPath 节点传递给控制器。然而，请注意它位于一个 KeepRunningUntilFailure 修饰节点下，确保控制器继续执行直到出现故障模式。该行为树将无限时间执行，直到导航请求被抢占或取消。

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