Drobotics SUPER Planner Rosbag 验证指南
自主飞行数据分析 · 规划器验证 · 故障诊断
Rosbag
SUPER Planner
FAST-LIO
PX4
通过 飞行验证
目的
自主飞行后，rosbag 应用于回答三个规划器问题：
1. 规划器是否收到目标点？
2. 规划器是否生成了轨迹命令？
3. 无人机是否实际朝这些目标移动？
2. 规划器是否生成了轨迹命令？
3. 无人机是否实际朝这些目标移动？
警告 仅靠话题频率是不够的。rosbag 可能有 /Odometry 和 /cloud_registered，但规划器可能仍未生成有效轨迹。
所需规划器话题
一个有用的 Drobotics SUPER 规划器 rosbag 应包含：
| 话题 | 含义 |
|---|---|
/goal_pose | 发送给 SUPER 的目标点 |
/planning/pos_cmd | SUPER 规划器轨迹命令输出 |
/mavros/setpoint_position/local | 发送给 PX4 的位置设定点 |
/Odometry | FAST-LIO SLAM 里程计 |
/mavros/local_position/odom | PX4 局部位置估计 |
/mavros/state | PX4 模式、解锁状态、OFFBOARD 区间 |
/cloud_registered | 用于建图的注册 LiDAR 点云 |
/fsm/path | 规划器路径可视化/调试 |
快速规划器 Rosbag 检查
source /opt/ros/humble/setup.bash
source ~/slam_ws/install/setup.bash
source ~/super_ws/install/setup.bash
BAG=$(cat ~/rosbags/.latest_indoor_hover)
echo "=== BAG ==="
echo "$BAG"
du -sh "$BAG"
ros2 bag info "$BAG"
echo
echo "=== PLANNER TOPICS ==="
ros2 bag info "$BAG" | grep -E \
'/goal_pose|/planning/pos_cmd|/mavros/setpoint_position/local|/Odometry|/mavros/local_position/odom|/mavros/state|/cloud_registered|/fsm/path'
规划器活动的预期迹象：
通过 /goal_pose count > 0
通过 /planning/pos_cmd count > 0
通过 /mavros/setpoint_position/local count > 0
通过 /goal_pose count > 0
通过 /planning/pos_cmd count > 0
通过 /mavros/setpoint_position/local count > 0
警告 如果 /goal_pose 存在但 /planning/pos_cmd 缺失或为零 → SUPER 可能未生成轨迹
警告 如果 /planning/pos_cmd 存在但 /mavros/setpoint_position/local 缺失 → 桥接未向 PX4 发送命令
警告 如果设定点存在但无人机未移动 → 检查 PX4 模式和 OFFBOARD 区间
警告 如果 /planning/pos_cmd 存在但 /mavros/setpoint_position/local 缺失 → 桥接未向 PX4 发送命令
警告 如果设定点存在但无人机未移动 → 检查 PX4 模式和 OFFBOARD 区间
规划器日志检查
echo "=== SUPER PLANNER EVENTS ==="
tail -300 /tmp/drobotics_super_planner_auto_nav.log \
| grep -E 'Get goal|SUCCESS|FAILED|min dis|GeneratePolytope|PlanFromRest|FOLLOW_TRAJ|Traj finish|OPT_FAILED'
良好信号：
通过 Get goal
通过 GenerateExpTrajectory SUCCESS
通过 FOLLOW_TRAJ
通过 Traj finish
通过 Get goal
通过 GenerateExpTrajectory SUCCESS
通过 FOLLOW_TRAJ
通过 Traj finish
不良信号：
错误 GeneratePolytopeFromLine failed
错误 PlanFromRest failed
错误 min dis...
错误 OPT_FAILED
错误 GeneratePolytopeFromLine failed
错误 PlanFromRest failed
错误 min dis...
错误 OPT_FAILED
解读：
Get goal → SUPER 收到目标点
GenerateExpTrajectory SUCCESS → SUPER 找到有效轨迹
FOLLOW_TRAJ → SUPER 正在主动输出轨迹命令
Traj finish → SUPER 认为轨迹已完成
PlanFromRest failed → SUPER 无法从当前状态生成安全轨迹
GeneratePolytopeFromLine failed → SUPER 无法创建安全走廊
min dis... → 障碍物净空太小
OPT_FAILED → 优化/重新规划失败
Get goal → SUPER 收到目标点
GenerateExpTrajectory SUCCESS → SUPER 找到有效轨迹
FOLLOW_TRAJ → SUPER 正在主动输出轨迹命令
Traj finish → SUPER 认为轨迹已完成
PlanFromRest failed → SUPER 无法从当前状态生成安全轨迹
GeneratePolytopeFromLine failed → SUPER 无法创建安全走廊
min dis... → 障碍物净空太小
OPT_FAILED → 优化/重新规划失败
自动导航日志检查
echo "=== AUTO NAV LOG ==="
tail -200 /tmp/super_auto_nav_slide.log
查找：
Trying forward
Trying slide_left
Trying slide_right
Trying backward
accepted
did not move enough
Trying forward
Trying slide_left
Trying slide_right
Trying backward
accepted
did not move enough
警告 重要注意： 在一次 Drobotics 测试中，rosbag 证明无人机移动了约 0.76 米，但自动导航日志报告 best_progress=0.00 米。这意味着物理管道正常工作，但自动导航进度检查器/日志记录需要修正。
因此，rosbag 是最终真相。
因此，rosbag 是最终真相。
从 Rosbag 验证运动
最重要的规划器验证是：无人机是否朝每个 /goal_pose 移动？
良好飞行应显示：
goal published ↓ setpoints published ↓ /Odometry moves toward goal ↓ planner finishes trajectory
goal published ↓ setpoints published ↓ /Odometry moves toward goal ↓ planner finishes trajectory
Drobotics 测试预期示例：
目标：前进 0.8 米
实际 /Odometry 移动：0.765 米
结果：通过 成功
目标：前进 0.8 米
实际 /Odometry 移动：0.765 米
结果：通过 成功
另一个示例：
目标：侧向/滑行移动
实际 /Odometry 移动：0.863 米
结果：通过 成功
目标：侧向/滑行移动
实际 /Odometry 移动：0.863 米
结果：通过 成功
规划器故障诊断
案例 1 — 无 /goal_pose
问题：自动导航或手动目标发布器未运行。
ros2 topic info /goal_pose --verbose
案例 2 — /goal_pose 存在，但无 /planning/pos_cmd
问题：SUPER 收到目标但未生成轨迹。
可能原因：
障碍物太近 · 地图太密 · 目标超出安全走廊 · 里程计帧错误 · 规划器未初始化
障碍物太近 · 地图太密 · 目标超出安全走廊 · 里程计帧错误 · 规划器未初始化
tail -300 /tmp/drobotics_super_planner_auto_nav.log \
| grep -E 'Get goal|FAILED|min dis|GeneratePolytope|PlanFromRest'
案例 3 — /planning/pos_cmd 存在，但无设定点
问题：Drobotics SUPER-to-MAVROS 桥接未运行或未订阅。
ros2 topic info /planning/pos_cmd --verbose
ros2 topic info /mavros/setpoint_position/local --verbose
案例 4 — 设定点存在，但无人机未移动
可能原因：
PX4 不在 OFFBOARD · PX4 未解锁 · 设定点速度太低 · 任务边界拒绝命令 · PX4 估计器未正确接受 EV
PX4 不在 OFFBOARD · PX4 未解锁 · 设定点速度太低 · 任务边界拒绝命令 · PX4 估计器未正确接受 EV
ros2 topic echo /mavros/state --once
查找：
armed: true 和 mode: OFFBOARD 案例 5 — 无人机移动，但未达到完整距离
可能原因：
步长/进度阈值/超时设置过于保守
步长/进度阈值/超时设置过于保守
-p forward_step_m
-p movement_wait_s
-p min_progress_m
-p max_setpoint_speed_mps
推荐规划器测试参数
首次安全自主测试
-p forward_step_m:=0.50
-p side_step_m:=0.30
-p backward_step_m:=0.20
-p movement_wait_s:=12.0
-p min_progress_m:=0.35
-p max_steps:=3
较大运动测试
-p forward_step_m:=0.80
-p side_step_m:=0.45
-p backward_step_m:=0.25
-p movement_wait_s:=16.0
-p min_progress_m:=0.35
-p max_steps:=3
仅证明前进 0.8 米
-p forward_step_m:=0.80
-p movement_wait_s:=16.0
-p min_progress_m:=0.65
-p max_steps:=1
警告 对于混合前进/侧向/后退行为，不要使用
min_progress_m:=0.65，因为侧向和后退步长小于 0.65 米。通过 什么算作成功的规划器飞行？
成功的 Drobotics SUPER 规划器飞行应具有：
通过 /goal_pose > 0
通过 /planning/pos_cmd > 0
通过 /mavros/setpoint_position/local > 0
通过 运动期间 PX4 模式为 OFFBOARD
通过 /Odometry 朝向目标移动
通过 规划器日志显示 SUCCESS / FOLLOW_TRAJ / Traj finish
通过 /goal_pose > 0
通过 /planning/pos_cmd > 0
通过 /mavros/setpoint_position/local > 0
通过 运动期间 PX4 模式为 OFFBOARD
通过 /Odometry 朝向目标移动
通过 规划器日志显示 SUCCESS / FOLLOW_TRAJ / Traj finish
成功结果示例：
前进目标命令：0.8 米
测量的 /Odometry 移动：0.765 米
规划器状态：FOLLOW_TRAJ
轨迹：已完成
结果：通过 成功
前进目标命令：0.8 米
测量的 /Odometry 移动：0.765 米
规划器状态：FOLLOW_TRAJ
轨迹：已完成
结果：通过 成功
总结
话题频率检查 证明传感器和桥接是活的。
规划器验证 证明自主性实际发生。
规划器验证 证明自主性实际发生。
对于 Drobotics SUPER 飞行，始终检查：
1. 我们发布了目标点吗？
2. SUPER 生成了 /planning/pos_cmd 吗？
3. 桥接发布了 MAVROS 设定点吗？
4. PX4 在 OFFBOARD 下解锁了吗？
5. /Odometry 朝目标移动了吗？
6. 规划器日志显示 SUCCESS 和 Traj finish 吗？
1. 我们发布了目标点吗？
2. SUPER 生成了 /planning/pos_cmd 吗？
3. 桥接发布了 MAVROS 设定点吗？
4. PX4 在 OFFBOARD 下解锁了吗？
5. /Odometry 朝目标移动了吗？
6. 规划器日志显示 SUCCESS 和 Traj finish 吗？
通过 这就是证明 Drobotics 自主 SLAM 规划器工作了的正确方法。
平台：Jetson · ROS2 Humble · FAST-LIO · SUPER · PX4
(c) 卓博泰科技 · Drobotics SUPER Planner Rosbag 验证指南
(c) 卓博泰科技 · Drobotics SUPER Planner Rosbag 验证指南