机器人 / 系统课时

复现动态障碍并验证全程导航

复现动态障碍并验证全程导航

用同一张 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 沙盒,保持终端不关闭。

bash
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 环境加载。

bash
source /opt/ros/jazzy/setup.bash
ros2 action list -t | grep /navigate_through_poses
gz service -l | grep /world/default/set_pose

预期输出应包含:

text
/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 则同时创建两个实体。

bash
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 替换为对应用例名。

bash
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 中确认实际存在;自定义命名空间要替换实际话题名。

在另一个终端发送三点任务并保存反馈:

bash
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,逐段穿过导航区域;实际仿真时间间隔会受计算负载影响。

bash
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、拒绝或其他真实终态及相应日志,不能用一次短暂停车代替。

结束录包后运行:

bash
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、标记参数;若障碍离开后代价栅格仍不消退,检查清除射线与有效距离。定位前不要反复清空代价地图掩盖根因。

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

目录与阅读设置

阅读设置

17

记录一个想法

复现动态障碍并验证全程导航

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

登录哪都通