第一次跑 ego_planner 仿真的人十有八九不是被算法参数劝退而是被 Gazebo 里的一堆话题配置逼疯。launch 文件跑起来终端里刷的全是红色报错——PointCloud2 topic does not exist、Could not get transform、Wait for odom timeout…… 这些问题看着吓人其实根子上都是三件事没捋清楚深度相机、点云、odom。这篇东西把我自己踩过的坑整理一下适合刚接触 ego_planner、打算在 Gazebo 里做三维避障仿真的人参考。你不需要一开始就懂规划器内部的优化公式先把传感器数据链路打通仿真就能跑起来了。1. 先理清 ego_planner 仿真到底要吃哪几路数据1.1 三个话题缺一个规划器就只能空转很多新手拿到 ego_planner 的源码第一反应是翻算法参数把最大速度、避障半径、安全距离这些调一遍。实际上一开始根本轮不到调这些因为规划器不是凭空生成轨迹的机器它得先知道自己在哪里、周围有什么、目标在哪。第一路是点云。ego_planner 的核心思路是在局部地图上构建 ESDF 或者占据栅格地图然后让路径避开障碍物。这个地图从哪来就是从实时点云灌进去的。没有点云规划器等于闭着眼睛在空房间里走路rviz 里看不到任何可飞行的地图痕迹后面所有功能都无从谈起。第二路是 odom。它告诉规划器当前机器人本体的位姿和速度。ego_planner 的局部规划器需要知道自己从哪个坐标开始膨胀轨迹没有 odom整个系统的状态估计就是空的。第三路是 TF。这一路最容易被忽视。点云的话题名对得上、odom 话题也在发布但 TF 树不完整照样报 Could not get transform。TF 的作用是把所有传感器数据放到同一个坐标系里去看点云的 frame 是 camera_link规划器的运算空间是 odom 或者 map中间缺任何一个变换数据就会被丢弃或者说坐标系不存在。1.2 先看数据流再谈调参我见过不少人卡在仿真环节好几天情绪一上来就开始怀疑算法是不是不适合自己的场景到处改参数结果问题根本不在算法层。比如点云没进来规划器自然看不到障碍物你调再大的避障半径也没用因为人家压根不知道障碍物在哪。所以调试 ego_planner 的第一个习惯应该是用 rostopic list、rostopic echo 和 rviz 把每一路数据的发布状态确认一遍而不是直接改 launch 和 yaml。数据链路通了后面调参才有意义。这也是为什么我把这篇文章的重点放在数据流梳理而不是规划器原理上。仿真环境和真实机器人不一样真实环境里你还要操心传感器噪声、标定误差、时间同步但在 Gazebo 里只要配置对了数据就是干净的问题通常出在人为设置上。2. 深度相机模型配置仿真里让机器人“看得见”的最小方案2.1 在 URDF 里加一个带点云输出的深度相机ego_planner 做的是三维空间避障所以它需要的是深度相机输出的三维点云不是单线雷达那种二维扫描线。我建议在机器人模型的 URDF 文件里直接挂一个 depth 类型传感器配合 gazebo_ros_depth_camera 插件就能把点云话题发出来。下面是我常用的一套精简配置放在 URDF 的 link 定义之后gazebo referencecamera_link sensor typedepth namecamera pose0 0 0.1 0 0 0/pose update_rate20/update_rate camera namecamera horizontal_fov1.57/horizontal_fov image width640/width height480/height formatR8G8B8/format /image clip near0.1/near far20/far /clip /camera plugin namecamera_plugin filenamelibgazebo_ros_depth_camera.so ros remappingdepth_image_topic:camera/depth/image_raw/remapping remappingcamera_info_topic:camera/depth/camera_info/remapping remappingpointcloud_topic:camera/depth/points/remapping /ros pointcloudtrue/pointcloud /plugin /sensor /gazebo这段配置里referencecamera_link指的是这个传感器挂在哪个 link 上必须在 URDF 里先定义好名为 camera_link 的 link。如果你定义的是camera_link但模型里实际叫front_camera那 Gazebo 启动时会报找不到 reference frame传感器直接不加载。pointcloudtrue/pointcloud这个必须开否则深度相机只发布深度图和相机信息不发布点云。早期我吃过这个亏sensor 明明在跑rostopic list 里就是见不到 points 话题后来才发现是这个开关默认没开。注意一下不同 ROS 版本里深度相机插件的名字。ROS1 的 Melodic/Noetic 下一般叫 libgazebo_ros_depth_camera.so如果你的 Gazebo 版本比较新可能需要换成 libgazebo_ros_depth_camera.so 的替代写法。启动的时候如果提示找不到插件先检查 gazebo_ros 包对应的插件命名别一头扎进 URDF 里改参数。2.2 深度相机关键参数怎么选直接给一张速查表都是我试过相对稳的取值参数推荐取值说明分辨率640x480 起步性能不够降到 320x240分辨率越高点云越密但带宽和 CPU 压力也会线性上涨horizontal_fov1.57 rad约 90°太窄会看不到侧面障碍物三维规划建议 90° 以上clip.near0.1 m太大会把近距离障碍物裁掉clip.far10 m 以上太近会导致远处障碍物消失规划器会误判前方空旷update_rate20 Hz 左右低于 10 Hz 时动态障碍物刷新会很迟钝这里要特别说下分辨率和 update_rate 的组合问题。Gazebo 仿真里深度相机生成点云是 CPU 密集操作如果你把分辨率拉到 1280x720、update_rate 拉到 60Hz机器扛不住是小事关键是 ego_planner 接收点云的频率也会受限于这个话题的发布频率高分辨率只会带来巨大的 TF 和时间同步压力规划效果并不会显著提升。我在仿真中一般用 640x480 20Hz如果环境较大且障碍物稀疏320x240 15Hz 完全能跑。还有一点容易被忽略USB 摄像头那种单目深度相机在 Gazebo 里其实不用太纠结内参因为深度数据是仿真生成的不会有真实相机的标定误差。所以不推荐为了更真实去给深度图像加噪声调试阶段先把 noise 相关参数全部设 0等系统稳定了再考虑模拟真实噪声。2.3 为什么别拿单线雷达点云硬顶有人觉得 ego_planner 反正订阅的是 PointCloud2 消息那我把单线激光雷达的话题直接重映射给它是不是也行这个话题在群里讨论过很多次我的结论是能跑但不推荐。单线雷达只有水平一圈扫描线无法感知竖直方向上的障碍物。你在室内仿真里放一张桌子单线雷达只能扫到桌腿桌面在雷达看来是空的。ego_planner 会在三维空间里规划路径它完全可能从桌面上方飞过去并且认为那是无障碍通道——如果仿真场景里桌面上方其实有吊灯或者其他障碍物就会出事。深度相机的点云是覆盖一个视锥体的至少在相机视野范围内垂直方向的信息是齐全的。如果你的场景比较简单比如纯二维平面避障单线雷达点云凑合能用但只要涉及无人机在三维环境里的上下避障我建议老老实实配深度相机别在这方面省事。3. odom 话题与 TF 树点云配好了规划器还是不动问题多半在这3.1 仿真里 odom 的三种来源ego_planner 对 odom 的要求是稳定、持续、坐标系明确。在 Gazebo 仿真里odom 一般有三种来源。第一种是直接用真值插件。Gazebo 里每个模型的位置和姿态都是仿真器知道的用插件把这些真值读取并发布成 odom 话题就是最简单的做法。这也是我最推荐新手在仿真阶段使用的方式因为它不涉及状态估计误差能让你先把规划算法跑通。第二种是自己写轮式里程计或者 IMU 融合。适用于移动底盘场景比如轮式机器人通过轮子转速累计位移输出 odom。但 ego_planner 多数用在旋翼无人机上这个方法不见得合适。第三种是通过视觉或者激光匹配估计位姿。比如用 laser 点云跑 ICP 或者用相机跑视觉里程计。这个方案最接近真实部署但仿真阶段引入它会增加大量调试成本而且传感器噪声没调好的话位姿漂移会把规划器带偏新手很难判断到底是规划器的问题还是状态估计的问题。3.2 用 P3D 插件快速生成 odom仿真阶段我用得最多的就是 gazebo_ros_p3d 插件几行配置就能把模型真值发布成 odom 和 TF。一个最小配置如下gazebo plugin nameodom_plugin filenamelibgazebo_ros_p3d.so ros remappingodometry_topic:odom/remapping /ros body_namebase_link/body_name frame_nameodom/frame_name update_rate50/update_rate /plugin /gazebo注意 body_name 要和机器人模型里的主体 link 名字一致frame_name 是 odom 坐标系的名字也就是里程计的参考系。P3D 这个插件会同时发布两个东西odom 话题和 odom 到 body_name 的 TF 变换。所以只要这个插件加载成功rosrun tf view_frames 就能看到 odom - base_link 这一段。补充一个使用心得P3D 发布的是真值意味着没有累积误差、没有噪声。这和真实环境不一样所以当你把 ego_planner 在仿真里调通后不要理所当然地认为移植到真机上也能直接飞。真机上的 odom 一定会有漂移和时间延迟那才是下一个大坑。但这不是这篇文章的重点仿真的首要任务是把数据链路打通。3.3 TF 树这个隐形大坑点云话题在发、odom 话题也在发可 ego_planner 还是说找不到变换这种问题我至少见过十次以上。原因多数出在 TF 树不完整。ego_planner 从点云话题里拿到的数据frame_id 是 camera_link。它要在 map 或者 odom 坐标系下做规划所以需要一条从 map/odom 到 base_link 再到 camera_link 的完整 TF 链。任何一个环节缺失tf2 就会直接拒绝使用这个点云。完整的 TF 树应该长这样map - odom仿真里一般不会涉及回环和漂移直接用静态变换发布即可odom - base_link由 P3D 或其他里程计节点负责base_link - camera_link在 URDF 里定义关节关系后通过 robot_state_publisher 或 static_transform_publisher 发布新手最容易漏掉的是 map - odom 这一段。如果你在 rviz 里把 fixed frame 设成 map发现点云消失了但切到 odom 就正常显示十有八九就是缺 map - odom。这种情况可以先用命令行顶上rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 map odom这个命令发布的是一个单位静态变换在仿真里足够用了。注意它只是调试手段发布节点退出后变换就消失。3.4 仿真时钟是另一个隐蔽问题Gazebo 仿真里时间有仿真时间和墙钟时间之分。如果 ROS 节点没有使用仿真时间TF 的时间戳就可能比仿真时间超前或落后导致 tf2 报 stale TF 或者 transform from ... to ... is not available。这个坑特别隐蔽因为用rostopic echo 看 odom 话题消息是在更新的但 rviz 里就是显示异常。解决办法很简单在 launch 文件里加上param name/use_sim_time valuetrue/凡是参与仿真数据流的节点都要保证它读取的是 /use_sim_time 参数。如果你的机器人节点里有一些是自己写的记得检查它们是否初始化了 use_sim_time。4. 从零跑通 ego_planner 仿真配置与验证实操4.1 launch 文件需要拉起的节点清单到这里你已经知道需要哪些数据了。实际落地时一个能跑通的 ego_planner 仿真 launch 文件至少需要包含这些节点Gazebo 环境加载包括世界文件、机器人的 URDF 模型robot_state_publisher把 URDF 里的关节状态发布成 TF生成 base_link 下所有子坐标系深度相机插件随机器人模型一起加载发布点云话题和相机相关 TFP3D 里程计插件发布 odom 话题和 odom - base_link TFmap - odom 静态 TF 发布节点ego_planner 的感知和规划节点rviz 可视化一个精简的 launch 片段大致长这样launch !-- 使用仿真时间 -- param name/use_sim_time valuetrue/ !-- 加载机器人模型 -- arg namemodel default$(find your_pkg)/urdf/robot.urdf/ param namerobot_description textfile$(arg model)/ node namerobot_state_publisher pkgrobot_state_publisher typerobot_state_publisher/ !-- 启动 Gazebo 并加载机器人 -- include file$(find gazebo_ros)/launch/empty_world.launch/ node namespawn_model pkggazebo_ros typespawn_model args-urdf -model robot -param robot_description -x 0 -y 0 -z 0.5/ !-- map - odom 静态变换 -- node pkgtf2_ros typestatic_transform_publisher namemap_to_odom args0 0 0 0 0 0 map odom/ !-- 启动 rviz -- node namerviz pkgrviz typerviz args-d $(find your_pkg)/rviz/ego.rviz/ /launch注意 launch 文件里加载机器人模型的顺序。spawn_model 必须在 robot_state_publisher 启动之后否则模型还没描述好Gazebo 那边报错会很奇怪。P3D 插件和深度相机插件是跟着 URDF 进 Gazebo 的不需要额外启动节点。4.2 ego_planner 配置里那几个揪心的话题名ego_planner 的配置文件一般是 yaml里面关于话题和坐标系的设置最容易被忽视。以下是我踩坑后整理出来的配置模板pointCloudTopic: camera/depth/points odometryTopic: odom global_frame: map local_frame: odom先说话题名。这里有个特别容易踩的坑话题名开头不要乱加斜杠。如果你的机器人节点挂在某个 namespace 下比如 /drone点云实际话题名是 /drone/camera/depth/points。你把 pointCloudTopic 写成 /camera/depth/points就是在硬编码全局话题而发布端的 namespace 是 /drone两者根本对不上。正确的做法是用相对话题名 camera/depth/points。这样在 /drone 这个 namespace 下它会自动解析成 /drone/camera/depth/points和发布端匹配。如果你的节点没有 namespace那相对话题和绝对话题效果一样但养成用相对话题的习惯后面移植到多机场景会省很多事。再说坐标系的设置。global_frame 用 map、local_frame 用 odom这是常见且稳妥的做法。local_frame 是局部规划器的运算坐标系odom 天然适合做这个global_frame 是全局坐标系的载体map 在仿真里往往就是世界原点。两者的关系必须有 TF 相连也就是前面反复强调的 map - odom 静态变换。如果你的点云 frame_id 是 camera_linkego_planner 内部做坐标转换时依赖的也是 TF 树。所以再一次强调TF 树必须完整而不是仅仅在 yaml 里改一个 frame 名字就能解决。4.3 rviz 验证检查单三步确认数据链路正常配置完别急着往地图里塞障碍物先用 rviz 把所有数据源可视化一遍确认链路正常。我通常按这个顺序查第一步在 rviz 里添加 PointCloud2 display话题选 camera/depth/pointsfixed frame 先设成 odom。正常情况下你应该看到一片彩色的三维点云并且和机器人模型的位置、姿态是吻合的。如果你看到点云在很遥远的地方或者不停乱跳说明点云的 frame_id 和 TF 树不匹配优先检查 TF。第二步打开 TF display确认存在 map - odom - base_link - camera_link 的完整链路。如果缺了哪一段页面上会有红色或者黄色警告直接点进去看是哪个父坐标系和子坐标系断了。第三步在场景里放一个障碍物比如一个长方体看 rviz 里的点云是否实时出现对应形状。然后在 rviz 里发布一个目标点让 ego_planner 规划轨迹。如果轨迹能顺利绕开障碍物说明整套仿真链路已经通了。检查的时候有一个容易误判的点rviz 的 fixed frame 如果设成 base_link点云看起来会跟着机器人一起动滚动视角时会觉得点云在移动。这不是数据错了是你选的参考坐标系问题。换成 odom 或者 map 再看一切正常。5. 常见问题速查与避坑笔记现象可能原因处理方式rostopic list 没有 camera/depth/points深度相机插件没加载或 pointcloud 开关没开检查 URDF 中 plugin 标签和 gazebo 是否报错点云话题有数据rviz 里显示空白fixed frame 设置不正确或 TF 树不完整先切 fixed frame 到 odom再检查 TF 是否完整TF 报 stalerobot_state_publisher 没启动或 /use_sim_time 没设检查 launch 文件是否启动 robot_state_publisherodom 话题没有数据P3D 插件没加载或 body_name 与模型 link 名不一致查看 Gazebo 启动日志里的插件报错点云偶尔断流相机 update_rate 太低或 queue_size 不够提高 update_rate检查点云发布队列规划器跑起来但轨迹穿障碍点云没有覆盖障碍物或分辨率太低导致地图稀疏提高深度相机分辨率检查视野是否覆盖5.1 点云话题收不到数据怎么排查我遇到点云话题不存在这类问题排查顺序一般是固定的。先用 rosnode info 看相机节点是否健在再用 rostopic list 确认话题是否真的没被创建。如果话题压根没出现问题基本在 URDF 或者插件加载环节进 Gazebo 启动日志找 plugin 相关的报错。如果话题存在但频率为 0再看相机 pose 是不是放到了机器人模型内部导致深度相机被自己身体挡住。最后才考虑 update_rate 和 topic 重映射对不对。5.2 点云断断续续像卡顿一样如果 rostopic hz 看点云话题频率忽高忽低先检查计算机负载和 Gazebo real time factor。Gazebo 里 CPU 跟不上时仿真时间会变慢所有话题频率都会掉。这时先降低模型复杂度、图像分辨率再考虑调低点云发布频率。还有一个常见原因是 urdf 里插件的 publish_tf 和 rviz 的 tf 订阅频率太高导致大量小消息挤占带宽。5.3 ego_planner 规划轨迹频繁抖动仿真里出现规划轨迹抖动多数不是算法参数的问题而是点云本身不稳定。比如深度相机视场边缘有噪点或者地图更新频率太低。调试时先把相机噪声清零再看 update_rate 是否稳定。确认传感器数据没问题后再去动 planner 的 safety margin 和 map 相关参数。我见过很多人一上来就调 planner 的轨迹权重结果越调越乱因为问题的源头根本不在那里。5.4 Gazebo 卡到没法玩Gazebo 仿真是个性能黑洞尤其当你把点云分辨率拉满、同时跑着 rviz 和 ego_planner 的多个节点时CPU 很容易吃满。我的经验是把深度相机分辨率降到 320x240update_rate 降到 15Hz同时把 rviz 中的 PointCloud2 display 的 Point Size 调小一点。如果还卡可以给 Gazebo 开 headless 模式只保留 rviz 做可视化性能能提升一大截。5.5 关于调试顺序的一个建议根据我个人的经验调试 ego_planner 仿真时不要一次配好所有东西再往上跑那样一旦出问题根本不知道是哪个环节坏了。我的习惯是先把 depth camera 点云调通让 rviz 里稳定看到点云再加 odom 和 TF确认 rviz 里模型位置、朝向都正确最后才启动 ego_planner 的规划节点。每一步都验证过再走下一步看起来多花了时间实际是排错最快的方式。6. 最后再补充一个容易被忽略的细节如果你把整套配置跑通后发现 ego_planner 偶尔还是报一些transform timeout之类的警告先不要慌。仿真环境里由于调度问题偶尔丢一两帧 TF 是正常的。关键要看持续时间和频率如果只是偶发一次不影响规划结果如果持续几秒钟就要回到 TF 树和时间同步上排查。另外如果你是在自己写的主控节点里启动 ego_planner记得确认它是基于 share memory 还是 topic 通信。默认话题通信下只要话题名和 frame 对得上问题一般不会太大。我个人在实际调试中最大的体会是底层话题配置这件事看起来很琐碎但它决定了上层算法能不能输出有效结果。把深度相机、点云、odom 这条链路理顺之后后面不管是调 ego_planner 的飞行速度、安全距离还是换成真实环境都会有更清晰的方向。希望这篇能帮你少熬几个夜晚。