ROS常用消息之PointCloud2

sensor_msgs/PointCloud2 ------ 3D 点云消息

官方文档：sensor_msgs/PointCloud2 Message

源码位置：File: sensor_msgs/PointCloud2.msg

1. 概述

sensor_msgs/PointCloud2 用于表示三维点云数据 ：每个点不仅包含坐标，还可包含强度、法线、颜色、时间戳等附加信息。它是 3D 激光雷达、深度相机（RGB-D）数据在 ROS 中的标准载体。

典型使用场景：

3D 激光雷达（Velodyne、Ouster）驱动输出
RGB-D 相机（Kinect、RealSense）深度点云
点云滤波（体素降采样、直通滤波）、配准、分割、目标检测
3D 建图与导航避障

何时使用： 当需要传输 N 维点集合 时使用本消息。

注意： 2D 激光雷达单帧扫描用 sensor_msgs/LaserScan；旧版消息 sensor_msgs/PointCloud 已不推荐使用。

2. 消息定义

Raw Message Definition

cpp 复制代码

# This message holds a collection of N-dimensional points, which may
# contain additional information such as normals, intensity, etc. The
# point data is stored as a binary blob, its layout described by the
# contents of the "fields" array.

# The point cloud data may be organized 2d (image-like) or 1d
# (unordered). Point clouds organized as 2d images may be produced by
# camera depth sensors such as stereo or time-of-flight.

# Time of sensor data acquisition, and the coordinate frame ID (for 3d
# points).
Header header

# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
uint32 height
uint32 width

# Describes the channels and their layout in the binary data blob.
PointField[] fields

bool    is_bigendian # Is this data bigendian?
uint32  point_step   # Length of a point in bytes
uint32  row_step     # Length of a row in bytes
uint8[] data         # Actual point data, size is (row_step*height)

bool is_dense        # True if there are no invalid points

PointField（字段描述）

cpp 复制代码

# This message holds the description of one point entry in the
# PointCloud2 message format.
uint8 INT8    = 1
uint8 UINT8   = 2
uint8 INT16   = 3
uint8 UINT16  = 4
uint8 INT32   = 5
uint8 UINT32  = 6
uint8 FLOAT32 = 7
uint8 FLOAT64 = 8

string name      # Name of field
uint32 offset    # Offset from start of point struct
uint8  datatype  # Datatype enumeration, see above
uint32 count     # How many elements in the field

Compact Message Definition

cpp 复制代码

std_msgs/Header header
uint32 height
uint32 width
sensor_msgs/PointField[] fields
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense

3. 字段速查表

字段	类型	说明
`header`	`std_msgs/Header`	采集时刻、坐标系（3D 点云所在 frame，如 `velodyne`）
`height`	`uint32`	点云组织方式：有序点云为行数；无序点云为 `1`
`width`	`uint32`	点云列数；无序点云时为点的总数
`fields`	`PointField[]`	每个字段（如 x/y/z/intensity/rgb）的名称、类型与偏移
`is_bigendian`	`bool`	数据是否大端存储
`point_step`	`uint32`	单个点占用的字节数
`row_step`	`uint32`	一行占用的字节数
`data`	`uint8[]`	点云二进制数据，大小 = `row_step × height`
`is_dense`	`bool`	是否无无效点（如 NaN / Inf）

PointField 数据类型常量

常量	值	含义
`INT8`	1	8 位有符号整数
`UINT8`	2	8 位无符号整数
`INT16`	3	16 位有符号整数
`UINT16`	4	16 位无符号整数
`INT32`	5	32 位有符号整数
`UINT32`	6	32 位无符号整数
`FLOAT32`	7	32 位浮点数
`FLOAT64`	8	64 位浮点数

4. 核心概念

数据布局：二进制 blob

PointCloud2 的 data 是一段连续二进制数据，不直接存放结构化点对象 。每个点由 point_step 字节组成，内部各字段通过 fields[i].offset 定位：

复制代码

data 偏移 = 行索引 × row_step + 列索引 × point_step
字段偏移 = 上述点偏移 + fields[i].offset

有序（organized）点云中，第 (row, col) 个点：

无序（unordered）点云中，第 i 个点偏移为 i * point_step，此时 height == 1、width == 点数。

常见 fields 组合

传感器	常见 fields	说明
3D 激光雷达	`x, y, z, intensity`	坐标 + 反射强度
RGB-D 相机	`x, y, z, rgb`	坐标 + 颜色（颜色打包在 4 字节中）
结构光 / 激光	`x, y, z`	纯几何
多回波雷达	`x, y, z, intensity, ring, time`	增加环号与时间

与 PCL 的互转

