RoboCup仿真救援代码解析：ROS多智能体目标分配与通信受限实战

简介：面向参加RoboCup Rescue仿真救援竞赛及学习多智能体决策的开发者，这份代码包提供一套可运行的仿真救援软件实现，覆盖灾害场景建模、搜索策略、路径规划、目标识别、避障与通信等核心环节，也适合作为人工智能与机器人课程的项目参考。压缩包共43个文件，以Java源码为主体（42个.java及1个备份文件），整体约74KB，体量轻便，便于在本地环境中阅读、编译和二次修改；从目录组织看，代码按基础工具、实体对象、决策模块等层次拆分，结构相对清晰。已有1579人学习下载，说明其在竞赛入门与算法复现方面具有一定参考价值。读者可从中了解仿真环境与传感器数据的处理方式、控制指令的生成逻辑，以及日志记录和效果评估的组织方法，进而梳理自主救援系统的模块边界与实现思路，为课程设计、竞赛预研和算法实验提供可借鉴的代码基础。

1. 从一局跑不动的仿真说起：这份仿真救援代码解决什么问题

第一次把这份仿真救援代码拖进工作空间，roslaunch敲下去，终端刷了半屏红字，rviz 里只剩一片灰——地图没起来，agent 也没注册上。这是大多数人打开它的第一幕。它干的是 RoboCup 仿真救援那套事：在一片被压塌的城市街区上，消防、警察、救护三类智能体在通信带宽被掐、信息有延迟、决策窗口只有几百步的条件下分工救人，最后由仿真内核按建筑存活和平民存活打分。它适合两类人：想练多智能体协同、目标分配、受限通信下决策的开发者，以及想找一个完整 ROS 工程、从内核一路拆到 agent 的练手样本的人。拿它当游戏玩，很快就会腻；拿它当架构样本，能拆的东西比想象中多。

2. 技术栈与运行骨架：一次仿真是怎么被拉起来的

这套代码的骨架比功能更值得先看。它不是单体程序，而是一组进程靠消息总线拼起来的系统：内核负责推世界、agent 负责做决策、可视化负责给人看、记录器负责事后复盘。把这四者的边界认清，后面改任何模块都不会迷路。

2.1 技术选型盘点：为什么是 ROS + C++ 内核 + 混合语言 agent

代码包是 ROS1 时代的产物，Melodic 和 Noetic 两个发行版都有人跑，工作空间的构建走 catkin。仿真内核用 C++ 写，原因很直接——每一步要推进几百个实体的位置、火势蔓延、建筑倒塌判定，纯 Python 撑不住这个频率；agent 侧则是 C++ 和 Python 混着来，高频的感知与指令下发用 C++，策略、目标分配这类需要反复试参数的逻辑用 Python 写原型，跑通了再下沉。

内核和 agent 拆成两个进程组，是这套架构里最关键的一个决定。agent 崩了只影响它自己那一队，整局还能继续跑；反过来，内核崩了整局直接废掉，所以内核侧几乎不允许有裸指针和未捕获异常。这个约束在后续二次开发时会被反复撞上：写策略时可以随便试错，写内核插件时就要当成生产代码对待。

时间基准靠/clock话题统一。内核按固定步长推进仿真时间并向/clock发布，所有订阅方都必须把use_sim_time打开，否则节点内部用的是墙钟时间，会出现「agent 觉得已经过了 5 秒、内核只推进了 0.5 秒」的错位，表现出来就是决策周期和仿真步长完全对不上。这类问题在后面的排查章节里会展开。

还有一类老分支：内核是 Java 写的，agent 靠 TCP socket 连过去收发 JSON，没有 ROS 这一层。判断手里这份属于哪种很简单的：看src/下有没有CMakeLists.txt和package.xml，有就是 ROS 版；看到一堆.jar和config.txt就是旧版。两套的算法逻辑可以互抄，但目录结构和启动方式完全不通用，别混着查文档。

2.2 启动链路：从 roscore 到 agent 注册的完整顺序

一次完整的仿真启动是有严格顺序的，顺序错了就是各种连不上。我给一个典型链路，实际包里的 launch 文件会把它们串起来，但知道底下发生了什么，出问题时才知道该看谁。

