系列： 2026 ROS 2 Lyrical 工程实例
环境： Windows 11、WSL2、Ubuntu 26.04、ROS 2 Lyrical、Gazebo Sim、Nav2、AMCL、RViz2
项目名称： 02-amr-mission-executor
完整代码： zephastra/robotics：02-amr-mission-executor
关键词： ROS 2、Lyrical、AMR、Nav2、任务状态机、异常恢复、低电量返航

前言

在工程实例（一）中，我们完成了 AMR 的激光 SLAM、地图保存、AMCL 定位和 Nav2 单目标导航。机器人已经能够回答一个基本问题：

给我一个目标点，我能不能到达？

但真实仓库不会只向机器人发送一个目标点。一个完整任务通常包含多个站点、不同业务动作以及一系列异常情况：

• 先去入库区确认货物；
• 再到指定货架取货或盘点；
• 前往质检区；
• 最后到出库区送货；
• 任务完成后返回充电区；
• 途中可能暂停、取消、遇到障碍或电量不足。

因此，能够调用 Nav2 并不等于拥有一套任务系统。我们还需要在 Nav2 上方增加“任务执行层”，让机器人知道：

去哪里 → 做什么 → 成功后去哪 → 失败后怎么办 → 什么时候必须返航

本文将工程一的单目标导航升级为一个可以独立运行的仓库多任务执行器，并实现状态机、失败策略、通道阻塞恢复、低电量返航、RViz2 状态显示和 JSON 任务报告。

最终版本已经完成实际验收：五个仓库任务全部完成，阻塞场景能够等待通道恢复后继续执行，低电量场景能够取消当前导航并自动返航，测试基线为 15 passed。

---

一、工程二解决了什么问题？

工程二是一个建立在 Gazebo + Nav2 之上的 AMR 自主任务执行器：它独立实现了差速机器人底盘和 2D LiDAR + 里程计，自带静态地图与定位（AMCL）配置，把 Nav2 单点导航作为底层执行能力；在此之上进一步封装出多站点任务调度（通过 YAML 配置任务点）、暂停/继续/取消（通过 Service 控制）、失败自动重试与跳过、通道阻塞检测、低电量自动返航，以及 JSON 格式的任务执行报告，从而把"只能导一次航"的基础能力升级成"能自动完成一整条多站点任务流并全程可管可控"的完整任务系统。

---

二、最终仓库任务流程

机器人从左下方充电区出发，依次完成五个业务站点：

顺序	任务 ID	业务名称	动作
1	inbound_confirmation	入库区货物确认	inspect
2	shelf_a_pickup	A 区货架取货	pickup
3	shelf_d_inventory	D 区货架盘点	inspect
4	quality_inspection	质检区检查	inspect
5	outbound_delivery	出库区送货	deliver

正常执行链如下：

充电区 Home
    ↓
入库区货物确认
    ↓
A 区货架取货
    ↓
D 区货架盘点
    ↓
质检区检查
    ↓
出库区送货
    ↓
自动返回充电区
    ↓
生成 JSON 报告

仓库场景约为 28 × 24 m，包含两组货架区、纵向窄通道、中央横向通道、入库区、出库区、质检区、充电区和可控制的红色测试货箱。

---

三、系统架构

工程的整体架构如下：

warehouse_world.sdf ──► Gazebo Sim ──► ros_gz_bridge
       │                                      │
       │                             /scan /odom /cmd_vel
       │                                      │
warehouse_map.yaml ──► AMCL + Nav2 ◄──────────┘
                              ▲
                              │ NavigateToPose
warehouse_demo.yaml ──► Mission Manager
                              │
             状态机 / 重试 / 阻塞恢复 / 自动返航
                              │
                 RViz2 Marker + Topic + JSON Report

各组件的职责如下：

组件	职责
Gazebo Sim	仓库环境、机器人运动和传感器仿真
ros_gz_bridge	桥接 /clock、/scan、/odom 和 /cmd_vel
AMCL	在静态地图中估计机器人位置
Nav2	规划路径、避障并输出速度指令
Mission Manager	解析任务、依次发送目标、处理异常与返航
Battery Simulator	发布模拟电量，提供强制低电量与复位接口
RViz2	显示地图、路径、代价地图和任务点状态
Report Writer	保存任务事件和最终执行结果