ROS 官方推荐用 pcl_conversions 在 PointCloud2 与 PCL 点云类型（如 pcl::PointCloud<pcl::PointXYZ>）之间转换：

cpp 复制代码

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>

// PointCloud2 -> PCL
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*msg, *cloud);

// PCL -> PointCloud2
sensor_msgs::PointCloud2 out;
pcl::toROSMsg(*cloud, out);
out.header = msg->header;

当需要保序（organized）或字段种类不固定时，也可直接用 pcl::PointCloud<pcl::PointXYZRGB> 等对应类型。

5. 实践案例

案例一：订阅点云，计算点的数量与最近点（C++）

cpp 复制代码

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <limits>

void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg(*msg, *cloud);

  if (cloud->empty())
  {
    ROS_WARN("empty cloud");
    return;
  }

  // 统计有效点并找最近点（到原点）
  double min_dist = std::numeric_limits<double>::infinity();
  size_t valid = 0;
  for (const auto& p : cloud->points)
  {
    if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z))
      continue;   // 跳过无效点
    ++valid;
    double d = p.x * p.x + p.y * p.y + p.z * p.z;
    if (d < min_dist) min_dist = d;
  }

  ROS_INFO("cloud: %zu points, %zu valid, nearest dist %.2f m",
           cloud->size(), valid, std::sqrt(min_dist));
}

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

  ros::Subscriber sub = nh.subscribe("/velodyne_points", 1, cloudCallback);

  ros::spin();
  return 0;
}

案例二：体素降采样（C++）

cpp 复制代码

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>

void downsampleCallback(const sensor_msgs::PointCloud2::ConstPtr& msg,
                        ros::Publisher& pub)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg(*msg, *cloud);

  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::VoxelGrid<pcl::PointXYZ> voxel;
  voxel.setInputCloud(cloud);
  voxel.setLeafSize(0.05f, 0.05f, 0.05f);   // 5cm 体素
  voxel.filter(*filtered);

  sensor_msgs::PointCloud2 out;
  pcl::toROSMsg(*filtered, out);
  out.header = msg->header;
  pub.publish(out);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "cloud_downsample");
  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("/cloud_downsampled", 1);
  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>(
      "/velodyne_points", 1,
      [&pub](const sensor_msgs::PointCloud2::ConstPtr& msg)
      { downsampleCallback(msg, pub); });

  ros::spin();
  return 0;
}

案例三：手动解析 PointCloud2 的 x/y/z 字段（不依赖 PCL，C++）

cpp 复制代码

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <cstring>

// 按字段名查找 PointField
const sensor_msgs::PointField* findField(
    const sensor_msgs::PointCloud2& cloud, const std::string& name)
{
  for (const auto& f : cloud.fields)
    if (f.name == name) return &f;
  return nullptr;
}

void parseCloud(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  const sensor_msgs::PointField* fx = findField(*msg, "x");
  const sensor_msgs::PointField* fy = findField(*msg, "y");
  const sensor_msgs::PointField* fz = findField(*msg, "z");
  if (!fx || !fy || !fz) return;

  size_t n = msg->width;   // 无序点云：height==1
  for (size_t i = 0; i < n; ++i)
  {
    const uint8_t* p = &msg->data[i * msg->point_step];

    float x, y, z;
    std::memcpy(&x, p + fx->offset, sizeof(float));
    std::memcpy(&y, p + fy->offset, sizeof(float));
    std::memcpy(&z, p + fz->offset, sizeof(float));

    if (std::isfinite(x) && std::isfinite(y) && std::isfinite(z))
      ROS_INFO("point %zu: (%.3f, %.3f, %.3f)", i, x, y, z);
  }
}

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

  ros::Subscriber sub = nh.subscribe("/velodyne_points", 1, parseCloud);

  ros::spin();
  return 0;
}

6. 常见问题与避坑

问题	说明 / 正确做法
把 `data` 当结构化数组直接索引	必须结合 `point_step`、`row_step`、`fields[].offset` 手动解析，或用 PCL 转换
忘记检查 `is_dense`	`is_dense == false` 时点云含 NaN / Inf，计算前应过滤，否则下游算法崩溃
字段名 / 类型不一致	解析前先按 `fields` 判断；不同传感器字段集合不同
有序点云按无序处理	organized 点云 `height > 1`，像素级操作（如结合图像）时需按 `(row, col)` 访问
数据大端	检查 `is_bigendian`，多数平台为小端（false）
点云频率高、数据量大	订阅队列设小（如 1），必要时降采样 / 直通滤波
坐标系错误	`header.frame_id` 须与 tf 树一致（如 `velodyne`），否则点云与机器人位姿无法对齐
用 LaserScan 承载 3D 数据	3D 数据用 PointCloud2；2D 单帧扫描用 LaserScan，两者字段完全不同