ROS2 sensor_msgs/PointCloud2教程

简介

sensor_msgs/PointCloud2是ROS2中用于表示三维点云数据的标准消息类型。点云是表示三维空间中点的集合的常用数据结构，广泛应用于机器人感知、计算机视觉等领域。本教程将介绍如何在ROS2中使用sensor_msgs/PointCloud2消息类型来发布和订阅点云数据。

消息定义

sensor_msgs/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.

std_msgs/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

主要字段说明：

header：标准的ROS2消息头，包含时间戳和坐标系信息。
height和width：点云的高度和宽度。如果点云是无序的，height为1，width为点云的长度。
fields：描述点云中每个点的数据格式，例如xyz坐标、颜色、法向量等。
is_bigendian：数据是否为大端字节序。
point_step：每个点的字节长度。
row_step：每行的字节长度。
data：实际的点云数据，以二进制blob的形式存储。
is_dense：如果点云中没有无效点，则为true。

发布点云数据

要发布点云数据，需要执行以下步骤：

创建一个ROS2节点。
创建一个sensor_msgs/PointCloud2消息的发布者。
填充PointCloud2消息的字段，包括header、height、width、fields、is_bigendian、point_step、row_step、data和is_dense。
发布PointCloud2消息。

通常，可以使用PCL（Point Cloud Library）库来处理点云数据，并将其转换为sensor_msgs/PointCloud2消息类型。

订阅点云数据

要订阅点云数据，需要执行以下步骤：

创建一个ROS2节点。
创建一个sensor_msgs/PointCloud2消息的订阅者。
定义一个回调函数，用于处理接收到的PointCloud2消息。
在回调函数中，可以将PointCloud2消息转换为PCL点云对象，以便进行进一步的处理和分析。

Example data sample

好的,让我为您提供一个具体的sensor_msgs/PointCloud2数据示例,并展示如何将其属性打印出来。

sensor_msgs/PointCloud2数据示例:

header:
  seq: 1234
  stamp:
    secs: 1621234567
    nsecs: 890000000
  frame_id: "base_link"
height: 1
width: 5
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
is_bigendian: False
point_step: 12
row_step: 60
data: [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, 1.3, 1.4, 1.5]
is_dense: True

在这个示例中:

header包含序列号(seq)、时间戳(stamp)和坐标系(frame_id)信息。
height为1,表示点云是无序的。
width为5,表示点云中有5个点。
fields描述了每个点的数据格式,包括x、y、z三个字段,数据类型为float32(datatype: 7)。
is_bigendian为False,表示数据为小端字节序。
point_step为12,表示每个点占用12个字节。
row_step为60,表示每行占用60个字节。
data为实际的点云数据,以一维数组的形式存储。
is_dense为True,表示点云中没有无效点。

打印sensor_msgs/PointCloud2数据属性的示例代码:

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2

class PointCloud2Subscriber(Node):
    def __init__(self):
        super().__init__('pointcloud2_subscriber')
        self.subscription = self.create_subscription(
            PointCloud2,
            'pointcloud2_topic',
            self.listener_callback,
            10)
        self.subscription

    def listener_callback(self, msg):
        self.get_logger().info('Received PointCloud2 message:')
        self.get_logger().info(f'Header: seq={msg.header.seq}, stamp={msg.header.stamp}, frame_id={msg.header.frame_id}')
        self.get_logger().info(f'Height: {msg.height}, Width: {msg.width}')
        self.get_logger().info(f'Fields: {msg.fields}')
        self.get_logger().info(f'Is Bigendian: {msg.is_bigendian}, Point Step: {msg.point_step}, Row Step: {msg.row_step}')
        self.get_logger().info(f'Data: {msg.data[:12]}...')
        self.get_logger().info(f'Is Dense: {msg.is_dense}')

def main(args=None):
    rclpy.init(args=args)
    pointcloud2_subscriber = PointCloud2Subscriber()
    rclpy.spin(pointcloud2_subscriber)
    pointcloud2_subscriber.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

在这个示例中,我们创建了一个PointCloud2Subscriber节点,并定义了一个回调函数listener_callback,用于处理接收到的PointCloud2消息。在回调函数中,我们使用get_logger().info()方法将PointCloud2消息的属性打印出来。

运行这个节点,当接收到PointCloud2消息时,将会在控制台输出类似以下的信息:

[INFO] [pointcloud2_subscriber]: Received PointCloud2 message:
[INFO] [pointcloud2_subscriber]: Header: seq=1234, stamp=1621234567890000000, frame_id=base_link
[INFO] [pointcloud2_subscriber]: Height: 1, Width: 5
[INFO] [pointcloud2_subscriber]: Fields: [name='x', offset=0, datatype=7, count=1...
[INFO] [pointcloud2_subscriber]: Is Bigendian: False, Point Step: 12, Row Step: 60
[INFO] [pointcloud2_subscriber]: Data: [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2]...
[INFO] [pointcloud2_subscriber]: Is Dense: True

这样,我们就可以清楚地看到PointCloud2消息的各个属性及其对应的值。

data

在sensor_msgs/PointCloud2消息中,data字段以一维数组的形式存储实际的点云数据。数组中的元素按照fields中定义的顺序和数据类型依次排列。

在上述示例中,fields定义了三个字段:x、y和z,它们的数据类型都是float32(datatype: 7),每个字段只有一个元素(count: 1)。因此,每个点的数据占用12个字节(point_step: 12),其中前4个字节表示x坐标,接下来4个字节表示y坐标,最后4个字节表示z坐标。

给定的data数组为:

data: [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, 1.3, 1.4, 1.5]

根据上述信息,我们可以将这个一维数组解析为五个三维点:

点1: (x=0.1, y=0.2, z=0.3)
点2: (x=0.4, y=0.5, z=0.6)
点3: (x=0.7, y=0.8, z=0.9)
点4: (x=1.0, y=1.1, z=1.2)
点5: (x=1.3, y=1.4, z=1.5)

这五个点构成了这个PointCloud2消息所表示的三维点云。每个点都有一个x、y和z坐标,分别表示点在三维空间中的位置。

在实际应用中,点云可能包含数千甚至数百万个点,每个点还可能包含其他属性,如颜色、法向量等。PointCloud2消息通过灵活的fields定义和紧凑的二进制存储格式,能够高效地表示和传输大规模点云数据。