Get RGB value from realsense d435i pointcloud in simulation

asked 2022-04-08 07:27:12 -0500

Pigeon gravatar image

Hi everyone,

i got a gazebo simulation running with a realsense d435i camera in it. All the proper ROS-topics are published and i can get the recorded pointcloud from the topic "/camera/depth/color/points" without a problem. My Problem is that I also need to get the RGB values of the points and I am struggling with that. I used this kind of script for it:

import sensor_msgs.point_cloud2 as pc2
import ctypes
import struct

#subscribe to pointcloud topic define the function that does the parsing
def PointCloud(self, PointCloud2):
    gen = pc2.read_points(PointCloud2, skip_nans=True)
    int_data = list(gen)
    for x in int_data:
        test = x[3] 
        # cast float32 to int so that bitwise operations are possible
        s = struct.pack('>f' ,test)
        i = struct.unpack('>l',s)[0]
        # you can get back the float value by the inverse operations
        pack = ctypes.c_uint32(i).value
        r = (pack & 0x00FF0000)>> 16
        g = (pack & 0x0000FF00)>> 8
        b = (pack & 0x0000FF00)
        print r,g,b # prints r,g,b values in the 0-255 range
                    # x,y,z can be retrieved from the x[0],x[1],x[2]

but i only get grayscale values (for example 155 155 155) for the points. Does someone know, if the camera/depth/color/points topic can only return grayscale and not rgb?

Thanks in advance Pigeon

edit retag flag offensive close merge delete