Nav2 仍然只负责“到达某一个目标”。任务管理器负责决定目标顺序以及每次导航前后的业务逻辑。

---

四、项目目录结构

克隆后的核心目录如下：

02-amr-mission-executor/
├── README.md
├── run_demo.sh                         # 一键启动三种演示模式
├── config/
│   ├── bridge_config.yaml              # Gazebo 与 ROS 2 桥接
│   └── nav2_params.yaml                # AMCL、Costmap 与 Nav2 参数
├── maps/
│   ├── warehouse_map.pgm               # 仓库占据栅格地图
│   └── warehouse_map.yaml
├── rviz/
│   └── warehouse_mission.rviz          # 任务可视化配置
├── scripts/
│   ├── amr_env.sh                      # 独立 ROS Domain 与 Gazebo Partition
│   ├── build.sh
│   ├── check_dependencies.sh
│   ├── test.sh
│   ├── run_mission.sh
│   ├── odom_to_tf.py
│   ├── block_aisle.sh                  # 放置测试障碍物
│   ├── clear_aisle.sh                  # 清除测试障碍物
│   ├── blocked_aisle_scenario.sh       # 自动阻塞恢复场景
│   └── trigger_low_battery_scenario.sh # 自动低电量场景
├── src/amr_mission_manager/
│   ├── package.xml
│   ├── setup.py
│   ├── config/warehouse_demo.yaml      # 仓库任务定义
│   ├── launch/mission_demo.launch.py
│   ├── amr_mission_manager/
│   │   ├── mission_models.py           # 数据模型与 YAML 校验
│   │   ├── mission_manager.py          # 任务状态机
│   │   ├── navigation_watchdog.py      # 导航进展与阻塞检测
│   │   └── battery_simulator.py        # 电量模拟器
│   └── test/                           # 功能单元测试
├── tests/
│   └── test_independent_assets.py      # 工程独立性测试
├── tools/
│   └── generate_warehouse_assets.py    # 生成世界与匹配地图
└── worlds/
    └── warehouse_world.sdf

---

五、使用 YAML 定义多站点任务

如果把目标点直接写死在 Python 中，每次改变任务都要修改代码。工程二把路线、动作、超时和异常策略统一放在 warehouse_demo.yaml 中。

任务级配置如下：

mission:
  id: warehouse_mission_002
  frame_id: map
  failure_policy: skip
  return_home: true
  return_home_on_cancel: true
  low_battery_threshold: 20.0

  blocked_timeout_seconds: 15.0
  minimum_progress_distance: 0.15
  recovery_wait_seconds: 6.0
  max_blocked_recoveries: 3

  home:
    name: charging_station
    x: -11.0
    y: -9.0
    yaw: 3.14

其中：

• failure_policy: skip：单个任务最终失败后跳过，继续执行下一站；
• return_home: true：任务结束后自动回到充电区；
• return_home_on_cancel: true：人工取消后仍然返航；
• low_battery_threshold: 20.0：电量低于或等于 20% 时触发返航；
• blocked_timeout_seconds：停止进展多久后判定阻塞；
• minimum_progress_distance：认定“发生有效进展”的最小距离；
• recovery_wait_seconds：阻塞后等待通道清除的时间；
• max_blocked_recoveries：同一任务允许的最大阻塞恢复次数。

单个任务配置如下：

- id: shelf_a_pickup
  name: A 区货架取货
  x: -5.0
  y: 4.5
  yaw: 1.57
  action: pickup
  stay_seconds: 4.0
  retries: 2
  timeout_seconds: 120.0

这里不仅有目标位姿，还包含业务动作和失败策略：

到达目标点
    ↓
执行 pickup 动作
    ↓
在站点停留 4 秒
    ↓
标记任务完成并进入下一站

当前示例中的 inspect、pickup 和 deliver 使用停留时间模拟业务动作。接入真实仓库时，可以把它们替换为机械臂 Action、PLC Service、扫码器 Topic 或仓储系统 API。

