简介面向参加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. 批量起剩下的 agentlaunch 里通常写成多实例 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 nameuse_sim_time valuetrue/就不要在命令行再设一遍两处不一致时后者可能覆盖前者表现是时好时坏。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_guardstd::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::vectorEntity byType(uint8_t type) const { std::lock_guardstd::mutex lk(mtx_); std::vectorEntity out; for (const auto e : entities_) if (e.type type) out.push_back(e); return out; } private: mutable std::mutex mtx_; std::vectorEntity 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, speed800.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(reverseTrue) # 收益高的先配 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 resulturgency里那个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_TYPERelease -j$(nproc) source devel/setup.bashCMAKE_BUILD_TYPERelease不能省。内核侧全是浮点运算和几何判定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 valuetrue/ !-- 内核加载地图和全局配置 -- node pkgrsl_core typecore_node namecore outputscreen param namemap valuecity_01/ rosparam file$(find rsl_core)/config/sim.yaml commandload/ /node !-- agent一个团队多实例靠 team_id 区分 -- node pkgrsl_agent typeagent_node namefire_team args--type fire --count 8 outputscreen/ node pkgrsl_agent typeagent_node namepolice_team args--type police --count 4 outputscreen/ node pkgrsl_agent typeagent_node nameambulance_team args--type ambulance --count 6 outputscreen/ !-- 录制出问题靠它复盘 -- node pkgrosbag typerecord namerecorder args-O $(env HOME)/round.bag /world/graph /cmd/ /launchroslaunch 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汇总一下就是一张表。改造位置典型改动风险建议策略层policy.py换目标分配、加启发式低首选改完直接重跑感知层perception.h加历史轨迹、视野过滤中改完先验证实体数量对不对通信层改消息裁剪、加压缩中高要同步改内核否则丢包率失真内核改物理、改评分高除非做研究日常不要碰验证方法上别只看单局分数。我的做法是固定地图和随机种子跑 10 局取中位数同时记录「平均救援数」和「平均响应步数」。只看总分容易被偶然的高分局骗过去——某一局运气好火点集中在起点附近分数虚高换个种子立刻打回原形。响应步数能反映出策略是不是在前期犹豫太久这个指标比总分敏感得多。还有一个具体技巧把每步的目标分配结果打进 bag。事后用rosbag filter捞出来画成图你会很直观地看到 agent 在哪些步上换了目标、换得有多频繁。我最早那版策略在图上是一团乱麻每步都在换目标改成带锁定窗口之后就变成一条条清楚的直线分数直接涨了一截。这种观察方式比盯着总分猜原因有效得多。顺带提一句性能策略层用 Python 写原型没问题但决策频率一旦压到每步一次Python 的解释开销就开始显出来了一局跑下来墙钟时间能翻倍。真要高频把热路径下沉成 C接口不变Python 版留着做对照。从那以后我每次动策略之前都强制先跑一遍基线、存下 bag、记下中位数分数再动第一行代码。没有基线的优化全是玄学跑出好结果也说不清是改动起效还是这局地图恰好顺手。把包拉下来先跑通一局再回头读代码比反过来快得多。希望帮到你。本文还有配套的精品资源点击获取