# 1. 先起消息总线和参数服务器 roscore & # 2. 起仿真内核：加载地图、初始化实体、开始发布 /clock 与 /world/graph rosrun rsl_core core_node _map_file:=$(rospack find rsl_core)/maps/city_01/map.bin \ _time_step:=0.1 \ _max_step:=300 # 3. 等内核把 /clock 发出来再起 agent，否则订阅方拿不到仿真时间 rosparam set /use_sim_time true rosrun rsl_agent agent_node _team_id:=1 _agent_type:=fire # 4. 批量起剩下的 agent（launch 里通常写成多实例） roslaunch rsl_agent team.launch team_id:=1 # 5. 可视化与记录 rosrun rviz rviz -d $(rospack find rsl_tools)/config/rescue.rviz rosbag record -O round1.bag /world/graph /cmd/*

这里每一步都有它的道理。_time_step是内核推进的最小时间单位，单位是秒，取值太小会让实体的移动显得黏滞、计算量翻倍，太大则传感器采样变粗；_max_step是一局的总步数，两者相乘就是这一局的仿真时长，通常落在 30 秒左右的时间窗里，真正的评分看的是这几十秒内救了多少人、塌了多少楼。_team_id必须全局唯一，两台 agent 用了同一个 id，后注册的那台会被内核直接踢掉，而且日志里只有一行不显眼的告警。

提示：launch 文件里如果写了<param name="use_sim_time" value="true"/>，就不要在命令行再设一遍，两处不一致时后者可能覆盖前者，表现是时好时坏。

2.3 目录结构：哪个包该动、哪个包别碰

代码包的目录大致是这么分的，不同分支的命名会有一点出入，但角色就这几个：

rescue_ws/ ├── src/ │ ├── rsl_core/ # 仿真内核，别轻易动 │ │ ├── maps/city_01/ # 地图数据：建筑、道路、平民初始位置 │ │ ├── config/sim.yaml # 一局的参数：实体数量、步长、评分权重 │ │ └── src/world.cpp # 世界推进主循环 │ ├── rsl_agent/ # 智能体框架，二次开发的主战场 │ │ ├── include/perception.h # 感知接口 │ │ ├── src/decision_node.cpp # 决策主循环（C++） │ │ └── scripts/policy.py # 策略原型（Python） │ ├── rsl_msgs/ # 消息定义，改了要重新编译全工程 │ └── rsl_tools/ # 评分、回放、可视化配置 ├── build/ └── devel/

分界线很清楚：rsl_msgs和rsl_core是底座，改它们等于给整套系统换地基，除非你有明确的性能瓶颈或者要加新的传感器类型，否则别碰；rsl_agent是应用层，换策略、加启发式、改目标分配，都在这里动手；rsl_tools是观测层，改它的收益被严重低估——很多人调策略调到崩溃，其实只是缺一个能把每步的目标分配画出来的可视化。

rsl_msgs有个隐性成本：改一次.msg就要全量重编，而且旧的 bag 包直接读不了。所以加字段时尽量往后追加，不要插在中间，也不要改已有字段的类型，这是我被坑过之后长期遵守的一条规矩。

3. 核心模块拆解：感知、决策、通信的三段式链路

agent 从收到世界模型到发出行动指令，中间只有三段：把消息翻成自己认识的实体表，在实体表上算目标分配，把分配结果转成内核能执行的指令。三段各自的失败模式完全不同，分开看能省很多时间。

3.1 感知层：把世界模型翻译成本地实体表

内核每步广播一次全量世界快照，但 agent 不该直接拿着原始消息写业务逻辑。中间加一层本地实体表，好处是后面换数据结构、加缓存、做历史轨迹都不用动决策代码。

