turtlebot subscriber pointcloud2 shows color in gazebo simulator but not in robot

asked 2016-07-01 11:58:21 -0600

fartagaintuxedo gravatar image

Hi, i am subscribing to topic "/camera/depth/points" and message PointCloud2 on a turtlebot (deep learning version)

I have used the python script below under the gazebo simulator environment and i can receive x, y, z and rgb values successfully.

However, when i run it in the robot, the rgb values are missing.

Is this a problem of my turtlebot version, or camera or is it that i have to specify somewhere that i want to receive PointCloud2 type="XYZRGB"? or is it a sync problem? Any clues please thanks!

#!/usr/bin/env python
import rospy
import struct
import ctypes
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2

file  = open('workfile.txt', 'w')

def callback(msg):

    data_out = pc2.read_points(msg, skip_nans=True)

    loop = True
    while loop:
        try:
            int_data = next(data_out)
            s = struct.pack('>f' ,int_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)

            file.write(str(int_data[0])+","+str(int_data[1])+","+str(int_data[2])+","+str(r)+","+str(g)+","+str(b)+"\n")

        except Exception as e:
            rospy.loginfo(e.message)
            loop = False
            file.flush
            file.close

def listener():

    rospy.init_node('writeCloudsToFile', anonymous=True)
    rospy.Subscriber("/camera/depth/points", PointCloud2, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()
edit retag flag offensive close merge delete