如何通过ROS或Velodyne驱动为VLP-16点云添加distance字段

完全可以实现，优先推荐自定义ROS节点方案，对原有系统侵入性低、适配性更强，两种方案的具体操作步骤如下：

方案1：自定义ROS节点处理（推荐） #

• 核心逻辑：订阅Velodyne驱动原生发布的sensor_msgs/PointCloud2话题，遍历每个点计算距离，构建新增了distance字段的新PointCloud2消息再发布
• 具体实现步骤：
  1. 新建ROS功能包，依赖添加roscpp、sensor_msgs、pcl_ros、pcl_conversions
  2. 定义自定义点类型，示例代码如下：

  // 仅保留x/y/z/distance四个字段的自定义点类型
  struct PointXYZDist {
    PCL_ADD_POINT4D; // 自带x/y/z字段定义
    float distance;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  } EIGEN_ALIGN16;
  
  // 注册点类型到PCL
  POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZDist,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, distance, distance)
  )
  

  3. 编写点云回调处理逻辑：

  ros::Publisher pub;
  void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& input_msg) {
    pcl::PointCloud<pcl::PointXYZI> input_cloud;
    pcl::fromROSMsg(*input_msg, input_cloud);
  
    pcl::PointCloud<PointXYZDist> output_cloud;
    output_cloud.resize(input_cloud.size());
    output_cloud.header = input_cloud.header;
  
    for (size_t i=0; i<input_cloud.size(); i++) {
      output_cloud.points[i].x = input_cloud.points[i].x;
      output_cloud.points[i].y = input_cloud.points[i].y;
      output_cloud.points[i].z = input_cloud.points[i].z;
      // 计算点到雷达中心的欧氏距离
      output_cloud.points[i].distance = sqrt(pow(input_cloud.points[i].x, 2) + pow(input_cloud.points[i].y, 2) + pow(input_cloud.points[i].z, 2));
    }
  
    sensor_msgs::PointCloud2 output_msg;
    pcl::toROSMsg(output_cloud, output_msg);
    pub.publish(output_msg);
  }
  

  4. 主函数中完成订阅、发布初始化即可正常运行
• 性能优化提示：如果追求极致处理效率，可以直接操作PointCloud2的原始字节流，跳过PCL类型转换步骤。

方案2：修改Velodyne驱动源码 #

• 核心逻辑：直接在Velodyne驱动的点云组装环节添加distance字段计算逻辑，驱动直接输出带distance字段的点云
• 具体实现步骤：
  1. 下载对应版本的Velodyne驱动源码到ROS工作空间的src目录，修改的核心文件为velodyne_pointcloud/src/lib/convert.cc
  2. 在文件的点云字段定义部分，新增distance字段，类型设置为float32
  3. 在每个点坐标赋值的代码段后，添加距离计算逻辑：point.distance = sqrt(x*x + y*y + z*z);
  4. 重新编译整个工作空间，启动驱动后发布的点云就会自带distance字段
• 注意事项：该方案修改了驱动源码，后续升级驱动版本需要重新移植修改内容，不同型号Velodyne驱动的修改位置可能存在差异。

内容的提问来源于stack exchange，提问作者Yi-Cheng