ros2 订阅 sensor_msgs/msg/PointCloud2

2024-03-12 04:52

本文主要是介绍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 headerbuiltin_interfaces/Time stampint32 secuint32 nanosecstring 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[] fieldsuint8 INT8    = 1uint8 UINT8   = 2uint8 INT16   = 3uint8 UINT16  = 4uint8 INT32   = 5uint8 UINT32  = 6uint8 FLOAT32 = 7uint8 FLOAT64 = 8string 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: 1710137367nanosec: 353645752frame_id: camera_depth_optical_frame
height: 1
width: 278114
fields:
- name: xoffset: 0datatype: 7count: 1
- name: yoffset: 4datatype: 7count: 1
- name: zoffset: 8datatype: 7count: 1
- name: rgboffset: 16datatype: 7count: 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 o3dpcdPath = "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) & 0x0000ffgreen = (rgb_int >> 8) & 0x0000ffblue = (rgb_int) & 0x0000ffreturn (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 warningself.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 = pointr, 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_pointsself.colors = temp_colorsif 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的点云

裁减后效果如下

这篇关于ros2 订阅 sensor_msgs/msg/PointCloud2的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/800188

相关文章

基于UE5和ROS2的激光雷达+深度RGBD相机小车的仿真指南(五):Blender锥桶建模

前言 本系列教程旨在使用UE5配置一个具备激光雷达+深度摄像机的仿真小车,并使用通过跨平台的方式进行ROS2和UE5仿真的通讯,达到小车自主导航的目的。本教程默认有ROS2导航及其gazebo仿真相关方面基础,Nav2相关的学习教程可以参考本人的其他博客Nav2代价地图实现和原理–Nav2源码解读之CostMap2D(上)-CSDN博客往期教程: 第一期:基于UE5和ROS2的激光雷达+深度RG

C++编程:ZeroMQ进程间(订阅-发布)通信配置优化

文章目录 0. 概述1. 发布者同步发送(pub)与订阅者异步接收(sub)示例代码可能的副作用: 2. 适度增加缓存和队列示例代码副作用: 3. 动态的IPC通道管理示例代码副作用: 4. 接收消息的超时设置示例代码副作用: 5. 增加I/O线程数量示例代码副作用: 6. 异步消息发送(使用`dontwait`标志)示例代码副作用: 7. 其他可以考虑的优化项7.1 立即发送(ZMQ_IM

code: 400, msg: Required request body is missing 错误解决

引起这个错误的原因是,请求参数按照get方式给。 应该给json字符串才对 补充: 1. @RequestBody String resource 加@RequestBody必须给json字符串,否则会报错400,记如标题错误。 不加这个的进行请求的话,其实post和get就没有什么区别了。 2. List<String> indexCodes=(List<String>)json.

MQTT协议中信息长度MSG len字段分析

截图自: 主要是说数据字节长度的计算: 每个字节由1个持续位和7个数据位组成:如果持续位为1,表示接下来的一个字节仍然表示长度的一部分 7个数据位表示的数据     0-127   共计128个数字 所以如上图的表格所示 1个字节,2个字节,3个字节,4个字节的数据范围 切记:MQTT长度的表示范围 最多使用4个字节  故这里存在着数据长度的限制  (不过真心牛掰! 试试Q

用python fastapi写一个http接口,使ros2机器人开始slam toolbox建图

如果你想使用Python的FastAPI框架编写一个HTTP接口,以便通过接口启动ROS 2机器人的SLAM Toolbox建图,可以按照以下方式进行: 首先,确保你已经安装了fastapi和uvicorn库。你可以使用以下命令进行安装: pip install fastapi uvicorn 接下来,创建一个Python文件(例如app.py),并将以下代码添加到文件中: import

ros2

https://www.guyuehome.com/805

[rk3588 ubuntu20.04]移植ROS2

目录 1 使用命令行安装ROS2 1.1设置语言 1.2添加源 1.3安装ROS2 1.4设置环境变量 2 在编译源码阶段安装ROS2 2.1调整roofts.img大小 2.2 安装ROS2 3 ROS2功能测试 1 使用命令行安装ROS2 1.1设置语言         设置语言为UTF-8。 sudo apt update && sudo apt insta

关于在使用Redssion发布的订阅遇到大bug

目录: 故事开始解决过程昙花一现最终解决 故事开始 在我开发一个仿微信的一个项目的时候,有一个功能在服务端接受到客户端消息时需要做出反应 比如:有多个客户端向服务端发送消息 ,然后服务端将消息返回给多个客户端,此时可以利用redis的发布订阅 通过将消息发送给同一个主题监听这个主题是否接受到消息从未做出相应操作。 我使用了redis 的客户端ReissionClient中

区块链 以太坊 多层调用,获取调用者 msg.sender

msg.sender:合约的直接调用者。 由于是直接调用者,所以当处于 用户A->合约1->合约2 调用链下, 若在合约2内使用msg.sender,得到的会是合约1的地址。如果想获取用户A,可以用tx.origin, tx.origin:交易的"始作俑者",整个调用链的起点。 pragma solidity ^0.4.25;contract Sample{​//Stat

微信小程序 云开发 订阅消息 获取下发权限失败 errCode: 20001

通过下面的代码获取订阅消息的下发权限 提示错误 errMsg: "requestSubscribeMessage:fail No template data return, verify the template id exist", errCode: 20001 这个错误的意思是,没有模板数据返回,请检查模板id是否存在。 确认模板ID是订阅消息的模板ID且正确。 可能