跟随动态点

此行为树从起点实现导航行为，尝试随时间跟随动态点。 此“动态点”可以是人、另一个机器人、虚拟胡萝卜，任何东西。 唯一的要求是您想要跟随的姿势已发布到``GoalUpdater``BT 节点中概述的主题。

在此树中，我们以 1 hz 重新规划，就像我们在 导航至目的地 中使用``ComputePathToPose``节点所做的那样。 但是，这次当我们重新规划时，我们会根据更新的目标主题中的最新信息更新``目标``。 在我们规划到这个动态点的路径后，我们使用``TruncatePath``节点从靠近动态点的路径末端删除路径点。 此行为树节点非常有用，这样即使机器人停止，它也能始终与障碍物保持至少“距离”的距离。 它还可以平滑任何与尝试规划路径到成本地图中可能被占用的空间有关的偏离路径行为。

计算并截断到动态点的新路径后，它将再次通过“FollowPath”节点传递给控制器​​。 但是，请注意，它位于“KeepRunningUntilFailure”装饰器节点下，以确保控制器继续执行，直到出现故障模式。 此行为树将无限期地执行，直到导航请求被抢占或取消。

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <PipelineSequence name="NavigateWithReplanning">
      <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
      <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
      <RateController hz="1.0">
        <Sequence>
          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
            <ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
          </GoalUpdater>
        <TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
        </Sequence>
      </RateController>
      <KeepRunningUntilFailure>
        <FollowPath path="{truncated_path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
      </KeepRunningUntilFailure>
    </PipelineSequence>
  </BehaviorTree>
</root>