ros2 订阅 sensor_msgs/msg/PointCloud2

1.消息结构 

daichang@daichang-ASUS:~/Desktop/ros2_ws$ ros2 interface show sensor_msgs/msg/PointCloud2
# 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).
std_msgs/Header header
	builtin_interfaces/Time stamp
		int32 sec
		uint32 nanosec
	string frame_id

# 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
	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      #
	uint32 offset    #
	uint8  datatype  #
	uint32 count     #

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

# 该消息保存了N维点的集合，可以
# 包含附加信息，例如法线、强度等。
# 点数据存储为二进制 blob，其布局由
# “fields”数组的内容。
#
# 点云数据可以组织为 2d（类似图像）或 1d（无序）。
# 组织为二维图像的点云可以由相机深度传感器生成
# 例如立体声或飞行时间。
# 传感器数据采集时间，以及坐标系ID（对于3d点）。

The data field of a PointCloud2 message contains the list of points encoded as a byte stream, where each point is a struct. The format of the points in an individual PointCloud2 message is defined by the fields, which is a list of sensor_msgs/PointField objects. In order to turn the data back into individual points, it has to be deserialized. The PointCloud library has defined methods that do this automatically (in Python, you'd use pc2.read_points.) Under the hood, these methods use memory reinterpretation to transform the byte stream to a list of points (e.g. Python's struct.unpack() or reinterpret casting in C++).PointCloud2 消息data编码为字节流的点列表，其中每个点都是一个struct体。单个 PointCloud2 消息中点的格式由字段定义，fields是 sensor_msgs/PointField 对象的列表。为了将数据转换回单个点，必须对其进行反序列化。 PointCloud 库定义了自动执行此操作的方法（在 Python 中，您可以使用pc2.read_points。在后台，这些方法使用内存重新解释将字节流转换为点列表（例如 Python 的 struct.unpack() 或在 C++ 中重新解释强制转换）。

The advantage of the byte stream representation is that it's a compact way to transport the data that is extremely flexible, allowing any arbitrary struct to be used for the points. The disadvantage is that the message unreadable without deserialization.字节流表示的优点是，它是一种非常灵活的数据传输方式，允许对点使用任何任意结构。缺点是消息如果不反序列化就无法读取。

运行ros2 topic info 输出如下

header:
  stamp:
    sec: 1710137367
    nanosec: 353645752
  frame_id: camera_depth_optical_frame
height: 1
width: 278114
fields:
- name: x
  offset: 0
  datatype: 7
  count: 1
- name: y
  offset: 4
  datatype: 7
  count: 1
- name: z
  offset: 8
  datatype: 7
  count: 1
- name: rgb
  offset: 16
  datatype: 7
  count: 1
is_bigendian: false
point_step: 20
row_step: 5562280
data:
- 236
- 139
- 90
- 190
- 47
- 6
- 14
- 190
- 36
- 219
- 121
- 62

 在ros2订阅节点中打印self.get_logger().info(f"Fields: {msg.fields}")

[INFO] [1710137924.596260401] [pointcloud_sub]: Fields: [sensor_msgs.msg.PointField(name='x', offset=0, datatype=7, count=1), sensor_msgs.msg.PointField(name='y', offset=4, datatype=7, count=1), sensor_msgs.msg.PointField(name='z', offset=8, datatype=7, count=1), sensor_msgs.msg.PointField(name='rgb', offset=16, datatype=7, count=1)]

• name: 字段名称，标识了数据的含义（例如'x'、'y'、'z'对应点的三维坐标，'rgb'对应点的颜色信息）。

• offset: 字段在点云数据点中的偏移量（以字节为单位）。例如，'x'的偏移量是0，表示每个点的第一个字段就是x坐标值。相应地，'rgb'的偏移量是16，表示在每个点的数据中，颜色信息跟在坐标信息后面。

