How to record Raw data (X,Y,Z,R,G,B ) from a Kinect V2 in a .txt flat file with ROS?

0

I'm Using ROS Kinetic (libfreenect2) and I'm trying to record XYZRBG data with python based on the follow code:

#!/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()

However, the recorded data is different to same topic in rviz visualization. Lighted color regions are missing on the saved file.

python
kinect
ros
kinect-v2
libfreenect2
asked on Stack Overflow May 8, 2018 by Harold Murcia

0 Answers

Nobody has answered this question yet.


User contributions licensed under CC BY-SA 3.0