机器人 / 系统课时
复现动态障碍并验证全程导航
复现动态障碍并验证全程导航
用同一张 Gazebo 地图固定起点与三个目标,先运行无新增障碍的基线,再加入静态箱体和按固定时序横穿的箱体。通过 Nav2 Action 反馈、终态和 rosbag2 数据,分别确认到达、绕障、恢复及失败,留下可以复查的证据。
学习目标
- 在 ROS 2 Jazzy 与 Gazebo Harmonic 中构造可重复的静态、移动障碍测试。
- 向
NavigateThroughPoses提交三个目标并记录反馈与终态。 - 用激光、代价地图和恢复次数区分正常避障、卡滞与导航失败。
固定仿真与路线
环境为 Ubuntu 24.04、ROS 2 Jazzy,已安装 nav2_bringup、nav2_minimal_tb3_sim 和 ros_gz_sim。第一个终端启动官方 TurtleBot3 沙盒,保持终端不关闭。
source /opt/ros/jazzy/setup.bash
ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False预期结果:Gazebo 与 RViz 打开,Nav2 生命周期节点进入 Active。若 RViz 尚未定位,使用工具栏 2D Pose Estimate 在地图上点击机器人实际位置并拖出朝向。此后的每轮测试都采用相同地图、初始位姿、参数文件和目标坐标。
另开终端确认接口及 Gazebo 世界名;新终端均先执行 ROS 环境加载。
source /opt/ros/jazzy/setup.bash
ros2 action list -t | grep /navigate_through_poses
gz service -l | grep /world/default/set_pose预期输出应包含:
/navigate_through_poses [nav2_msgs/action/NavigateThroughPoses]
/world/default/set_pose本例使用 default 世界和三个 map 坐标目标:A(-2.4,1.4)、B(-0.6,0.55)、C(1.7,0.55),朝向均为 0 rad。先在 RViz 地图确认这三点处于空闲栅格;若使用自定义地图,应同步替换后续目标和障碍坐标,不能把示例坐标直接视为可通行保证。
放入静态与移动障碍
先在无新增障碍时完成基线 B0;其余用例重新启动相同仿真后注入障碍。创建一个长、宽均为 0.4 m,高 0.5 m 的带碰撞箱体。S1 只创建 nav_static,D1 则同时创建两个实体。
cat > /tmp/nav_case_box.sdf <<'EOF'
<?xml version="1.0"?>
<sdf version="1.8">
<model name="test_box">
<static>true</static>
<link name="body">
<!-- 碰撞与可视几何一致,避免看到的障碍与激光不符 -->
<collision name="collision">
<geometry><box><size>0.4 0.4 0.5</size></box></geometry>
</collision>
<visual name="visual">
<geometry><box><size>0.4 0.4 0.5</size></box></geometry>
</visual>
</link>
</model>
</sdf>
EOF
ros2 run ros_gz_sim create -world default -file /tmp/nav_case_box.sdf -name nav_static -x -1.8 -y 0.4 -z 0.25
ros2 run ros_gz_sim create -world default -file /tmp/nav_case_box.sdf -name nav_moving -x -0.6 -y -1.0 -z 0.25预期结果:Gazebo 中可见两个箱体,且 /scan 扫描与 RViz 的局部、全局代价地图在对应位置出现障碍。只做 S1 时不执行第二条 create 命令。新增障碍不属于原始静态地图,须由激光观测及代价地图的障碍层或体素层标记。
移动箱体虽然在 SDF 中设为 static,但可经 Gazebo set_pose 服务逐次更新位姿;这是离散位置驱动的障碍测试,不模拟行人加减速或接触动力学。

