📚 课程目录第2章 Cartographer 配置文件结构解析与自定义

2. Cartographer 配置文件结构解析与自定义

任务目标

编写一个ROS节点，订阅2D激光雷达原始话题（如/scan），将sensor_msgs/LaserScan消息转换为Cartographer所需的点云格式（sensor_msgs/PointCloud2），并发布到指定话题供Cartographer前端使用。

环境与依赖

Ubuntu 20.04 + ROS Noetic（或ROS Melodic）
已安装cartographer_ros及其依赖
激光雷达驱动已启动，并发布/scan话题（消息类型sensor_msgs/LaserScan）

核心实现

创建功能包lidar_converter，包含以下文件：

CMakeLists.txt：添加可执行文件与依赖
package.xml：声明依赖
src/laser_to_pointcloud_node.cpp：节点主程序

节点逻辑：订阅/scan，将每个激光点（距离+角度）转换为三维坐标（z=0），同时过滤掉无穷远点，生成PointCloud2并发布到/pointcloud_for_cartographer。

关键完整代码

// src/laser_to_pointcloud_node.cpp
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

ros::Publisher pub;

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    pcl::PointCloud<pcl::PointXYZ> cloud;
    cloud.header.frame_id = scan->header.frame_id;
    cloud.header.stamp = pcl_conversions::toPCL(scan->header.stamp);

    float angle = scan->angle_min;
    for (size_t i = 0; i < scan->ranges.size(); ++i, angle += scan->angle_increment)
    {
        float r = scan->ranges[i];
        if (std::isfinite(r) && r > scan->range_min && r < scan->range_max)
        {
            pcl::PointXYZ pt;
            pt.x = r * cos(angle);
            pt.y = r * sin(angle);
            pt.z = 0.0f;
            cloud.points.push_back(pt);
        }
    }
    cloud.width = cloud.points.size();
    cloud.height = 1;
    cloud.is_dense = true;

    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(cloud, output);
    pub.publish(output);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "laser_to_pointcloud_node");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("/scan", 10, scanCallback);
    pub = nh.advertise<sensor_msgs::PointCloud2>("/pointcloud_for_cartographer", 10);

    ros::spin();
    return 0;
}

CMakeLists.txt 关键配置

find_package(catkin REQUIRED COMPONENTS
  roscpp sensor_msgs pcl_ros pcl_conversions
)

add_executable(laser_to_pointcloud_node src/laser_to_pointcloud_node.cpp)
target_link_libraries(laser_to_pointcloud_node ${catkin_LIBRARIES})
add_dependencies(laser_to_pointcloud_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

编译与运行

# 在catkin工作空间内
catkin_make --pkg lidar_converter
source devel/setup.bash
rosrun lidar_converter laser_to_pointcloud_node

验证方法

启动激光雷达驱动，确认/scan话题有数据：rostopic echo /scan/ranges 能输出距离数组。
运行本节点后，用 rostopic echo /pointcloud_for_cartographer/points 查看点云数据，应能看到有限数量的三维点坐标。
在rviz中添加PointCloud2显示，话题选择/pointcloud_for_cartographer，Fixed Frame设为激光雷达的frame_id（如laser），应能看到与激光扫描匹配的点云轮廓。
通过条件：rviz中显示的点云形状与原始激光数据一致，且无NaN或无穷值点。

常见问题

frame_id不匹配：确保输出点云的frame_id与后续Cartographer配置中的tracking_frame一致。
点云为空：检查range_min/range_max设置，若激光数据超出范围会被过滤。
编译错误：确认pcl_ros和pcl_conversions已在CMakeLists中正确find_package。