机器人 / 系统课时

通过路径可视化定位规划失败

通过路径可视化定位规划失败

规划失败时,单看日志很难区分目标坐标错误、目标落在障碍内和真正的不可达。这里以 ROS 2 Jazzy 的 Nav2 为例,把 RViz 中的全局路径与代价地图叠在一起,再读取规划 Action 的返回结果,定位失败发生在哪个环节。

学习目标

  • 在 RViz 中同时查看静态地图、全局代价地图和 /plan 路径。
  • 使用 ComputePathToPose Action 验证规划结果和错误码。
  • 区分 TF 故障、目标占据、路径不连通和仅显示异常。

建立可对照的 RViz 视图

前提是地图和定位已启动,机器人在 RViz 中的位置与仿真一致,Nav2 的 planner_server 正在运行。以下采用常见的无命名空间配置;若节点或话题被重映射,应使用实际名称。

  1. 在 RViz 左侧 Displays → Global Options → Fixed Frame 设为 map。不要为了让报错消失而改成 base_link,否则无法以全局地图为基准检查目标。
  2. 用 Add → By display type → Map 加入静态地图,Topic 设为 /map,Alpha 设为 1.0。
  3. 再加入一个 Map,Topic 设为 /global_costmap/costmap,Alpha 可先设为 0.5;它表示规划器实际参考的障碍及代价,而不只是原始墙体。
  4. 用 Add → By display type → Path 加入全局路径,Topic 设为 /plan,Line Style 选 Lines,Line Width 设为约 0.03,Buffer Length 设为 1。
  5. 保持 RViz 的 Path 订阅开启,再发规划请求。/plan 不是永久保存上一条路径的接口,订阅建立前发出的那次路径可能看不到。

正确叠图时,机器人、目标和路径应落在地图的同一组空间位置。若障碍看起来被整体平移,先检查 map、odom、base_link 的 TF 关系,不要直接修改地图原点来掩盖定位错误。

直接请求规划结果

在已经加载 ROS 2 Jazzy 环境的终端,先确认规划节点处于活动状态、插件名称与 Action 类型。下面是使用默认 GridBased 插件的示例。

bash
# 确认规划节点已激活,并取得实际插件 ID
ros2 lifecycle get /planner_server
ros2 param get /planner_server planner_plugins
ros2 action type /compute_path_to_pose

预期输出(默认 Nav2 配置):

text
active [3]
String values are: ['GridBased']
nav2_msgs/action/ComputePathToPose

接着在 RViz 地图上找一处明确的可通行目标点,将下面的 (1.0, 0.0) 替换为该点在 map 坐标系中的实际米数坐标;若插件名称不同,也要替换 GridBased。

bash
# 只计算从当前位置到地图目标点的路径,不发送底盘运动指令
ros2 action send_goal /compute_path_to_pose nav2_msgs/action/ComputePathToPose \
  "{goal: {header: {frame_id: map}, pose: {position: {x: 1.0, y: 0.0, z: 0.0}, orientation: {w: 1.0}}}, planner_id: GridBased, use_start: false}"

预期输出(规划成功时的关键行节选;完整结果还包含路径点与规划耗时):

text
Goal accepted with ID: <ID>
  error_code: 0
Goal finished with status: SUCCEEDED

<ID> 为每次请求生成的动态标识。use_start: false 表示使用机器人当前位姿作为起点,因此必须先完成定位;goal.header.frame_id: map 表示目标坐标采用全局地图。成功时应在返回结果的 path.poses 中看到非空路径,RViz 的 /plan 也应出现折线。注意这是 Action,不是用 ros2 service call 调用的 Service。

按错误码定位空间问题

先看 Action 最后的状态,再看 error_code 和 RViz 叠图。下面的错误码适用于 Nav2 Jazzy 的 ComputePathToPose,不要把控制器的跟踪失败误判为全局规划失败。

错误码或现象 定位重点 具体修正
202:TF_ERROR 无法从机器人底盘取得全局起点,或无法转换目标坐标 检查 map → odom → base_link 是否连通,并确认各节点统一使用仿真时间
203/204:起点或目标超出地图 map 的原点、宽高或目标坐标填错 在 RViz 内选取地图边界以内的点,注意位置单位是米,不是栅格像素
205/206:起点或目标被占据 起点或目标位姿与全局代价地图的致命障碍发生冲突 将位姿移到真正可通行的区域;检查障碍层和底盘占地轮廓
208:NO_VALID_PATH 起终点可能各自空闲,但通道被障碍或代价地图约束切断 沿地图检查整段通道、膨胀带和未知区域的规划策略
201:INVALID_PLANNER planner_id 与实际加载的插件 ID 不一致 以 planner_plugins 参数里的名称为准

全局代价地图中可达路径、目标占据与不可达区域的空间对比

注意:膨胀带不等于每个栅格都是致命障碍;灰色未知区域也不必然禁止穿越,其处理取决于代价地图与所用规划插件的设置。判断时以 全局代价地图 和真实 Action 结果为准。

区分规划失败和显示失败

如果 Action 已返回 SUCCEEDED、error_code: 0 且 path.poses 非空,但 RViz 没有线,问题优先归入可视化而非规划。逐项确认 Path 订阅的是 /plan、Fixed Frame 为 map、Path 状态无 TF 报错,并在订阅建立后重新发送一次目标。若使用了命名空间,/plan 的实际全名也可能改变。

若是 TF_ERROR,可运行 ros2 run tf2_ros tf2_echo map base_link,预期会持续打印平移和旋转;若无法取得变换,检查 AMCL 发布的 map → odom 与里程计发布的 odom → base_link。若坐标正常但目标被拒绝,在 RViz 中只把目标移动到同一连通空闲区域,再用同一 Action 重测,不要同时调整多个代价地图参数。

最后以三件事验收:Action 成功、path.poses 非空、路径在地图上避开致命障碍并连接起终点。如果三项都满足但机器人仍不移动或跟踪振荡,应转查 controller_server、局部代价地图与速度指令链路;这已经不是全局规划未生成路径的问题。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

通过路径可视化定位规划失败

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通