Skip to content
Anima Mundi Anima Mundi

带恢复的路线图导航行为树

该行为树采用了一种与本节其他行为树不同的导航方式。它不使用自由空间规划器 ComputePathToPose 直接规划到目标的完整路径，而是通过 Route Server 在预定义的导航图上查找通往目标的路线。这种方式在大型环境中尤为实用，例如长距离自由空间实时规划的计算量过大、无法在整幅地图范围内规划，或者需要确定性导航行为以及受限导航区域、车道或路线。

该行为树使用 ComputeRoute 节点计算穿越环境的路线。该节点在初始化时执行一次，并在以下任一条件满足时重新执行：目标因抢占而更新（GlobalUpdatedGoal），或当前路线因碰撞而失效（ValidatePath）。随后，如果机器人起始位姿距离路线第一个节点太远，会使用自由空间规划器将当前位姿连接到第一个节点。这一步称为「首英里」（first mile），由 ComputePathToPose 节点完成。如果只需在导航图上导航，且机器人始终位于图上或附近，可以移除该步骤。

与之对应的是「末英里」（last mile）操作：机器人使用自由空间规划器将路线的最后一个节点连接到目标位姿。这同样由 ComputePathToPose 节点完成，可按需移除。计算出的路径（包括首英里和末英里路径）随后在平滑器（smoother）中进行平滑处理，使转角更自然流畅。最后使用 FollowPath 跟随该路径。

有关选择器节点、恢复行为以及回退（fallback）机制的详细说明，请参阅本节中的其他行为树文档。

  <root BTCPP_format="4" main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
      <RecoveryNode number_of_retries="6" name="NavigateRecovery">
        <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"/>
          <RecoveryNode number_of_retries="1" name="ComputeRoute">
            <RateController hz="0.5" name="ComputeRouteRateController">
                <Fallback>
                  <!-- Compute a new route if a new goal is found or the path is no longer valid -->
                  <ReactiveSequence>
                    <Inverter>
                      <GlobalUpdatedGoal/>
                    </Inverter>
                    <ValidatePath path="{path}"/> <!-- Base it on the complete connected 'path', not simply the 'route_path' -->
                  </ReactiveSequence>
                  <Sequence name="ComputeAndSmoothRoute">
                    <!-- Compute the route -->
                    <ComputeRoute goal="{goal}" path="{route_path}" route="{route}" use_poses="true" error_code_id="{compute_route_error_code}" error_msg="{compute_route_error_msg}"/>


                    <!-- Find if the route start node is far from the robot's current pose; if so, connect them for 'first mile'. -->
                    <ReactiveSequence>
                      <GetCurrentPose current_pose="{current_pose}"/>
                      <GetPoseFromPath path="{route_path}" index="0" pose="{route_start_pose}"/>
                      <Inverter>
                        <ArePosesNear ref_pose="{current_pose}" target_pose="{route_start_pose}" tolerance="0.3"/>
                      </Inverter>
                      <ComputePathToPose goal="{route_start_pose}" path="{first_mile_path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
                      <ConcatenatePaths input_path1="{first_mile_path}" input_path2="{route_path}" output_path="{route_path}"/>
                    </ReactiveSequence>


                    <!-- Find if the route end node is far from the goal pose; if so, connect them for 'last mile'. -->
                    <ReactiveSequence>
                      <GetPoseFromPath path="{route_path}" index="-1" pose="{route_end_pose}"/>
                      <Inverter>
                        <ArePosesNear ref_pose="{goal}" target_pose="{route_end_pose}" tolerance="0.1"/>
                      </Inverter>
                      <ComputePathToPose start="{route_end_pose}" goal="{goal}" path="{last_mile_path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
                      <ConcatenatePaths input_path1="{route_path}" input_path2="{last_mile_path}" output_path="{route_path}"/>
                    </ReactiveSequence>


                    <!-- Smooth the completed route -->
                    <SmoothPath unsmoothed_path="{route_path}" smoothed_path="{path}" smoother_id="route_smoother" error_code_id="{smoother_error_code}" error_msg="{smoother_error_msg}"/>
                  </Sequence>
                </Fallback>
              </RateController>
            <Sequence>
              <Fallback>
                <WouldARouteRecoveryHelp error_code="{compute_route_error_code}"/>
                <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
              </Fallback>
              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
            </Sequence>
          </RecoveryNode>
          <RecoveryNode number_of_retries="1" name="FollowPath">
            <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
            <Sequence>
              <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
              <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
            </Sequence>
          </RecoveryNode>
        </PipelineSequence>
        <Sequence>
          <Fallback>
            <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
            <WouldAPlannerRecoveryHelp error_code="{compute_route_error_code}"/>
          </Fallback>
          <ReactiveFallback name="RecoveryFallback">
            <GoalUpdated/>
            <RoundRobin name="RecoveryActions">
              <Sequence name="ClearingActions">
                <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
                <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
              </Sequence>
              <Wait wait_duration="5.0" error_code_id="{wait_error_code}" error_msg="{wait_error_msg}"/>
              <BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
            </RoundRobin>
          </ReactiveFallback>
        </Sequence>
      </RecoveryNode>
    </BehaviorTree>
  </root>