robot_agent的执行状态机与waypoint推进

继fleet_manager和path_planner这两个节点之后. 但如果继续顺着当前这个逻辑往下走,第三个必须单独拿出来写的模块,其实就是robot_agent.

原因很直接: fleet_manager负责把任务分出去,path_planner负责把任务翻译成route, robot_agent负责把route真正变成一个会流动的执行过程

也就是说,如果没有robot_agent, 前面两层再完整,系统也只是停留在:manager认为自己已经分配了任务,但任务并没有真正经历执行生命周期
这里就有关键的几个问题: robot_agent为什么本质上是一个简化执行状态机, waypoint是如何被逐步推进并转成状态回传的,当前这个agent模型已经解决了什么,又还没有解决什么

当前阶段目标

当前robot_agent在整个V1系统里的目标并不复杂,但很关键: 订阅task_assignments, 只接收发给自己的assignment, 保存route_waypoints,按周期推进当前waypoint,发布idle / executing / completed状态迁移

如果把这个目标放回整个项目背景里看,它其实是在做一件非常具体的事情:把planner给出的静态路线,转成manager可观察的动态执行过程

这一点之所以重要,是因为当前项目不是单机算法demo,而是一个多节点协调系统. 在这种系统里,"已经发布assignment"和"assignment真的被执行了"不是一回事.

先看接口: robot_agent到底消费什么,又产出什么

当前agent消费的输入是TaskAssignment.msg:

1
2
3

string robot_id
fleet_msgs/Task task
string[] route_waypoints

这个接口说明,agent接到的不是抽象任务意图,而是已经带好了离散路线的执行指令.换句话说,当前agent不负责再去理解pickup和dropoff之间怎么规划,它只需要回答下面两个问题:这是不是发给我的任务, 这条route我现在推进到哪里了

而它周期性发布的输出是RobotState.msg:

1
2
3
4
5
6

string robot_id
geometry_msgs/Pose pose
string current_waypoint
string status
float32 battery_percent
string current_task_id

这里最关键的不是pose本身,而是三件事:, status,current_waypoint, current_task_id

因为对fleet_manager来说,它真正需要知道的不是底层控制量,而是: 这个robot是不是空闲, 它正在执行哪个task,它已经推进到哪一个离散点

所以从系统语义看,robot_agent其实是在把route序列翻译成状态序列.

为什么说它本质上是一个简化执行状态机

看robot_agent_node.cpp会发现,当前实现没有很正式地写出一个枚举状态机,但它实际上已经具备了很清楚的状态迁移结构.

内部关键状态主要有: active_task_, completion_announced_,active_task_id_, route_waypoints_, route_index_, current_waypoint_

这些变量组合起来,基本就把一个最小执行器的内部过程表达出来了.粗略可以压缩成下面这个过程:

无任务
-> 收到属于自己的assignment
-> active_task_ = true
-> 沿route逐点推进
-> 发布completed
-> 清空任务上下文
-> 重新回到idle

注意这里最有意思的一点是:agent不是在收到assignment的瞬间就立刻宣布completed,也不是只维护一个布尔busy标志,而是显式保留了一个执行中间过程

这说明当前项目已经意识到,哪怕是最小demo执行器,也必须把"执行中"当成一个独立状态来建模. 否则manager永远看不到任务生命周期真正流动起来.

assignment是怎么被接住的

当前agent订阅task_assignments, 回如果msg->robot_id不是自己的robot_id, 直接忽略, 如果当前active_task_已经为真, 直接拒绝新assignment,如果route_waypoints为空, 直接忽略. 这三步虽然简单,但语义很清楚.

第一层: agent只处理发给自己的任务

这让task_assignments这个topic可以被多个agent同时订阅,但每个agent只消费属于自己的那一条.

第二层: 当前agent一次只允许一个active task

这与整个V1系统的调度假设一致. 现在还不是多任务队列执行器,而是单robot单任务模型.

第三层: 没有route就不接受assignment

这也再次说明当前系统协议是收敛的:agent消费的是可执行assignment,不是半成品任务

过滤通过后,agent会把: active_task_置为true,completion_announced_置为false, 保存active_task_id_, 保存整条route_waypoints_, 把route_index_重置为0,把current_waypoint_设成route第一个点

这里有一个值得注意的小细节:当前实现把current_waypoint_直接设成route_waypoints_.front().

这意味着assignment一旦被接收, agent内部就认为自己已经处在这条执行路线的起点语义上了. 对当前V1来说这是合理的,因为planner生成route时,第一点本来就应该是robot当前所在的离散waypoint.

waypoint推进是怎么发生的

当前agent没有单独的执行线程,而是把推进逻辑放进了一个1秒周期的定时器里. 每次定时器触发时都会调用publish_state().

这意味着当前执行模型不是:收到assignment后立刻高速推进完所有路径

而是: 每个周期发布一次状态,如果还在route中间,就把route_index_往前推进一格,同时更新current_waypoint_

从代码看,核心分支大概分成4类.

1. 没有active task

如果active_task_ == false, 直接发布: status = "idle", current_task_id = ""

这对应系统里的空闲态.

2. 还没走到route末尾

如果route_index_ + 1 < route_waypoints_.size(), 就:++route_index_ 把current_waypoint_更新为新索引对应点
/ 发布status = "executing"
/发布当前active_task_id_

这一步非常关键. 它说明当前执行语义不是"我有任务所以一直executing",而是:只要route还有后续waypoint, agent就在每个timer tick里显式推进一次离散执行位置

因此manager侧看到的不是静态busy,而是会随时间变化的current_waypoint.

3. 已经到达route末尾,但还没宣布完成