mission_models.py 会在启动时校验 YAML，包括任务 ID 是否重复、动作是否合法、超时是否大于零、电量阈值是否在 0～100 之间。配置错误会在机器人运动以前暴露，而不是运行到一半才失败。

---

六、任务状态机设计

任务执行器定义了以下主要状态：

IDLE
WAITING_FOR_NAV2
NAVIGATING
EXECUTING_TASK
PAUSED
RETRYING
BLOCKED
WAITING_FOR_CLEARANCE
RECOVERING
LOW_BATTERY
RETURNING_HOME
COMPLETED
COMPLETED_WITH_WARNINGS
CANCELED
FAILED

正常任务的状态变化为：

IDLE
  ↓
WAITING_FOR_NAV2
  ↓
NAVIGATING ⇄ EXECUTING_TASK
  ↓        （循环执行五个站点）
RETURNING_HOME
  ↓
COMPLETED

发生通道阻塞时：

NAVIGATING
  ↓
BLOCKED
  ↓
WAITING_FOR_CLEARANCE
  ↓
RECOVERING
  ↓
NAVIGATING

发生低电量时：

NAVIGATING / EXECUTING_TASK
  ↓
LOW_BATTERY
  ↓
RETURNING_HOME
  ↓
COMPLETED_WITH_WARNINGS

状态机的价值是让“当前发生了什么”变得可观察、可记录，也让暂停、取消、低电量和阻塞等事件不会被写成互相覆盖的条件分支。

---

七、从单个 Nav2 Goal 到任务队列

MissionManager 继承 Nav2 Simple Commander 的 BasicNavigator：

class MissionManager(BasicNavigator):
    """Execute a validated mission and coordinate recovery and return-home logic."""

每个 YAML 任务都会转换为一个 PoseStamped，然后提交给 Nav2：

self._set_state(state, f"Navigating to {pose.name}")
running_task = self.goToPose(self._pose_stamped(pose))

任务提交后不能直接阻塞等待，因为执行过程中还要响应暂停、取消、电量变化和阻塞检测。因此程序在循环中持续检查：

while not self.isTaskComplete(running_task):
    if self.cancel_requested:
        ...
    if self.pause_requested:
        ...
    if self._is_low_battery():
        ...

    feedback = self.getFeedback(running_task)
    ...

导航结束后再读取结果：

result = self.getResult()
if result == TaskResult.SUCCEEDED:
    return "succeeded"

所以，一次任务执行并不是简单的：

发送目标 → 等待结果

而是：

发送目标
  ↓
持续处理 Nav2 Feedback
  ├─ 检查距离
  ├─ 检查电量
  ├─ 检查暂停和取消
  ├─ 检查总超时
  └─ 检查是否阻塞
  ↓
得到结果并决定下一步

---

八、为什么不能只依赖 Nav2 的成功或失败？

真实运行中，机器人可能长期停在障碍物前，但 Nav2 Action 仍然没有立即返回失败。它也可能不断进行局部恢复，看起来一直在运动，却始终没有真正接近目标。

如果任务层只等待最终结果，就无法及时告诉上层系统“通道可能被占用”。因此项目增加了 NavigationProgressWatchdog。

它同时观察两个指标：

1. 机器人实际位置是否发生了足够位移；
2. Nav2 Feedback 中的 distance_remaining 是否真正减小。

关键逻辑可以概括为：

improvement = reference_distance - distance

position_progress = hypot(
    current_x - reference_x,
    current_y - reference_y,
) >= minimum_progress_distance

只观察机器人位置不够。机器人可能在障碍物前反复左右摆动，位置发生变化，但距离目标并没有改善。

只观察剩余距离也不够。在局部路径变化或反馈抖动时，单个数值可能不能准确反映机器人是否完全停止。

因此阻塞判断采用双条件：

stopped_too_long = now - last_motion_at >= timeout_seconds

no_goal_progress_too_long = (
    now - last_progress_at >= timeout_seconds * 3.0
)

