跟踪点行为树

该行为树实现了一种导航行为：从起始点出发，随时间持续跟随一个动态点。这个动态点可以是人、另一个机器人、虚拟的「胡萝卜」（virtual carrot）等任何物体。唯一的要求是，待跟随的位姿需发布到 GoalUpdater 行为树节点指定的话题上。

在这棵树中，与 导航到目标位姿行为树 类似，使用 ComputePathToPose 节点以 1 Hz 的频率重新规划。不过，每次重规划时都会根据目标更新话题上的最新信息来更新 goal。规划出到达动态点的路径后，使用 TruncatePath 节点移除路径末端靠近动态点的路径点。该节点非常实用——它能确保机器人始终与障碍物保持至少 distance 的距离，即使停下来也是如此。同时，它还能平滑路径上的偏差——这些偏差通常出现在向代价地图中可能被占用的区域规划路径时。

计算并截断到达动态点的新路径后，路径再次通过 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}" error_msg="{compute_path_error_msg}"/>
            </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}" error_msg="{follow_path_error_msg}"/>
        </KeepRunningUntilFailure>
      </PipelineSequence>
    </BehaviorTree>
  </root>