// perception.h // 感知层：订阅内核广播的世界模型，维护一份本地实体快照 class Perception { public: struct Entity { uint32_t id; uint8_t type; // 0=建筑 1=平民 2=消防车 3=警车 4=救护车 float x, y; // 地图坐标，单位米，原点在地图几何中心 float hp; // 实体剩余耐久，建筑为结构强度，平民为生命值 bool buried; // 平民是否被埋在废墟下，只有警察能标记 }; void onWorldGraph(const rsl_msgs::WorldGraph::ConstPtr& msg) { std::lock_guard<std::mutex> lk(mtx_); last_update_step_ = msg->step; // 消息携带的仿真步号，用于判陈旧 entities_.clear(); for (const auto& e : msg->entities) { // 只保留视野内的实体，视野外的一律丢弃，避免决策时误用 if (!inRange(e.x, e.y, self_x_, self_y_, view_range_)) continue; entities_.push_back({e.id, e.type, e.x, e.y, e.hp, e.buried}); } } // 按类型筛选，决策层最常用的入口 std::vector<Entity> byType(uint8_t type) const { std::lock_guard<std::mutex> lk(mtx_); std::vector<Entity> out; for (const auto& e : entities_) if (e.type == type) out.push_back(e); return out; } private: mutable std::mutex mtx_; std::vector<Entity> entities_; uint32_t last_update_step_ = 0; float self_x_ = 0, self_y_ = 0, view_range_ = 30000.0f; // 视野半径，米 };

这段代码有三个地方值得说。lock_guard是必要的，ROS 的回调线程和决策主循环是两个线程，不加锁就是随机崩溃，而且崩得毫无规律，排查起来极其难受。view_range_的裁剪看着像优化，实际是建模的一部分——仿真里 agent 的感知本来就有限，视野外的东西你看不见，硬塞进来会让策略学到不存在的先验。last_update_step_记下来是为了判陈旧：如果决策主循环发现当前步号比last_update_step_大出好几步，说明世界模型断了，这时候宁可原地待命也别按旧数据冲。

坐标单位统一用米、原点在地图几何中心，这一点在跨模块传参时最容易翻车。有的分支地图文件里存的是栅格索引，有的存的是 UTM 偏移量，读地图的代码里如果没做统一，agent 会朝着一个完全正确的方向走但永远到不了——因为目标点和自己在两套坐标系里。

3.2 决策层：目标分配是这套系统的真正核心

救援仿真的难点从来不是「看到火就去灭」，而是「8 台消防车、12 处火点、2 条被堵的路，先派谁去哪」。目标分配做不好，所有 agent 会一起冲向最近的那栋楼，剩下的火点烧到塌。

常见做法是先算代价矩阵再做全局匹配，规模小的时候用匈牙利算法，规模大或者要实时就用贪心加去重。给一个可以直接跑的贪心版本：

# policy.py # 目标分配：按「到达时间 + 目标紧迫度」排序后贪心匹配，带全局去重 import math def urgency(entity, step, max_step): """紧迫度：剩余耐久越低、离塌陷越近，权重越高""" hp_ratio = max(entity["hp"], 1e-6) / 100.0 # 步数越往后，濒危目标越优先，避免前期铺得太开、后期来不及 time_pressure = 1.0 + 2.0 * (step / float(max_step)) return time_pressure / hp_ratio def assign(agents, targets, step, max_step, speed=800.0): """返回 [(agent_id, target_id)]，一个目标只派一个 agent""" pairs = [] for a in agents: for t in targets: dist = math.hypot(a["x"] - t["x"], a["y"] - t["y"]) travel = dist / speed # 预计到达耗时，秒 score = urgency(t, step, max_step) / (travel + 1.0) pairs.append((score, a["id"], t["id"])) pairs.sort(reverse=True) # 收益高的先配 used_a, used_t, result = set(), set(), [] for score, aid, tid in pairs: if aid in used_a or tid in used_t: continue used_a.add(aid) used_t.add(tid) result.append((aid, tid)) return result

urgency里那个time_pressure是我加的经验项。纯按距离贪心的话，前期 agent 会跑去救远处的高价值目标，等走到那儿建筑已经塌了；把步数进度折算进紧迫度，等于让策略随时间自然地变得更保守。speed是 agent 的移动速度，单位米每秒，这个值必须和内核实体的实际速度对上，填大了会严重低估到达时间，派出去的车半路发现目标没了，等于白白浪费一趟。