return stopped_too_long or no_goal_progress_too_long

在默认配置下：

• 机器人连续 15 秒没有达到 0.15 m 的位移，判定停止阻塞；
• 机器人虽然在移动，但连续 45 秒没有向目标产生至少 0.15 m 的有效进展，同样判定阻塞。

这里使用 ROS 仿真时钟，而不是 Windows 或 Linux 的墙上时间。Gazebo 运行速度低于实时速度时，任务不会因为电脑负载较高而被提前取消。

---

九、阻塞以后如何恢复？

检测到阻塞后，任务管理器不会立即无限重发同一个目标，而是先取消当前 Nav2 Goal：

检测阻塞
   ↓
向 Nav2 请求取消当前目标
   ↓
等待取消结果真正结束
   ↓
进入 BLOCKED
   ↓
进入 WAITING_FOR_CLEARANCE
   ↓
等待 6 秒
   ↓
进入 RECOVERING
   ↓
重新提交同一目标

为什么要等待取消完成？因为旧目标尚未结束时就提交新目标，可能出现两个导航任务在时间上重叠，状态和结果也会变得难以判断。

阻塞恢复与普通失败重试是两套计数：

• blocked_recoveries：路径被占用后的恢复次数；
• retries：非阻塞导航失败后的普通重试次数。

将两者分开以后，最终报告可以回答：机器人是因为规划或控制失败而重试，还是因为仓库通道被临时占用而等待。

超过 max_blocked_recoveries 后，任务根据 failure_policy 选择：

skip  → 标记当前任务失败或跳过，继续下一站
abort → 终止整条任务链并进入返航/失败流程

---

十、可重复的通道阻塞演示

为了让阻塞恢复不是“靠手工碰巧复现”，项目提供了自动场景：

bash run_demo.sh blocked_aisle

脚本不会简单地在固定秒数后放置障碍，因为不同电脑的 Gazebo 启动速度不同。它会等待任务管理器真正开始执行 shelf_a_pickup，再把红色测试货箱移动到 A 区目标附近。

完整演示过程为：

任务开始 shelf_a_pickup
        ↓
红色货箱自动进入通道
        ↓
机器人无法继续接近目标
        ↓
Watchdog 判定 BLOCKED
        ↓
任务进入 WAITING_FOR_CLEARANCE
        ↓
场景脚本确认恢复计数增加
        ↓
红色货箱自动移走
        ↓
任务重新导航并继续后续站点

这样得到的演示与电脑性能无关，更适合录制视频、自动测试和文章复现。

也可以手动控制货箱。另开终端并加载项目环境：

cd robotics/projects/02-amr-mission-executor
source scripts/amr_env.sh

放置障碍物：

bash scripts/block_aisle.sh

清除障碍物：

bash scripts/clear_aisle.sh

---

十一、低电量为什么必须打断当前导航？

电池模拟器通过下面的话题发布电量：

/mission/battery_percentage

默认初始电量为 100%，按照配置持续下降。任务管理器在导航和站点动作执行期间都会检查：

def _is_low_battery(self) -> bool:
    return (
        self.battery_percentage is not None
        and self.battery_percentage <= self.mission.low_battery_threshold
    )

电量达到阈值后，程序不会等待当前任务自然结束，而是：

1. 取消当前 Nav2 Goal；
2. 将状态切换为 LOW_BATTERY；
3. 保留尚未完成的任务；
4. 进入 RETURNING_HOME；
5. 返航时忽略低电量重复触发；
6. 回到充电区后生成报告；
7. 最终状态为 COMPLETED_WITH_WARNINGS。

低电量演示命令：

bash run_demo.sh low_battery

也可以在另一个终端中手动触发：

ros2 service call \
  /mission/battery/force_low \
  std_srvs/srv/Trigger

重置电量：

ros2 service call \
  /mission/battery/reset \
  std_srvs/srv/Trigger

这仍然只是电量模拟器。接入真实机器人时，可以订阅驱动层或 BMS 发布的 sensor_msgs/msg/BatteryState，任务状态机的返航逻辑可以继续复用。

---

