如果你刚把 ego_planner 的仓库克隆到本地按 README 装完依赖满怀期待地运行仿真 launch结果 Rviz 窗口里除了地面网格之外空无一物——恭喜你你遇到了几乎所有 ego_planner 新手都会遇到的第一道坎。这个坎十有八九不在规划算法本身而是出在深度相机、点云和 odom 话题这三样“感知输入”的配置上。为什么会这样因为 ego_planner 是一个依赖实时环境感知的局部规划器它本质上是在不断回答“我周围有没有障碍、我从哪来、我要去哪”。在仿真里这三个问题分别由深度相机或激光雷达、点云话题和里程计话题来回答。你哪怕把规划算法本身搞得再明白只要数据链路没搭通Rviz 里就不会有一点反应。我整理这篇文章的动机很简单把我在复现 ego_planner 仿真时踩过的、以及帮别人排查时的那些“看起来像算法 bug、实际上是配置问题”的情况集中讲清楚。内容包括 Gazebo 里深度相机的挂载方式、点云消息字段的含义与 Rviz 显示排障、odom 话题的来源和 TF 树检查方法最后给出一套可以直接对照修改的 URDF/launch 配置和验证流程。适合刚接触 ego_planner、准备在仿真环境里验证算法效果或者在自建 Gazebo 场景中接入 ego_planner 的读者。1. 为什么仿真一启动就“黑屏”传感器数据链路比规划算法更早决定成败很多人的第一反应是去翻规划器代码盯着控制台日志看有没有报错然后反复重启 launch 文件。但在我遇到过的绝大多数 ego_planner 仿真问题里规划器本身根本没做错过什么它只是安静地坐在那里等数据而数据一直没有来。1.1 ego_planner运行前必须到位的三个数据输入我这里以常见的 ego_planner 仿真需求为例无人机在 Gazebo 环境里飞行ego_planner 节点订阅里程计、点云和 map/goal 相关话题输出规划轨迹。你可以通过rostopic list看到实际发布的话题但不管怎么命名信息流大致是这三部分里程计信息通常叫/odom消息类型nav_msgs/Odometry。它告诉规划器“当前状态是什么”包括位置、姿态、速度。环境感知信息一类是全局/局部障碍物点云常见话题名/map、/cloud、/camera/depth/points消息类型sensor_msgs/PointCloud2。ego_planner 用点云构造 ESDF 地图通知规划器哪里有障碍物。目标信息通常是/goal、/move_base_simple/goal等告诉规划器“你想去哪”。我见过不少新手的误区是以为只要启动 ego_planner 的 planning 节点它就会自动在仿真里“看到”障碍物。实际上它只是一个“消费者”如果上游不发布点云和 odom它连第一步都走不出去。有个很直接的验证习惯先不要打开 Rviz直接跑rostopic hz /odom和rostopic hz /camera/depth/points。如果这两个话题没有任何输出那规划器必然什么都不做。这个习惯养成之后能省下太多无头绪的 debug 时间。1.2 新手最容易误判的三种“程序没起来”假象我总结了几种特别容易让人误判的情况基本上你迟早会遇到其中一种。第一种Rviz 里点云完全消失。控制台没有报错脚本看起来也都在跑。我去检查时发现Gazebo 侧深度相机插件根本没被加载或者插件加载了但话题名不对。这种情况下你盯着规划器代码看一天也没用问题在传感器侧。第二种odom 话题有数据但 TF 树是断的。ego_planner 拿到 odom 后还要依赖 TF 把点云从相机坐标系变换到世界坐标系。如果 TF 缺少odom - base_link这条边Rviz 里就会出现红色错误提示点云同样不显示规划器同样不工作。这种问题经常被误读成“点云没发出来”。第三种点云和 odom 都正常但按下目标点后没有任何路径输出。这个时候才轮到怀疑规划器参数但据我观察九成情况是目标点坐标的 frame_id 和 Rviz 的固定坐标系不一致或者目标点被发布到了规划器没监听的话题上。记住一个原则遇到黑屏先查输入再查输出最后才查规划参数。这个排查顺序能帮你绕开大量无效劳动。2. 深度相机在Gazebo里的选型与挂载决定点云能不能出来的第一道关卡既然点云是 ego_planner 最核心的输入那我们先解决仿真环境里“深度相机怎么才能真正产生点云”的问题。这一步最直接也最坑因为 Gazebo 里有好几种相机方案行为差异还挺大。2.1 两种常用深度相机插件的行为差异我在不同项目里用过两类深度相机模拟方式第一类是用libgazebo_ros_depth_camera.so第二类是用较老的libgazebo_ros_openni_kinect.so。两者都能产生深度图和点云但在话题命名、ROS 版本兼容性和维护状态上有差别。我把它们的典型特性整理成了下表方便你对照自己的环境对比项gazebo_ros_depth_cameragazebo_ros_openni_kinectROS 兼容性Noetic/Melodic 可用老项目常见发布内容深度图、点云、相机 info深度图、rgb 图、点云点云话题/camera/depth/points/camera/depth/points插件文件名libgazebo_ros_depth_camera.solibgazebo_ros_openni_kinect.so维护活跃度较新长期维护偏旧但网上资料多如果你是新项目我强烈建议直接用gazebo_ros_depth_camera不要因为看到别人博客里总在贴 openni_kinect 的例子就跟着用旧的。旧插件在 ROS Noetic 下的兼容性已经不太行了尤其是点云话题的坐标系和 frame_id 处理容易出一些不明不白的问题。2.2 相机参数里三个决定“有没有点云”的开关我发现很多人写好 URDF 和 Gazebo 插件后点云不出来排查到最后发现是参数问题。这里有几个参数是真正的“开关”。第一个是话题名。插件里会有图片话题配置、深度话题配置和点云话题配置。如果点云话题名没写对或者被 launch 文件重映射掉了你rostopic list里就可能找不到它。我建议在插件里明确指定point_cloud_topic_name不要依赖默认值。第二个是裁剪距离。Gazebo 深度相机的clip里会有 near 和 far这个范围如果太小障碍物稍微远一点就测不到如果太大点云里会出现大量远处的点影响计算性能。仿真里我一般设成near0.2、far6.0既能覆盖近距离避障场景又不至于把远处没用的点都纳进来。第三个是frame_name。这个参数会直接写进点云消息的header.frame_id里比如camera_link或camera_depth_optical_frame。如果这里填的坐标系在 TF 树里不存在或者和 Rviz 的 Fixed Frame 对不上点云就会在 Rviz 里失踪。还有一个经常被忽略的是更新频率。深度相机插件的update_rate如果太低比如 1Hz你会感觉点云“卡顿”规划器的反应也会明显迟滞。我通常给 15~30Hz这个频率对仿真规划足够了。2.3 只有深度图没有点云时的补救方案还有一种常见情况深度相机插件正常发布了深度图/camera/depth/image_raw但始终没有点云话题。有时候是因为你用的是纯深度相机图像插件而不是深度相机点云插件有时候是插件版本把点云发布功能拆开了。这时候不需要重新换模型直接在 ROS 层加一个depth_image_proc节点就能把深度图转换成点云。典型的启动配置是把下面几个节点连起来rosrun depth_image_proc depth_to_pointcloud \ camera:/camera \ depth:/camera/depth/image_raw注意depth_image_proc只是补数据链路的手段能用但多了一个中间环节会增加时间戳和坐标系对齐的复杂度。我的建议是能用相机插件直接出点云就用直出方案实在不行再走深度图转点云。3. 点云话题的字段、坐标系和Rviz显示排障到这里大部分人的点云话题已经能发布了。但“发布出来”和“在 Rviz 里正常显示并喂给规划器”之间还隔着几个细节。我见过不少人的点云话题频率正常但 Rviz 里就是看不到原因千奇百怪但大部分都集中在下面这几类。3.1 看懂PointCloud2消息里的几个必要字段如果你用rostopic echo /camera/depth/points -n1看过点云消息会被一长串嵌套字段吓到。其实不需要全懂但下面几个字段必须会看header.frame_id点云是在哪个坐标系下表达的。Rviz 显示点云前会把这个 frame_id 通过 TF 变换到 Fixed Frame所以这个字段必须合理。header.stamp时间戳。如果时间戳和 TF 树里的时间戳差太远TF 变换会失败或者被忽略。fields点云里每个点包含哪些通道常见的有x、y、z、rgb。point_step和row_step表示每个点和每行数据占多少字节。如果这两个值是零说明点云数据格式有问题。is_dense是否所有点都有效。如果某个点是 NaN设置成 dense 反而会导致 Rviz 渲染困难。理解这些字段的意义是排查的基础。比如你看到一个点云消息里header.frame_id是空字符串那不用继续往下查一定是传感器侧配置漏了 frame_name。3.2 Rviz里看不到点云的常见原因清单我把 Rviz 里点云不显示的常见原因整理成一个排障表按这个顺序查基本都能定位症状可能原因解决办法没有任何点云话题没发布rostopic list确认话题名提示 “No transform from … to …”点云的 frame_id 在 TF 树中不存在检查并补齐 TF重点是camera_link提示 ”Fixed Frame … does not exist”Rviz 的 Global Fixed Frame 填错改成存在的坐标系比如odom或map点云存在但非常密集地挤在一处near/far 设置太近或点云坐标尺度不对调整clip参数检查点云坐标单位点云断断续续、一闪一闪时间戳不同步或 TF 抖动同步时间戳检查 TF 是否稳定发布图像正常但点云缺失点云转换节点没启动补启动 depth_image_proc 节点3.3 一个典型的相机外参旋转错误案例有一次我给一台仿真相机挂点云点云话题正常TF 树听起来也完全但 Rviz 里点云整体躺在地面上像是障碍物被“压扁”了。最后发现问题出在相机 link 的 joint 外参上——我把rpy里的旋转写错了 90 度导致相机光轴和实际朝向不一致点云被旋转变换得面目全非。这种问题在 Rviz 里看起来很像“点云数据错了”实际上只是外参矩阵错了。修复方式很简单调整 URDF 中相机 joint 的rpy让相机的光轴朝向模型的正前方。排查时可以在 Rviz 里把点云话题的 Fixed Frame 临时设成camera_link如果看到点云正常就说明相机本身的坐标系正确问题一定出在 base_link 到 camera_link 的外参上。4. odom话题的来源选择与TF树对不上的解决办法点云解决之后下一步最常见的坑就在 odom 话题上。很多 ego_planner 仿真环境里无人机的状态估计来自 odom 话题。odom 不出数据或者 TF 不完整规划器同样什么都做不了。4.1 odom消息的三种常见发布方式Gazebo 仿真里的 odom 来源通常有三种选择哪一种取决于你手头项目的成熟度。第一种是用 Gazebo 自带的插件直接发布里程计。比如libgazebo_ros_p3d.so可以发布 3D 位姿再配合robot_localization或简单封装就能生成/odom。这种方式最直接适合刚起步调试 ego_planner 的仿真。第二种是跑 SLAM 或状态估计节点。比如用gmapping、cartographer、fastlio或robot_localization融合 IMU、轮式里程计和视觉里程计。这种方式更接近真实系统但也会引入更多需要标定的东西新手很容易迷失在配置里。第三种是 ego_planner 自带的仿真器节点自带 odom。很多 demo 为了降低使用门槛直接在 simulator 节点内部生成一个虚拟无人机运动模型同时发布/odom和 TF。这种情况下你不需要自己搭 Gazebo 模型但你也失去了“让算法跑在真实传感器场景中”的意义。我自己的建议是如果你只是想验证规划算法用第三种最快如果你想在自建仿真场景里完整跑通一套无人机系统用第一种起步后续再切第二种。4.2 里程计消息里的隐性要求频率、时间戳与协方差很多人以为发一条nav_msgs/Odometry就完事了其实规划器对 odom 的频率和内容是很敏感的。频率方面规划器需要“足够近”的状态输入。仿真里 30Hz 左右基本能工作但我更推荐 50~100Hz。频率太低会让规划器在两次状态之间产生明显的位置跳变严重时会导致轨迹抖动或碰撞漏检。时间戳方面odom 消息的header.stamp如果长时间不更新或者和 TF 的时间差太大系统里的坐标变换会报错。一个容易犯的小错误是你自己写发布节点时忘了更新 stamp结果tf2_echo查不到最新变换。协方差方面很多主题节点的 odom 协方差明明设成全 0 也能跑但某些下游算法或滤波器会直接用协方差做权重计算全 0 就会导致除零或权重异常。建议至少把位置方差和对角线值设成一个小正数比如0.01这样可以规避不少隐性问题。4.3 用tf2_monitor快速定位缺哪条TF边TF 问题是最常见的“看起来跟 odom 有关、但实际是坐标变换断了”的问题。我在调试时几乎离不开tf2_monitor。先启动你的仿真然后rosrun tf2_ros tf2_monitor这个命令会打印出当前系统的 TF 树结构。正常情况下你应该看到至少map/odom - base_link - base_footprint - camera_link这样的链路。如果有人跟你说“odom 话题有数据但 Rviz 一直报错”我第一个动作就是运行这个命令。它能很清楚地告诉你谁在发布什么变换哪个坐标系等不到。比如输出里发现camera_link根本没有被任何人发布那问题就很清楚了要么相机 link 在 URDF 里没写好要么 robot_state_publisher 没跑起来。5. 一套可以直接对照的传感器配置与全链路验证流程前面讲了不少原理和排错思路这一章给出一套可以照着改的配置示例以及完整的验证流程。这样你可以先把东西跑起来再慢慢理解细节。5.1 可直接参考的相机URDF/Xacro与Gazebo插件片段以 ROS Noetic 和 Gazebo 11 为例下面这段 Xacro 片段定义了一个挂在base_link前方的深度相机并把点云输出到/camera/depth/points。!-- 相机link -- link namecamera_link visual origin xyz0 0 0 rpy0 0 0/ geometry box size0.05 0.03 0.03/ /geometry /visual inertial mass value0.02/ origin xyz0 0 0 rpy0 0 0/ inertia ixx0.00001 ixy0 ixz0 iyy0.00001 iyz0 izz0.00001/ /inertial /link !-- 相机与机体的连接 -- joint namecamera_joint typefixed parent linkbase_link/ child linkcamera_link/ origin xyz0.15 0 0.1 rpy0 0 0/ /joint !-- Gazebo深度相机插件 -- gazebo referencecamera_link sensor typedepth namecamera update_rate20/update_rate camera pose0 0 0 0 0 0/pose horizontal_fov1.0472/horizontal_fov image width640/width height480/height formatR8G8B8/format /image clip near0.2/near far6.0/far /clip /camera plugin namecamera_plugin filenamelibgazebo_ros_depth_camera.so camera_namecamera/camera_name frame_namecamera_link/frame_name image_topic_name/camera/image_raw/image_topic_name depth_topic_name/camera/depth/image_raw/depth_topic_name point_cloud_topic_name/camera/depth/points/point_cloud_topic_name /plugin /sensor /gazebo注意不同版本的gazebo_ros_depth_camera插件参数写法可能略有差异。如果发现rostopic list里没有任何相机话题先检查插件的参数名是否匹配你当前 Gazebo 插件版本的文档。另外ego_planner 规划节点监听的话题名通常是可配置的。强烈建议你拿到一个 ego_planner 项目后先打开它的 launch 文件和 yaml 配置文件确认odom_topic、pointcloud_topic或map_topic到底叫什么再决定是否需要在 launch 文件里加remap from/camera/depth/points to/local_pointcloud/这样的重映射。5.2 从启动到出轨迹的五个验证步骤我建议你每次启动 ego_planner 仿真后都按下面这五个步骤过一遍。看起来繁琐但真的节省时间。第一步检查节点和话题。启动完 launch 后立刻执行rosnode list确认 Gazebo、深度相机插件、robot_state_publisher、ego_planner planning 节点都活着然后执行rostopic list确认 odom、点云、goal 相关话题存在。第二步验证数据频率。rostopic hz /odom和rostopic hz /camera/depth/points两者都应该有稳定输出。如果其中一个是 0别往下看 Rviz先解决这个。第三步验证 TF 树。rosrun tf2_ros tf2_monitor或直接看 Rviz 左侧的 TF 显示确认odom - base_link - camera_link链路完整。第四步验证点云显示。在 Rviz 里添加 PointCloud2 显示把 Fixed Frame 设为odom选择点云话题为/camera/depth/points。这时应该能看到环境中的障碍物点云。第五步发布目标点并观察轨迹。在 Rviz 里用 2D Nav Goal 按钮或直接rostopic pub /goal发布一个目标点观察 ego_planner 是否开始生成规划路径并输出速度指令。如果到这里才卡住那才是真正需要看规划器参数的地方。5.3 跑通之后给点云和里程计做点减负当你终于看到规划器在仿真里稳定绕障不要急着就说完事。我建议你对点云和里程计做一轮“减负”与“加固”这样后续移植到真实环境时能少踩很多坑。点云方面如果相机分辨率高、场景复杂点云密度可能极大直接喂给 ego_planner 会造成不小计算压力。很多人在仿真里没感觉是因为场景简单一旦换成复杂城市或室内模型立刻卡得离谱。可以在点云进入规划器之前加一个 VoxelGrid 滤波节点把体素大小设为0.1m或0.2m既保留环境结构又大幅降低点数量。里程计方面如果发现仿真飞行时无人机状态有明显的抖动建议把原始 Gazebo odom 接到robot_localization的ekf_localization_node里做滤波同时融合 IMU 数据。滤波后的 odom 会更平滑规划器的轨迹也会更稳定。这一步在真实平台上几乎是必须的仿真里先做起来后面移植就轻松很多。根据我个人经验ego_planner 仿真调试里最耗时的一段往往不是你理解算法的时间而是反复折腾“为什么数据没有按预期流动”的时间。深度相机该挂在哪、点云话题从哪来、odom 的 TF 怎么补齐、Rviz 里怎么验证这些都是看起来很琐碎的配置问题但它们直接决定了你能否顺利看到一条平滑的规划轨迹。先把这条数据链路跑顺再去调规划参数你会觉得 ego_planner 突然“听话”了很多。如果你自己也被某个奇怪的仿真问题卡住不妨用这篇文章里的排查顺序从头捋一遍大概率能找到答案。