贪心的缺点是不保证全局最优，两个 agent 的收益差很小时容易配错。规模在几十个目标以内，换成匈牙利算法一局也就多几毫秒，值得。但要注意匈牙利要求代价矩阵是方阵，agent 数和目标数不等时得补虚拟行列，补的时候虚拟成本要给一个足够大的常数，不然会强行凑出无效配对。

还有一个容易被忽略的点：分配结果是每步重算的，但 agent 走到一半的路程不该被浪费。实际跑的时候我会加一个「锁定窗口」——已经出发且目标还在的目标，保持配对至少若干步，除非目标消失。不加这个，agent 会在两栋楼之间来回横跳，一局下来哪儿都没到。

3.3 通信层：带宽模拟与消息裁剪

这套仿真最有意思的设计是通信不是免费的。内核会按配置模拟丢包和延迟，agent 之间的消息可能晚到几步甚至干脆到不了。中心决策节点虽然能看到全局，但它广播的指令同样受这条约束。

# msg/WorldGraph.msg Header header uint32 step # 当前仿真步号，接收方用它判断数据新旧 Entity[] entities # 全量实体快照，视野内 uint8[] roads_blocked # 被阻断的道路 id 列表

# msg/AgentCommand.msg uint32 agent_id uint8 action # 0=待命 1=移动 2=灭火 3=清障 4=救援 float32 target_x float32 target_y uint32 target_entity # 作用对象 id，移动时填 0 uint32 expire_step # 过期步号，超过就作废

expire_step字段是必须的，而且是很多人第一次写 agent 时会漏掉的。没有过期机制，一条延迟很久的指令到了之后 agent 还在执行，人早就被救走了或者楼已经塌了，表现出来就是 agent 在原地做一些莫名其妙的小动作。加上过期判定，决策层只需要写一句「当前步号超过 expire_step 就丢弃」，整条链路的时序问题就收敛了。

队列长度也要控制。ROS1 的subscribe第三个参数是队列长度，默认值偏大，世界模型这种高频全量消息如果积压，agent 会拿几十步之前的数据做决策，效果比丢包还糟。我一般给世界模型设 1，宁可丢旧帧也不要迟到帧；给自身状态消息设 10，那个频率高但数据小，允许少量堆积。

4. 跑通第一局：编译、地图加载与评分闭环

理论和代码都清楚了，接下来是最实际的一步——让它在这台机器上跑出一局有分数的仿真。这一节按顺序来，每一步都有可以对照的产物，卡住了就知道停在哪一环。

4.1 环境准备与工作空间编译

依赖不复杂，但版本要对上。ROS1 桌面版装好之后，补几个常见的库就行：

# 依赖安装：编译工具、数学库、可视化插件依赖 sudo apt-get install -y build-essential cmake libeigen3-dev \ ros-$ROS_DISTRO-rviz ros-$ROS_DISTRO-tf # 建工作空间，把代码包解到 src 下（不要嵌套两层目录） mkdir -p ~/rescue_ws/src cd ~/rescue_ws/src # 代码包解压后应直接看到 rsl_core / rsl_agent / rsl_msgs / rsl_tools 四个目录 ls cd ~/rescue_ws catkin_make -DCMAKE_BUILD_TYPE=Release -j$(nproc) source devel/setup.bash

CMAKE_BUILD_TYPE=Release不能省。内核侧全是浮点运算和几何判定，Debug 模式下慢一个数量级，本来一局跑 30 秒仿真时间要 10 秒墙钟，换成 Debug 可能要两分钟，调策略时根本等不起。-j$(nproc)用满核，编译时间从几分钟压到一分钟以内。

编完先别急着起仿真，跑一遍包列表确认所有包都被 catkin 认到了：

rospack list | grep rsl_ # 应该能看到 rsl_agent / rsl_core / rsl_msgs / rsl_tools 四行

只有三个或者少一个，通常是某个包缺package.xml，或者src下多套了一层目录，catkin 扫不到。

4.2 地图与配置：一局的参数都在这里

地图不是一张图片，是一组二进制数据和描述文件。典型结构是这样：

maps/city_01/ ├── map.bin # 建筑多边形、道路拓扑、平民初始位置，二进制 ├── map.yaml # 地图元信息：尺寸、原点、坐标系 └── scenario.yaml # 这一局的实体数量与分布