十二、暂停、继续与取消

任务管理器提供三个 ROS 2 Service：

ros2 service call /mission/pause std_srvs/srv/Trigger
ros2 service call /mission/resume std_srvs/srv/Trigger
ros2 service call /mission/cancel std_srvs/srv/Trigger

暂停不是冻结进程。为了避免机器人继续执行旧目标，系统会先取消当前导航，然后进入 PAUSED，等待继续指令。恢复后重新提交当前任务目标。

取消时，系统根据：

return_home_on_cancel: true

决定是否返回充电区。本实例设置为 true，所以取消任务并不意味着让机器人停在仓库通道中，而是结束业务任务后安全返航。

---

十三、RViz2 与运行状态观察

RViz2 中的任务点使用不同颜色表示状态：

颜色	含义
蓝色	等待执行
黄色	正在导航或执行
绿色	任务成功
红色	任务失败
灰色	任务被跳过
紫色	Home 充电区

除了图形界面，还可以从 Topic 观察系统：

ros2 topic echo /mission/status
ros2 topic echo /mission/events
ros2 topic echo /mission/battery_percentage

/mission/status 包含：

当前状态
当前任务及序号
任务总数
当前电量
成功、失败、跳过数量
普通重试次数
阻塞恢复次数
累计阻塞等待时间

/mission/events 发布状态变化、导航开始、导航失败、阻塞发生、任务完成和返航结果等结构化事件，适合后续接入日志平台或仓库管理系统。

---

十四、JSON 报告记录了什么？

每次任务结束后，项目会在 reports/ 中生成一个 JSON 文件：

warehouse_mission_002-YYYYMMDD-HHMMSS.json

核心字段包括：

{
  "mission_id": "warehouse_mission_002",
  "final_state": "COMPLETED",
  "completed_tasks": [],
  "failed_tasks": [],
  "skipped_tasks": [],
  "total_retries": 0,
  "total_blocked_recoveries": 1,
  "total_blocked_wait_seconds": 6.0,
  "final_battery_percentage": 84.65,
  "events": []
}

上面的数组内容仅为结构示意，实际报告会写入完整任务 ID 和事件记录。

报告让一次演示从“看起来跑完了”升级为可以审计的结果：

• 哪些任务完成了？
• 哪些任务失败或跳过了？
• 是否发生过阻塞？
• 等待了多久？
• 是否进行过普通重试？
• 最后是否安全返航？
• 结束时还剩多少电量？

真实系统还可以继续增加任务耗时、各站点等待时间、路径长度和能耗等指标。

---

十五、克隆并运行完整项目

完整代码地址：

https://github.com/zephastra/robotics/tree/main/projects/02-amr-mission-executor

克隆仓库：

git clone https://github.com/zephastra/robotics.git
cd robotics/projects/02-amr-mission-executor

检查依赖：

bash scripts/check_dependencies.sh

1. 正常五站任务

bash run_demo.sh normal

脚本会依次启动：

[1/7] Gazebo 仓库场景
[2/7] Gazebo Bridge
[3/7] TF 发布节点
[4/7] 静态地图与 AMCL
[5/7] RViz2
[6/7] Nav2
[7/7] Mission Manager

任务会自动开始，不需要在 RViz2 中手动设置 Initial Pose 或 Nav2 Goal。

2. 通道阻塞恢复

bash run_demo.sh blocked_aisle

3. 低电量返航

bash run_demo.sh low_battery

4. 无界面运行

AMR_HEADLESS=1 bash run_demo.sh normal

5. 只启动仿真、定位和导航

AMR_SKIP_MISSION=1 bash run_demo.sh

按 Ctrl+C 后，脚本会按进程组停止自己启动的全部进程，避免 Gazebo、Bridge、AMCL 或 Nav2 残留并干扰下一次运行。

---

十六、测试与实际验收

运行自动测试：

bash scripts/test.sh

当前项目测试基线为：

15 passed

测试覆盖任务配置校验、状态模型、阻塞 Watchdog、Nav2 取消行为和工程资源独立性等内容。

端到端验收不只检查节点能否启动，还实际执行仓库任务。最终验证结果如下：