提交多点目标并采集证据
在独立终端先启动录包,等待出现数据写入提示,再发送 Action。B0、S1、D1 分别使用不同的录包目录和日志文件;下例展示 D1,执行其他用例时将 dynamic 替换为对应用例名。
ros2 bag record -o /tmp/nav_case_dynamic \
/clock /scan /odom /tf /tf_static /rosout \
/local_costmap/costmap /global_costmap/costmap预期结果:录制保持运行,直到手动按 Ctrl+C 才完成写盘。上述话题必须先在 ros2 topic list 中确认实际存在;自定义命名空间要替换实际话题名。
在另一个终端发送三点任务并保存反馈:
GOAL=$(cat <<'YAML'
poses:
- header: {frame_id: map}
pose: {position: {x: -2.4, y: 1.4, z: 0.0}, orientation: {w: 1.0}}
- header: {frame_id: map}
pose: {position: {x: -0.6, y: 0.55, z: 0.0}, orientation: {w: 1.0}}
- header: {frame_id: map}
pose: {position: {x: 1.7, y: 0.55, z: 0.0}, orientation: {w: 1.0}}
behavior_tree: ''
YAML
)
ros2 action send_goal /navigate_through_poses \
nav2_msgs/action/NavigateThroughPoses "$GOAL" \
--feedback 2>&1 | tee /tmp/nav_case_dynamic_action.log预期结果:目标被接受后持续打印 Feedback,可观察 number_of_poses_remaining、distance_remaining、number_of_recoveries。只有全部完成时才应出现 Goal finished with status: SUCCEEDED 和 error_code: 0;若为 ABORTED 或目标遭拒,应原样保留真实结果。number_of_poses_remaining 下降不等于精确停靠:Jazzy 示例默认行为树会在距中间目标 0.7 m 内移除该目标,且不检查其中间目标朝向。
重放横穿并观察恢复
D1 中让机器人开始向 B 行驶,在它接近 B 所在通道时,从另一终端执行以下脚本。箱体在 x=-0.6 m 处每隔约 1 秒增加 y=0.2 m,逐段穿过导航区域;实际仿真时间间隔会受计算负载影响。
for y in -1.0 -0.8 -0.6 -0.4 -0.2 0.0 0.2 0.4 0.6 0.8 1.0; do
# 将可见且有碰撞的箱体移到下一个测试位置
gz service -s /world/default/set_pose \
--reqtype gz.msgs.Pose --reptype gz.msgs.Boolean \
--timeout 2000 \
--req "name: \"nav_moving\", position: {x: -0.6, y: ${y}, z: 0.25}"
sleep 1
done预期输出:各次成功调用返回 data: true,Gazebo 中箱体逐步移动。RViz 中应能观察到障碍标记更新;机器人可能绕行、减速或等待,是否触发恢复取决于实际路径及参数,不以肉眼看到转弯就认定发生了 Recovery。欲观察卡滞,可在下一轮将箱体停留在实际必经通道约 12~15 秒,记录是否出现恢复次数增长或任务终止,勿在同一轮临时修改导航参数。
对照验收与故障定位
| 用例 | 固定条件 | 必须保存的观察 |
|---|---|---|
| B0 基线 | 不注入箱体 | 三点终态、耗时、到达位置 |
| S1 静态 | 仅 nav_static |
静态障碍标记、路径变化、终态 |
| D1 横穿 | 两箱体,移动脚本按顺序执行 | 障碍出现与消退时刻、绕行或等待、恢复计数、终态 |
| F1 阻塞 | 在已确认的通道前保持阻挡 12~15 秒 | 恢复动作或实际失败终态;若仍成功,记录为恢复成功而非失败 |
判定标准:A、B 在默认 0.7 m 通过半径内,最终 C 满足官方示例 Goal Checker 的 xy_goal_tolerance=0.25 m、yaw_goal_tolerance=0.25 rad,Action 终态为 SUCCEEDED 且全程无碰撞,才算一次完整通过。绕障需有路线或速度响应与代价地图变化的对应证据;number_of_recoveries 变大才表示行为树真正执行过恢复。失败必须有 ABORTED、拒绝或其他真实终态及相应日志,不能用一次短暂停车代替。
结束录包后运行:
ros2 bag info /tmp/nav_case_dynamic
ros2 param get /controller_server progress_checker.movement_time_allowance
grep -E 'Goal finished|error_code:|number_of_recoveries:|number_of_poses_remaining:' \
/tmp/nav_case_dynamic_action.log | tail -n 25预期结果:ros2 bag info 列出实际录制话题及消息数;使用官方 Jazzy 示例参数时,进度检查时间窗为 10.0 秒;最后一条命令展示各关键反馈及最终结果,具体数字随运行变化。每轮同时记录地图版本、Nav2 参数文件路径、初始位姿、目标坐标、障碍起止位置和日志目录。
出现异常按固定顺序查看:Gazebo 实体是否真的移动;/scan 能否扫到箱体;局部 VoxelLayer 与全局 ObstacleLayer 是否标记及清除;全局规划是否改道;控制器是否因无进度触发恢复。若激光里有障碍而代价地图没有,优先检查传感器话题、TF、标记参数;若障碍离开后代价栅格仍不消退,检查清除射线与有效距离。定位前不要反复清空代价地图掩盖根因。