Nav2 机器人导航
实验 2 我们给了小车一张地图。这节课让小车"学会看地图自己走路"——你在 RViz 里点一个目标点，它自己规划路线、自己转弯、自己躲开静态障碍，安全到达。这背后是 Nav2 导航栈在工作。
01问题引入：给了地图，它怎么自己走
地图存好了（map_01.yaml），目标也明确了（"从这走到那"），但中间一系列决策没人替小车做：
02原理通俗讲解：五大服务器与代价地图
| 服务器 | 白话类比 | 本项目配置 |
|---|---|---|
| planner_server | 导航员（画大路） | ThetaStarPlanner，期望 20 Hz 出图（行为树按 1 Hz 触发重规划） |
| controller_server | 司机（握方向） | DWB（FollowPath），20 Hz 控制循环 |
| bt_navigator | 总调度（对讲机） | navigate_to_pose_without_backup.xml / navigate_through_poses_without_backup.xml |
| behavior_server | 救援队 | spin / drive_on_heading / assisted_teleop / wait（未启用 backup） |
| velocity_smoother | 安全员（抹平速度） | max_velocity [0.4, 0, 3.0]，输出到 controller/cmd_vel |
坐标系 map，覆盖整张地图：static 静态层 + obstacle 障碍层 + inflation 膨胀层（inflation_radius 0.18 m），1.0 Hz 更新。
坐标系 odom，3 m × 3 m 滚动窗口跟着车跑：voxel_layer 体素层（z_voxels 16）+ inflation 层，5 Hz 刷新，装实时雷达点。
像地图周围撒一圈"热力晕"：0=空闲、1~252 按 cost_scaling_factor 4.0 衰减、254 障碍、255 未知。规划宁愿绕远也不进红区，车就不贴墙。
unconfigured → inactive（已配置未出力）→ active（干活）：两个 lifecycle_manager（localization / navigation）autostart 一键把 map_server、amcl 与五个服务器逐个激活。
controller_server 发 cmd_vel_nav → velocity_smoother 平滑 → 改名 controller/cmd_vel → 底盘 odom_publisher 收。teleop 走的才是 /cmd_vel。
03关联知识点
地图质量、定位准确、速度约束——任何一环掉链子，症状都是"走歪/撞墙/乱转"。
到点判定 xy 0.10 m、yaw 0.1 rad：不是"毫米级必须踩点"，留了合理误差带。
[[0.18,0.135],…] 底盘真实外廓（半长 0.18 m、半宽 0.135 m）参与碰撞检测——比圆形近似更贴墙也不剐蹭。
required_movement_radius 0.1、movement_time_allowance 10.0 s：10 秒挪不到 10 cm 就喊"卡了"，交给行为树恢复分支处理。
navigate_to_pose 单点；navigate_through_poses 一串点（EXP-06 用它）。两者由同一颗 bt_navigator 注册提供。
map 系"绝对世界"（AMCL 修正会跳变），odom 系"平滑里程"（局部控制用）——机器人导航标准双系。
本车是麦轮可横移，行为树文件名直接叫 without_backup，恢复只做"清代价地图 → 转 1.57 rad 找路 → 等 5 秒"，不后退。
04实验目标
- 跑通 navigation.launch.py map:=map_01，五个服务器全部 active
- RViz 中手动给初始位姿与目标点，小车自主规划、自主到达（误差 < 0.10 m）
- 读懂全局 / 局部两张 costmap 的帧系、尺寸、层数与刷新率差异
- 用命令行 action send_goal 发一次目标，观察 feedback
- 独立排查三类故障：贴墙走、原地转、规划失败
05实验环境与准备
| 项目 | 要求 |
|---|---|
| 地图 | EXP-02 产出的 map_01.pgm + map_01.yaml（通过四标准验收的图）；放在 slam/car1_slam/maps/ 下 |
| 软件包 | car1_navigation（navigation.launch.py + config/nav2_params.yaml + 两棵行为树 XML） |
| 硬件 | 小车 C1 满电、3 m × 3 m 以上空旷平整场地、人在旁监护 |
| 前置实验 | EXP-02 建图验收通过；理解 AMCL 默认初始位姿（0,0,0） |
| RViz | launch 自带 navigation.rviz：Map / Path / 两张 Costmap / PoseArray（/particle_cloud）/ LaserScan 已配好 |
06动画讲解：全局路径与膨胀绕障
青色折线 = planner 画的全局路径（避开膨胀热区）；小车沿线前进，到障碍近前被膨胀层（橙色晕圈）顶着走"外圈"，绝不贴死墙。
07互动讲解：全局规划 vs 局部控制对比台
同样一个"卡住"症状，两层的职责完全不同——点按钮切换场景，看各层该背锅还是该出手：
08实操步骤：navigation.launch 与 2D Goal
# ① 启动导航（map 参数是 maps/ 目录下的地图名，不含 .yaml 后缀） ros2 launch car1_navigation navigation.launch.py map:=map_01 # ② 自检五大服务器与定位节点是否就位 ros2 node list | grep -E "planner|controller|bt_navigator|behavior|amcl|map_server|velocity_smoother" # ③ 查看某服务器生命周期状态（预期 active） ros2 lifecycle get /planner_server # ④ 命令行发一个目标（与 RViz 点目标等价） ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose \ "{pose: {header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 1.0}, orientation: {w: 1.0}}}}" # ⑤ 看速度有没有真的下发到车（注意：平滑后的名字是 controller/cmd_vel） ros2 topic echo /controller/cmd_vel # 现场手势：rviz 顶部工具栏 2D Pose Estimate → Nav2 Goal（若配置里是 2D Goal Pose 同理）
TimerAction(period=10.0) 的是建图用的 slam.launch.py。09重点与易错点（故障排查）
| 症状 | 可能原因 → 解法 |
|---|---|
| A · 贴墙走/蹭墙 | 膨胀半径太小或没生效 → 查 inflation_layer 与 inflation_radius（0.18），确认 footprint 已设 |
| B · 原地打转不走 | 局部最小值/定位漂了 → 给准初始位姿（2D Pose Estimate），看 ParticleCloud 是否聚拢 |
| C · 规划失败"No valid plan" | 目标落在障碍/未知区、地图过期 → 重新点目标；确认用的是最新保存的 map_01 |
| 到点差一截就报成功 | goal 容差 xy 0.10 m 是正常范围；要更准调 goal_checker |
| 速度一冲一冲 | velocity_smoother 未起 → node list 查 /velocity_smoother，看 max_velocity 限幅 |
| 卡在 inactive | lifecycle_manager 未 autostart 或 bond 断了 → ros2 lifecycle set /xxx activate 手动激活并查该节点日志 |
- 五大服务器 全部 active，/map 正常加载 map_01
- RViz 点目标后小车 自主到达，全局路径自动绕开障碍热区
- 能指着两张 costmap 说出 map vs odom 帧系、尺寸、频率差异
- 能画清速度链路 cmd_vel_nav → velocity_smoother → controller/cmd_vel → 底盘
- 三个安全习惯：满电出发、人跟车、改参数先重启导航栈
- 记住红线：导航不与建图（slam.launch）同开——定位靠 AMCL 吃静态地图
10总结与思考
navigation.launch 跑通，RViz 与命令行双通道发目标均成功到点。
规划/控制/调度/恢复/平滑各司其职，全局看大路、局部看眼前。
node list 过滤、lifecycle get、action send_goal 带 frame_id 的完整姿势。
全局层（膨胀/规划）→ 局部层（DWB/平滑）→ 定位层（AMCL/初始位姿），对症下药。