验收项目	结果
Gazebo 仓库、独立 AMR 与传感器	通过
/scan、/odom、/cmd_vel Bridge	通过
map → odom → base_link → laser_link	通过
AMCL 与 Nav2 启动	通过
五个正常仓库任务	全部完成
失败任务	0
跳过任务	0
正常场景普通重试	0
自动返航	第一次尝试成功
最终状态	COMPLETED
正常任务结束电量	约 84.65%
阻塞场景	发生 1 次阻塞恢复后继续完成
自动化测试	15 passed

一个需要强调的区别是：

测试通过 ≠ 机器人任务通过

单元测试验证算法和边界条件，端到端演示则验证 Gazebo、Bridge、TF、AMCL、Nav2 和 Mission Manager 是否真的连成一条可工作的链路。两者缺一不可。

---

十七、常见问题

1. 一直等待 AMCL

脚本会等待 map → odom 出现。如果超时，检查：

ros2 run tf2_ros tf2_echo map odom
ros2 topic echo /scan --once
ros2 topic echo /initialpose --once

还要确认新终端加载了本项目环境：

source scripts/amr_env.sh

2. 一直等待 Nav2 Action Server

ros2 action info /navigate_to_pose
ros2 lifecycle nodes

如果没有 Action Server，检查 logs/navigation.log 和 Nav2 生命周期节点。

3. 机器人反复恢复但无法通过

查看通道是否真的清除，并检查：

ros2 topic echo /mission/status
ros2 topic echo /mission/events

如果是固定障碍物或目标点本身位于障碍物中，等待和重试不会解决问题，应修改任务坐标、地图或 Costmap 配置。

4. 低电量后没有返航

检查电量和任务参数：

ros2 topic echo /mission/battery_percentage

low_battery_threshold: 20.0
return_home: true

5. 运行第二次时出现重复节点

先在原运行终端按 Ctrl+C。如果此前异常关闭终端，可以检查：

pgrep -af "warehouse|nav2_|amcl|ros_gz_bridge|mission_manager"

不要直接在多个终端重复执行 run_demo.sh。

---

十八、从演示工程走向真实仓库

当前项目已经具备任务层的主要结构，但业务动作和电池仍然是模拟的。迁移到真实机器人时，可以沿着以下方向扩展：

1. 使用 BMS 的 BatteryState 替换电量模拟器；
2. 将 pickup 和 deliver 接入机械臂 Action 或 PLC Service；
3. 从 WMS/MES 动态获取任务，而不是只读取本地 YAML；
4. 给每个任务增加优先级、截止时间和前置依赖；
5. 增加任务断点保存，进程重启后继续执行；
6. 将 JSON 报告上传到数据库或可视化看板；
7. 从单机器人任务队列扩展到多机器人调度与交通管制。

最重要的是保持分层：

Nav2 负责“怎样到达”
Mission Manager 负责“到哪里、做什么、失败怎么办”
WMS/MES 负责“为什么执行这项业务任务”

每一层只承担自己的职责，系统才容易测试、替换和扩展。

---

十九、总结

工程实例（二）完成了从“会导航”到“会执行任务”的升级。

我们在 Nav2 上方加入任务执行层，实现了：

• YAML 多站点任务定义；
• 明确的任务状态机；
• NavigateToPose 的顺序调度；
• 暂停、继续和取消；
• 导航超时、重试、跳过和终止策略；
• 基于实际位移与剩余距离的双指标阻塞检测；
• BLOCKED → WAITING_FOR_CLEARANCE → RECOVERING 恢复链；
• 低电量取消当前任务并自动返航；
• RViz2 Marker、状态 Topic、事件 Topic 和 JSON 报告；
• 正常、阻塞和低电量三种可重复演示模式。

最终系统能够完成五站仓库任务，并在通道被占用或电量不足时作出明确、可观察、可记录的处理。

第一篇文章让机器人拥有了地图和导航能力；第二篇文章则让机器人开始具备任务意识。接下来还可以继续加入真实业务接口、动态任务分配和多机器人协同。

---