• datatype: 字段的数据类型。这里的值7对应于FLOAT32，意味着每个字段都是32位浮点数。这是一种常用的格式，适合表示三维空间中的坐标和颜色信息。

• count: 每个字段的元素数量。这里的值1意味着每个字段（x、'y'、'z'和'rgb'）都只包含一个元素。对于坐标来说，这很直观，每个坐标轴一个值；而对于'rgb'，尽管实际上包含了三个颜色分量，但它们被打包成了单个的FLOAT32数值。

特别地，'rgb'字段的处理稍微复杂一些。虽然它被声明为FLOAT32类型，但实际上通常是将三个8位整数（分别代表红、绿、蓝分量）连同一个8位的透明度分量（通常不使用）打包到一个32位整数中。这意味着你需要对这个FLOAT32值进行位操作，以提取出RGB颜色值。

综上所述，这条信息告诉我们PointCloud2消息中的每个点都包括了三维坐标（x, y, z）和颜色（rgb）信息，每个字段都是32位浮点数格式，颜色信息需要特殊处理以提取RGB分量。这为我们提供了必要的信息，来正确地解析和处理点云数据。

2.订阅节点

通过ros2订阅节点并保存为ply文件，实现如下：

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField
import sensor_msgs_py.point_cloud2 as pc2
import struct
import numpy as np
import open3d as o3d


pcdPath = "src/markless-calibration/pcd/output_point_cloud.ply"

def parse_rgb_float(rgb_float):
    # 将float32编码的rgb值转换为整数
    rgb_int = struct.unpack('I', struct.pack('f', rgb_float))[0]
    # 按位提取rgb值
    red = (rgb_int >> 16) & 0x0000ff
    green = (rgb_int >> 8) & 0x0000ff
    blue = (rgb_int) & 0x0000ff
    return (red, green, blue)

class PointCloud2Subscriber(Node):
    def __init__(self):
        super().__init__('pointcloud_sub')
        self.subscription = self.create_subscription(
            PointCloud2,
            '/camera/camera/depth/color/points',
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning
        self.points = []
        self.colors = []
        

    def listener_callback(self, msg):

        # 解析点云数据
        point_list = list(pc2.read_points_list(msg, field_names=("x", "y", "z", "rgb"),skip_nans= True))

        temp_points = []
        temp_colors = []

        for point in point_list:
            x, y, z, rgb = point
            r, g, b = parse_rgb_float(rgb)
            temp_points.append([x, y, z])
            # 颜色值需要从[0, 255]范围转换到[0, 1]范围
            temp_colors.append([r / 255.0, g / 255.0 , b / 255.0])
        
        self.points = temp_points
        self.colors = temp_colors

        if len(self.points) > 10000:
            self.save_point_cloud_to_ply(pcdPath)
            self.get_logger().info(f'Received {len(self.points)} points to output_point_cloud.ply')
            self.points = []
            self.colors = []



        # self.get_logger().info(f"Fields: {msg.fields}")
        
    
    def save_point_cloud_to_ply(self, filename):
        # 创建一个空的点云对象
        point_cloud = o3d.geometry.PointCloud()
        # 设置点云和颜色
        point_cloud.points = o3d.utility.Vector3dVector(self.points)
        point_cloud.colors = o3d.utility.Vector3dVector(self.colors)
        # 保存点云到PLY文件
        o3d.io.write_point_cloud(filename, point_cloud)
        self.get_logger().info(f"Received {len(self.points)} points saved to {filename}")


def main(args=None):
    rclpy.init(args=args)
    pointcloud_subscriber = PointCloud2Subscriber()
    rclpy.spin(pointcloud_subscriber)
    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    pointcloud_subscriber.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

得到点云文件output_point_cloud.ply，在cloudcompare中显示如下

pointcloud in compare

3.裁减生成的点云

由于d405的最佳工作距离为7到50cm，在launch文件中设置{'name': 'clip_distance', 'default': '0.5', 'description': ''},来裁减到距离大于50cm的点云

裁减后效果如下