# config/sim.yaml —— 一局的全局参数 world: map: city_01 time_step: 0.1 # 单步仿真时长（秒） max_step: 300 # 总步数，乘出来是 30 秒仿真时间 comm_drop_rate: 0.1 # 通信丢包率，0 表示理想信道 comm_delay_step: 2 # 通信延迟步数，2 步约等于 0.2 秒 agents: police: 4 # 警车：清障、标记被埋平民 fire: 8 # 消防车：灭火 ambulance: 6 # 救护车：转运平民 center: 1 # 中心决策节点 score: building_weight: 0.6 # 建筑存活在总分中的权重 civilian_weight: 0.4 # 平民存活在总分中的权重

comm_drop_rate和comm_delay_step这两项决定了这一局到底是练协同还是练单机。刚开始跑通的时候把两个都设成 0，先确认基本链路是通的；等 agent 能稳定拿分了，再把丢包和延迟加回去，你会看到分数明显掉一截——掉的那部分就是协同本身的价值，也是这套仿真想考察的东西。

score的权重一般在评分脚本里，配置里只是覆盖项。改权重会改变策略的取向：把建筑权重调高，agent 会变得保守，宁可先灭火也不冒险进废墟救人；调高平民权重，行为会激进很多。做对比实验时一定要固定权重，只改一个变量，否则跑出来的差异说不清是谁造成的。

4.3 启动与观察：怎么判断这一局是跑对了还是跑歪了

标准 launch 长这样，把它存成round.launch放在rsl_tools/launch下最省事：

<launch> <!-- 全局仿真时间，必须开，否则时间戳全乱 --> <param name="/use_sim_time" value="true"/> <!-- 内核：加载地图和全局配置 --> <node pkg="rsl_core" type="core_node" name="core" output="screen"> <param name="map" value="city_01"/> <rosparam file="$(find rsl_core)/config/sim.yaml" command="load"/> </node> <!-- agent：一个团队，多实例靠 team_id 区分 --> <node pkg="rsl_agent" type="agent_node" name="fire_team" args="--type fire --count 8" output="screen"/> <node pkg="rsl_agent" type="agent_node" name="police_team" args="--type police --count 4" output="screen"/> <node pkg="rsl_agent" type="agent_node" name="ambulance_team" args="--type ambulance --count 6" output="screen"/> <!-- 录制，出问题靠它复盘 --> <node pkg="rosbag" type="record" name="recorder" args="-O $(env HOME)/round.bag /world/graph /cmd"/> </launch>

roslaunch rsl_tools round.launch # 另一个终端看评分，评分节点在整局结束时输出 rostopic echo /score

跑起来之后该盯什么，我按优先级排一下。第一看/world/graph的频率，正常应该是每步一条，如果变成好几步一条或者干脆没有，问题在内核或者时钟；第二看 agent 有没有在动，rviz 里三类实体的颜色区分开，全挤在一处说明目标分配退化成了一窝蜂；第三才是看分数。分数低但过程正常，那是策略问题，改policy.py就行；过程就不正常，改策略纯属浪费时间。

/score输出里我最关心两件事：建筑毁坏面积占比和平民死亡率。这两项在早期局里通常一个高一个低，同时都低的局才说明协同真的起来了。

5. 避坑与排查：让分数归零的几类低级错误

跑通之后到能拿分之间，有一段全是坑的路。下面这几条是我踩过、也在别人的代码里见过很多次的，每条都按「现象 → 原因 → 解决」写，照着对号入座能省掉大半天。

现象：编译全绿，rviz 里除了网格什么都没有。原因通常是use_sim_time没开或者开晚了——rviz 用墙钟时间做显示，而消息的时间戳来自仿真时钟，两者相差几十秒，所有实体都落在「未来」被丢弃。解决：把use_sim_time的 param 写在 launch 最前面，保证内核起来之前就生效；已经开着的检查rosparam get /use_sim_time是不是true，不是就手动设上并重启 rviz。

