def pcl_to_ros(pcl_array):
""" Converts a ROS PointCloud2 message to a pcl PointXYZRGB
Args:
pcl_array (PointCloud_PointXYZRGB): A PCL XYZRGB point cloud
Returns:
PointCloud2: A ROS point cloud
"""
ros_msg = PointCloud2()
ros_msg.header.stamp = rospy.Time.now()
ros_msg.header.frame_id = "world"
ros_msg.height = 1
ros_msg.width = pcl_array.size
ros_msg.fields.append(PointField(
name="x",
offset=0,
datatype=PointField.FLOAT32, count=1))
ros_msg.fields.append(PointField(
name="y",
offset=4,
datatype=PointField.FLOAT32, count=1))
ros_msg.fields.append(PointField(
name="z",
offset=8,
datatype=PointField.FLOAT32, count=1))
ros_msg.fields.append(PointField(
name="rgb",
offset=16,
datatype=PointField.FLOAT32, count=1))
ros_msg.is_bigendian = False
ros_msg.point_step = 32
ros_msg.row_step = ros_msg.point_step * ros_msg.width * ros_msg.height
ros_msg.is_dense = False
buffer = []
for data in pcl_array:
s = struct.pack('>f', data[3])
i = struct.unpack('>l', s)[0]
pack = ctypes.c_uint32(i).value
r = (pack & 0x00FF0000) >> 16
g = (pack & 0x0000FF00) >> 8
b = (pack & 0x000000FF)
buffer.append(struct.pack('ffffBBBBIII', data[0], data[1], data[2], 1.0, b, g, r, 0, 0, 0, 0))
ros_msg.data = "".join(buffer)
return ros_msg
pcl_helper.py 文件源码
python
阅读 25
收藏 0
点赞 0
评论 0
评论列表
文章目录