Python 生成带有RGBA信息的 ROS PointCloud2 message

最新推荐文章于 2024-07-18 16:21:12 发布

风海流

最新推荐文章于 2024-07-18 16:21:12 发布

阅读量3.3k

点赞数 4

分类专栏： ROS 机器人 python 文章标签： ROS PointCloud2 RGB python

本文链接：https://blog.csdn.net/huyaoyu/article/details/103193523

版权

本文介绍了如何在Python环境下，根据点云数据与距离信息生成带有RGBA颜色的ROS PointCloud2消息。重点在于理解如何将RGBA信息编码到uint32整数中，以及利用NumPy的structured array实现点云上色。最终提供了参考代码和相关资源链接。

摘要由CSDN通过智能技术生成

起因

最近有一个工作是需要把一组三维点以ROS PointCloud2 messge的形式进行publish。并且需要使用python环境。原始点云只有坐标数据，需要根据点距离坐标原点的距离对点云进行上色。

经过

通过参考一些开源项目的源码，并通过NumPy的structured array功能实现了PointCloud2 message的生成。其中需要注意的是一点，PointCloud2 message 通过一个uint32类型的整数表达RGBA四个通道。虽然称作RGBA，但是在uint32的4个字节中，从高位向低位分别是B, G, R, A通道。另外，对2维的structured array进行索引，其方法和行为可能都和普通的高维array有些差异。

结果

直接上源码。

import numpy as np

import rospy
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Header
from ColorMapping import color_map

DIST_COLORS = [\
    "#2980b9",\
    "#27ae60",\
    "#f39c12",\
    "#c0392b",\
    ]

DIST_COLOR_LEVELS = 20

def convert_numpy_2_pointcloud2_color(points, stamp=None, frame_id=None, maxDistColor=None):
    '''
    Create a sensor_msgs.PointCloud2 from an array
    of points. 

    This function will automatically assign RGB values to each point. The RGB values are
    determined by the distance of a point from the origin. Use maxDistColor to set the distance 
    at which the color corresponds to the farthest distance is used.

    points: A NumPy array of Nx3.
    stamp: An alternative ROS header stamp.
    frame_id: The frame id. String.
    maxDisColor: Should be positive if specified..

    This function get inspired by 
    https://github.com/spillai/pybot/blob/master/pybot/externals/ros/pointclouds.py
    https://gist.github.com/lucasw/ea04dcd65bc944daea07612314d114bb
    (https://answers.ros.org/question/289576/understanding-the-bytes-in-a-pcl2-message/)
    and expo_utility.xyz_array_to_point_cloud_msg() function of the AirSim package.

    ROS sensor_msgs/PointField.
    http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointField.html

    More references on mixed-type NumPy array, structured array.
    https://docs.scipy.org/doc/numpy/reference/arrays.dtypes.html
    https://stackoverflow.com/questions/37791134/merge-width-x-height-x-3-numpy-uint8-array-into-width-x-height-x-1-uint32-array
    https://jakevdp.github.io/PythonDataScienceHandbook/02.09-structured-data-numpy.html
    '''
    
    # Clipping input.
    dist = np.linalg.norm(