现象：agent 起来了但立刻退出，日志里只有一行「registration failed」。这是team_id冲突或者内核还没准备好。原因大多是所有 agent 用了同一个默认 id，或者 agent 比内核先起、注册时连不上。解决：给每个实例显式传team_id，或者在 agent 侧加一个重试注册的循环，连不上就等 200 毫秒再试，最多试 20 次。

现象：一局跑完分数是 0，但 agent 明明动过。这个最坑。原因多半是决策周期和仿真步长不匹配——agent 每 1 秒才决策一次，而仿真 0.1 秒一步，等指令下发时expire_step早过了，内核判定为无效指令全部丢弃。解决：把决策周期压到单步以内，或者在消息里按「预计执行步数」动态算expire_step，别写死成某个常数。

现象：第二局启动就崩，日志显示状态异常。典型原因是全局静态变量没重置。第一局结束时某些缓存、计数器留在全局区，第二局一进来就是脏数据。解决：给所有带状态的对象写reset()，在仿真开始的消息回调里统一调一次；实在找不到是哪个，用valgrind或者干脆把内核做成独立进程、每局重启，简单粗暴但有效。

现象：两台 agent 抢同一个目标，到地方互相卡住。目标分配没有全局去重，每台 agent 各自算自己的最优目标。解决：要么走中心节点统一分配再下发，要么在共享话题上做一行式的占用声明，后声明的自动退让。前者一致性好，后者实现快，看你要不要为这点一致性付通信代价。

注意：排查顺序永远是「时钟 → 通信 → 数据 → 策略」，反过来调策略，大概率是在给时钟问题打补丁。

还有一个不算 bug 但很浪费时间的坑：改完rsl_msgs之后忘了重新source devel/setup.bash，新开的终端用的还是老消息定义，序列化直接对不上，报的错跟消息毫无关系。改过消息定义之后，一律重编、重开终端。

6. 二次开发：换掉决策模块而不动内核

真正要上手改的时候，最大的诱惑是把整套代码重构一遍。我的建议是反着来——先定死哪些不改，剩下的空间反而清晰。内核、消息定义、通信时序这三块冻住，只在决策层做文章，一两天就能跑出对比结果；反过来先动内核，两周之后你可能还在调编译。

接口抽象是这件事的前提。我会把策略做成一个极薄的类，只有reset和act两个方法，任何策略实现都塞进这个壳里：

# strategies/base.py —— 所有策略都实现这两个方法 class Policy: def reset(self, config): """每局开始时调用，清空状态，config 是本局参数""" raise NotImplementedError def act(self, world, self_state, step): """每步调用一次，返回动作列表；返回空列表表示待命""" raise NotImplementedError

这么做的直接好处是 A/B 对比变得很便宜：同一个 launch，换个参数就能切策略，同一张地图、同一个随机种子跑十局，把/score汇总一下就是一张表。

验证方法上，别只看单局分数。我的做法是固定地图和随机种子，跑 10 局取中位数，同时记录「平均救援数」和「平均响应步数」。只看总分容易被偶然的高分局骗过去——某一局运气好，火点集中在起点附近，分数虚高，换个种子立刻打回原形。响应步数能反映出策略是不是在前期犹豫太久，这个指标比总分敏感得多。

还有一个具体技巧：把每步的目标分配结果打进 bag。事后用rosbag filter捞出来画成图，你会很直观地看到 agent 在哪些步上换了目标、换得有多频繁。我最早那版策略在图上是一团乱麻，每步都在换目标，改成带锁定窗口之后就变成一条条清楚的直线，分数直接涨了一截。这种观察方式比盯着总分猜原因有效得多。

顺带提一句性能：策略层用 Python 写原型没问题，但决策频率一旦压到每步一次，Python 的解释开销就开始显出来了，一局跑下来墙钟时间能翻倍。真要高频，把热路径下沉成 C++，接口不变，Python 版留着做对照。

从那以后我每次动策略之前，都强制先跑一遍基线、存下 bag、记下中位数分数，再动第一行代码。没有基线的优化全是玄学，跑出好结果也说不清是改动起效还是这局地图恰好顺手。把包拉下来，先跑通一局再回头读代码，比反过来快得多。希望帮到你。

本文还有配套的精品资源，点击获取