激光雷达多点导航避障

01问题引入：一个目标 vs 一串目标

本课用 ROS2 的 Action 通信（goal → feedback → result，就像"下单 → 物流跟踪 → 签收确认"）驱动两个导航器，完成单点、多点连续导航，并研究卡住时的进度检查（progress_checker）与行为树恢复动作（清代价地图 / Spin / Wait）。

02原理通俗讲解：派快递、必经点与"外卖员迷路了怎么办"

03关联知识点

04实验目标

• 理解 ROS2 Action 通信机制与两个导航器的语义差异 —— 原理理解
• 掌握单点导航 navigate_to_pose 的命令行操作与反馈查看 —— 流程掌握
• 掌握多点连续导航 navigate_through_poses 与目标序列调度 —— 流程掌握
• 完成卡住恢复（progress_checker + behavior_server）观察与堵死通道测试 —— 流程掌握
• 用四指标评估多点导航鲁棒性并处理异常 —— 迁移应用

05实验环境与准备

06动画讲解：机器人依次访问 A→B→C 航点

图 6-1：一次下发 poses[] 三个途经点，小车按 A→B→C 顺序行驶，整条路线一次规划完成，每段都靠局部规划避障（橙色弧线是绕过纸箱的调整）；途经点进入 0.7 m 半径即被 RemovePassedGoals 剔除并切往下一段，最后一个点才按 0.10 m / 0.1 rad 精确到站——这就是它和"单点连发"的本质区别。

07互动讲解：单点 vs 多点导航器选择台

08实操步骤：send_goal 发送多目标并跟踪反馈

# ① 启动导航
ros2 launch car1_navigation navigation.launch.py map:=map_01

# ② 查看 action 定义（goal / feedback / result）
ros2 interface show nav2_msgs/action/NavigateToPose
ros2 interface show nav2_msgs/action/NavigateThroughPoses   # goal 是 poses[] 数组

# ③ 单点导航（frame_id 必须是 map）+ 实时反馈
ros2 action send_goal --feedback /navigate_to_pose nav2_msgs/action/NavigateToPose \
  "{pose: {header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 0.5}, orientation: {w: 1.0}}}}"

# ④ 多点连续导航：A→B→C 一次下发（注意点是"掠过"而非"逐点停"）
ros2 action send_goal /navigate_through_poses nav2_msgs/action/NavigateThroughPoses \
  "{poses: [{header: {frame_id: 'map'}, pose: {position: {x: 0.0, y: 0.5}, orientation: {w: 1.0}}},
            {header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 1.0}, orientation: {w: 1.0}}},
            {header: {frame_id: 'map'}, pose: {position: {x: 1.5, y: 0.5}, orientation: {w: 1.0}}}]}" --feedback

# ⑤ 跟踪轨迹与到达误差
ros2 topic echo /amcl_pose     # 时间序列可绘制成轨迹图验证"依次经过"

# ⑥ 观察恢复动作有没有真的触发
ros2 topic echo /behavior_tree_log

# ⑦ 调度脚本骨架：目标序列 + 失败重试（SUCCEEDED 才继续）
goals = [g1, g2, g3]
for g in goals:
    result = send_navigate_to_pose(g)          # 阻塞等待完成
    if result == SUCCEEDED:
        print(f"目标 {g} 完成")
    else:
        print(f"目标 {g} 失败 → 重试 1 次")
        result = send_navigate_to_pose(g)      # 仍失败则记录/跳过，禁止静默忽略

09重点与易错点

• 四指标合格线（参考）：到达率 ≥80%、避障响应 <2 s、中途停车 ≤1 次/任务
• 调度脚本先做"目标可达"预检查（costmap 判断），避免明知不可达还发
• 恢复动作测试时人远离小车，Spin 旋转半径内清场

10总结与思考：失败重试与进度检查