如果route已经走完并且completion_announced_ == false, agent不会立刻回到idle,而是先单独发布一次: status = "completed", current_task_id = active_task_id_

然后把completion_announced_置真.

这个设计很重要. 因为如果它在最后一个waypoint推进完成后立刻把任务上下文清空, manager就只能看到: 前一拍还在executing, 下一拍突然idle

中间就少了一个明确的完成事件.

而当前实现显式保留了一拍completed, 本质上是在告诉上游:这个任务不是消失了,而是已经完成了

4. 完成事件已经发过,才真正回到idle

下一拍再进入publish_state()时, agent才会:

1. active_task_ = false
2. 清空active_task_id_
3. 清空route_waypoints_
4. 把route_index_重置为0
5. 发布idle

这个两阶段尾声很像在状态机里补了一个"完成已确认"过渡态. 对demo系统来说,这个做法非常值.

为什么这个设计对fleet_manager是必要的

如果把robot_agent单独看,它只是个简单执行器. 但如果把它放回整个闭环里,就会发现它对fleet_manager的意义非常直接: manager需要看到robot何时进入executing.manager需要看到task何时进入completed.manager需要知道什么时候可以安全把robot重新视为idle

也就是说,agent真正提供的不是"路径执行能力"本身,而是:assignment生命周期的下游事实来源

这也是为什么上一篇fleet_manager专题会强调assignment状态保持. manager内部虽然会保留一份比topic表面值更强的分配真相,但最终它仍然需要agent给出明确的执行与完成反馈.

没有robot_agent, 当前V1系统最多只能证明:task能进入队列, planner能生成route,assignment能被发布

但还证明不了:route能不能被消费,task会不会真的走完,robot资源会不会正确释放

所以robot_agent这个模块虽然简单,但它恰好补上了整个主链路最后那段最关键的动态反馈.

waypoint坐标为什么也在agent里占一个位置

当前robot_agent除了维护current_waypoint_, 还会通过lookup_waypoint_position()给RobotState.pose填坐标.

坐标来源有两层: 优先读launch参数里的waypoint_positions, 如果没有配置或查不到,就退回内置fallback map

其中demo_map.yaml里把waypoint位置配置成:

1. dock_a:0.0:0.0
2. mid_1:1.0:0.0
3. pickup_zone:2.0:0.0
4. mid_2:3.0:0.5
5. dropoff_zone:4.0:1.0
6. dock_c:3.0:1.5

而demo.launch.py又把同一个配置文件传给path_planner和两个robot_agent, 再分别给两个agent设置: robot_1从dock_a起步,robot_2从dock_c起步

这件事的意义不只是"日志更好看". 它其实说明当前项目已经在做一个很实用的分层: planner负责离散图连通关系,agent负责把离散waypoint映射成可消费的位置状态

也就是说,当前RobotState不是纯逻辑状态,而是在往更真实的机器人状态接口靠拢.

遗留问题和现象1: 当前pose仍然是waypoint级离散跳变

它不是连续运动轨迹,没有速度,没有朝向演化,也没有局部路径跟踪. 所以现在的pose更适合作为demo状态输出,还不是底盘控制层的真实观测.

这个agent已经解决了什么

如果从当前V1工程目标看,robot_agent已经把几件非常关键的事做成了:

1. assignment不再停留在manager内部

现在任务真的可以被agent接住,而不是只在中心节点里自说自话.

2. route变成了可观察的执行过程

manager和日志都能看到current_waypoint逐步推进,这让闭环从"有route"变成"route在被消费".

3. completed事件被显式建模出来

这一点会直接影响manager何时释放robot资源,也让任务生命周期更完整.

4. 执行状态与地图配置产生了连接

同一份launch-time配置既服务planner,也服务agent状态输出,这让demo图和执行反馈不再是两套分裂语义.

当前它还没有解决什么

理解这个模块,同样要看它刻意没做的部分.

1. 没有真实导航控制

当前agent是按timer离散跳点,不是接Nav2,不是接底盘控制器,也不是订阅真实定位反馈.

2. 没有执行失败语义

目前状态只有idle / executing / completed. 如果机器人中途卡住,偏航,丢失waypoint,或者任务取消,系统还没有对应状态模型.

3. 没有中断/重分配处理

当前只要active_task_为真, agent就直接拒绝新assignment. 这对V1是合理的,但说明它还没有进入"可抢占执行器"阶段.

4. 没有更细粒度进度表达

现在的进度单位是waypoint跳变,不是边内进度,不是预计剩余时间,也不是更复杂的执行反馈.

5. battery目前是静态常量

battery_percent固定为100.0F, 这说明接口已经预留了资源状态字段,但当前运行语义还没有真正接进去.遗留问题和现象2: 当前robot_agent已经足以支撑任务闭环里的执行反馈,但它支撑的是"离散任务执行语义",还不是"真实机器人执行语义"

从这里往后最自然会接到哪里

把robot_agent分析完以后,当前系列其实形成了一个很清楚的三段式: fleet_manager把task接住并分出去, path_planner把task翻译成route, robot_agent把route翻译成执行状态流

到这里,V1最小闭环里的三个核心节点都已经拆开写过了.

现在已经可以去回答一个更大的问题:当前这个V1阶段到底已经真正完成了什么, 又还差什么

也就是: 哪些能力已经构成了可运行原型, 哪些能力还只是接口预留,为什么冲突检测,更强调度和真实导航接入会成为下一轮复杂度跃迁点

如果只用一句话总结当前这个robot_agent, 那就是:它现在做的不是机器人真实导航,而是把TaskAssignment稳定地翻译成一个可观察的执行状态机,从而让整个V1系统第一次真正拥有了从分配到完成的动态闭环

• Next Post →