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的点云

裁减后效果如下

  • 13
    点赞
  • 15
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
您可以按照以下步骤将sensor_msgs/PointCloud2保存为bin文件: 1. 首先,将PointCloud2消息转换为ROS中的PointCloud2数据类型。您可以使用`rosmsg`命令来查看消息的结构。 ```bash rosmsg show sensor_msgs/PointCloud2 ``` 2. 在ROS中,可以使用C++或Python代码来访问PointCloud2消息的数据。以下是一个示例Python代码: ```python import rospy from sensor_msgs.msg import PointCloud2 def callback(pointcloud): # 在这里访问pointcloud数据并进行保存为bin文件的操作 pass rospy.init_node('save_pointcloud') rospy.Subscriber('/your_pointcloud_topic', PointCloud2, callback) rospy.spin() ``` 3. 在回调函数中,您可以按照PointCloud2消息的结构访问数据。PointCloud2消息包含一个或多个字段,例如位置(x、y、z)和颜色(rgb)。您需要根据实际情况进行相应的处理。 4. 在回调函数中,您可以使用Python中的`struct`模块将数据以二进制形式保存为bin文件。以下是一个示例代码: ```python import struct def callback(pointcloud): # 获取PointCloud2消息的数据格式 fields = pointcloud.fields # 获取PointCloud2消息的二进制数据 data = pointcloud.data # 创建一个保存为bin文件的文件对象 with open('pointcloud.bin', 'wb') as f: # 对每个点进行迭代 for i in range(0, len(data), pointcloud.point_step): # 对每个字段进行迭代 for field in fields: # 获取字段的偏移量和大小 offset = field.offset size = field.count * field.datatype # 从二进制数据中提取字段的值 value = struct.unpack_from(field.datatype, data, i + offset)[0] # 将字段的值以二进制形式写入bin文件 f.write(struct.pack(field.datatype, value)) ``` 5. 运行ROS节点并接收PointCloud2消息。当消息到达时,回调函数将被调用,并将点云数据保存为bin文件。 请注意,这只是一个示例代码,您可能需要根据您的实际情况进行适当的修改和调整。此外,如果您想保存其他字段(例如法线、纹理等),您需要相应地修改代码。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值