1. 从Ego-Planner中剥离plan_env模块的工程实践在机器人导航领域Ego-Planner作为一个优秀的运动规划框架其内部集成的plan_env模块或称gridmap模块提供了强大的环境感知能力。这个模块能够实时融合位姿信息和深度图数据构建用于导航的3D占据栅格地图。但在实际项目中我们常常需要将这个功能独立出来作为一个轻量级的模块与其他SLAM系统如VINS配合使用。我最近就遇到了这样的需求需要将Ego-Planner的plan_env模块解耦出来构建一个独立的ROS功能包。经过几天的摸索和实践终于成功实现了这个目标。下面我就详细分享一下整个过程包括关键步骤、遇到的坑以及解决方案。2. 模块解耦的核心步骤2.1 准备工作与环境搭建首先需要明确的是plan_env模块在Ego-Planner中主要负责处理深度图与位姿信息的融合构建用于导航的占据栅格地图。要将其独立出来我们需要准备以下环境安装ROS推荐Melodic或Noetic版本安装必要的依赖库包括Eigen3、PCL等准备一个测试用的bag包包含位姿和深度图数据我使用的是maxibooksiyi在Gitee上分享的ego_grid_map仓库作为基础。这个仓库已经将gridmap模块单独提取出来大大减少了我们的工作量。克隆仓库后在工作空间下编译即可cd ~/catkin_ws/src git clone https://gitee.com/maxibooksiyi/ego_grid_map.git cd .. catkin_make2.2 代码结构与功能分析解耦后的plan_env模块主要包含以下几个核心部分GridMap类负责地图的初始化、更新和发布深度图处理将输入的深度图转换为3D点云位姿融合根据当前位姿将点云融合到全局地图中地图发布定期发布占据栅格地图供其他模块使用关键函数包括initMap()初始化地图参数和ROS话题depthPoseCallback()处理深度图和位姿数据publishMap()发布构建好的地图2.3 话题适配与参数配置这是最容易出问题的地方。原始代码中订阅的话题名称是硬编码的我们需要根据实际数据源进行调整。在我的项目中位姿数据来自VINS系统发布在/vins_estimator/odometry话题而深度图发布在/camera/depth/image_raw。修改方法有两种直接修改源代码中的话题名称通过ROS参数服务器动态配置我推荐第一种方法因为更直接可靠。在grid_map.cpp中找到以下代码段进行修改// 修改前 std::string odom_topic /odom; std::string depth_topic /camera/depth/image_raw; // 修改后 std::string odom_topic /vins_estimator/odometry; std::string depth_topic /camera/depth/image_raw;3. 与SLAM系统的集成实践3.1 位姿数据格式处理不同的SLAM系统输出的位姿数据格式可能不同。Ego-Planner默认使用nav_msgs::Odometry格式但VINS等系统可能输出geometry_msgs::PoseStamped。好在plan_env模块已经考虑到了这一点在代码中可以通过参数切换bool use_pose_stamped true; // 设置为true使用PoseStamped格式如果发现话题已经订阅但地图没有更新很可能是位姿格式不匹配导致的。这时需要检查SLAM系统输出的位姿类型并相应调整代码中的这个参数。3.2 深度图到点云的转换深度图的处理是整个流程中的关键环节。plan_env模块内部会调用PCL库将深度图转换为点云这个过程有几个参数需要注意depth_scale深度图的缩放因子depth_min和depth_max有效深度范围fx,fy,cx,cy相机内参这些参数通常需要在launch文件中配置param namedepth_scale value1000.0 / param namedepth_min value0.1 / param namedepth_max value10.0 / param namefx value525.0 / param namefy value525.0 / param namecx value319.5 / param namecy value239.5 /3.3 地图参数调优独立后的plan_env模块提供了丰富的地图参数可以根据实际应用场景进行调整map_resolution地图分辨率米/格map_size_x/y/z地图尺寸米inflate_radius障碍物膨胀半径max_ray_length射线投射最大长度在我的无人机项目中经过多次测试最终采用了以下配置param namemap_resolution value0.1 / param namemap_size_x value20.0 / param namemap_size_y value20.0 / param namemap_size_z value5.0 / param nameinflate_radius value0.3 / param namemax_ray_length value5.0 /4. 运行验证与问题排查4.1 基础功能验证模块启动后可以通过以下步骤验证基本功能使用rostopic list检查话题是否正常发布在RViz中添加OccupancyGrid或PointCloud2显示检查坐标系是否正确设置为world如果发现地图没有更新可以按照以下步骤排查使用rostopic hz检查输入话题是否有数据检查位姿和深度图的时间戳是否同步确认相机内参配置是否正确4.2 常见问题与解决方案在实际使用中我遇到了几个典型问题问题1话题订阅成功但地图不更新原因位姿格式不匹配或坐标系设置错误 解决检查位姿消息类型确认use_pose_stamped参数设置正确问题2地图更新延迟大原因地图分辨率过高或尺寸过大 解决适当降低分辨率或减小地图尺寸问题3障碍物显示不完整原因深度图范围设置不合理 解决调整depth_min和depth_max参数4.3 性能优化建议对于实时性要求高的应用可以考虑以下优化措施降低地图分辨率从0.1m调整为0.2m可以显著减少计算量限制地图尺寸根据实际场景需要设置合适的地图边界调整发布频率减少地图发布频率可以节省带宽使用多线程在资源允许的情况下启用多线程处理5. 进阶应用与功能扩展5.1 添加色彩信息原始的plan_env模块只处理几何信息但在某些场景下色彩信息也很重要。我们可以扩展模块使其支持RGB-D相机的彩色信息修改代码订阅RGB图像话题在点云生成时为每个点添加颜色发布带颜色的点云地图关键代码修改如下// 添加新的话题订阅 image_transport::Subscriber rgb_sub it_.subscribe(/camera/rgb/image_raw, 1, GridMap::rgbCallback, this); // 在点云生成时添加颜色 pcl::PointXYZRGB point; point.r r; point.g g; point.b b;5.2 多传感器融合独立后的plan_env模块可以方便地与其他传感器数据融合。例如我们可以添加激光雷达数据订阅激光雷达点云话题将点云转换到世界坐标系与深度图生成的点云融合这种融合可以弥补单一传感器的不足提高地图的完整性和准确性。5.3 动态障碍物处理原始模块对动态障碍物的处理较为简单。我们可以通过以下方式改进添加时间戳过滤移除长时间静止的点实现简单的运动物体检测为动态障碍物创建独立的地图层这些改进可以使地图更加适合动态环境下的导航任务。6. 实际项目中的应用案例在我的一个室内无人机项目中这个独立后的plan_env模块表现相当出色。项目需求是在未知室内环境中实现无人机的自主探索与避障。系统架构如下感知层Intel RealSense D435i提供深度图和位姿建图层独立后的plan_env模块构建3D占据栅格地图规划层基于地图进行路径规划经过测试模块在i7处理器上运行稳定占用资源约15% CPU内存消耗约500MB完全满足实时性要求。地图更新频率达到10Hz能够很好地支持无人机的避障需求。一个特别实用的技巧是调整inflate_radius参数。对于无人机应用我将其设置为0.5米这样规划时就会自动与障碍物保持安全距离。而在移动机器人应用中可以适当减小这个值提高路径规划的灵活性。在另一个服务机器人项目中我还尝试将多个独立plan_env模块部署在不同的计算节点上通过ROS分布式通信实现大范围环境的地图构建。这种架构特别适合需要覆盖大面积场景的应用。