Navigation2中follow_path动作失效问题分析与解决方案

问题背景

在使用ROS2 Navigation2导航系统时，开发者可能会遇到follow_path动作无法正常工作的问题。具体表现为当通过ros2 action send_goal命令发送路径规划请求时，系统返回"Resulting plan has 0 poses in it"的警告信息，最终导致动作被中止(ABORTED)。

问题现象

当执行以下命令发送路径规划请求时：

ros2 action send_goal /follow_path nav2_msgs/action/FollowPath "
controller_id: 'FollowPath'
goal_checker_id: 'general_goal_checker'
path:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: 'map'
  poses:
    - header:
        frame_id: 'map'
      pose:
        position:
          x: -2.0
          y: 1.5
          z: 0.0
        orientation:
          x: 0.0
          y: 0.0
          z: 0.0
          w: 1.0
    - header:
        frame_id: 'map'
      pose:
        position:
          x: 1.0
          y: -1.5
          z: 0.0
        orientation:
          x: 0.0
          y: 0.0
          z: 0.0
          w: 1.0
    - header:
        frame_id: 'map'
      pose:
        position:
          x: -1.0
          y: 2.0
          z: 0.0
        orientation:
          x: 0.0
          y: 0.0
          z: 0.0
          w: 1.0 "

系统会返回如下错误信息：

Goal accepted with ID: eb1efacf83804822bc97d5bff0818820
Result: {}
Goal finished with status: ABORTED

同时控制器服务器日志显示：

[WARN] [controller_server]: Resulting plan has 0 poses in it.
[WARN] [controller_server]: Received plan with zero length
[ERROR] [controller_server]: Controller patience exceeded

问题原因分析

经过深入调查，发现这个问题主要由以下两个因素导致：

1. 路径点超出地图范围：在默认的Turtlebot3仿真环境中，地图范围有限。当路径点坐标超出当前加载的地图边界时，Navigation2系统会自动过滤掉这些无效点，导致最终生成的路径为空。

2. 命令格式问题：虽然最初怀疑是命令格式问题，但经过验证，正确的YAML格式命令能够被系统正确解析。问题主要出在坐标值上而非命令格式本身。

解决方案

要解决这个问题，可以采取以下措施：

1. 使用合理范围内的坐标值：在默认仿真环境中，建议使用以下范围内的坐标值：

   ros2 action send_goal /follow_path nav2_msgs/action/FollowPath "
   controller_id: 'FollowPath'
   goal_checker_id: 'general_goal_checker'
   path:
     header:
       stamp:
         sec: 0
         nanosec: 0
       frame_id: 'map'
     poses:
       - header:
           frame_id: 'map'
         pose:
           position:
             x: -1.73
             y: -0.523
             z: 0.0
           orientation:
             x: 0.0
             y: 0.0
             z: 0.0
             w: 1.0
       - header:
           frame_id: 'map'
         pose:
           position:
             x: -1.18
             y: -0.59
             z: 0.0
           orientation:
             x: 0.0
             y: 0.0
             z: 0.0
             w: 1.0
       - header:
           frame_id: 'map'
         pose:
           position:
             x: -0.60
             y: -0.519
             z: 0.0
           orientation:
             x: 0.0
             y: 0.0
             z: 0.0
             w: 1.0 "
   

2. 验证地图范围：在使用自定义地图时，应先确认地图的实际范围，确保路径点坐标在地图边界内。

3. 使用自动补全功能：为避免命令格式问题，可以使用ros2 action send_goal命令的自动补全功能（按Tab键两次）来生成正确的消息模板。

技术原理

Navigation2的路径跟随功能依赖于几个关键组件：

1. 控制器服务器：负责接收路径规划请求并生成控制指令。
2. 全局规划器：根据地图和路径点生成全局路径。
3. 局部规划器：执行路径跟踪和避障。

当路径点超出地图范围时，全局规划器无法生成有效路径，导致控制器服务器接收到的路径为空，最终触发"Controller patience exceeded"错误。

最佳实践

1. 在使用follow_path动作前，先通过RViz或其他可视化工具确认地图范围和机器人当前位置。
2. 对于复杂路径规划，建议先使用/compute_path_to_pose服务验证路径可行性。
3. 在开发过程中，逐步增加路径点数量，从简单路径开始测试。
4. 注意坐标系一致性，确保所有路径点使用相同的坐标系（通常是'map'坐标系）。

通过遵循这些实践，可以避免大多数与路径规划相关的问题，确保Navigation2系统能够正常工作。