[Python] How to get distance from PointCloud2
Hello guys. I follow this question to extract informations (distance) from PointCloud2 with kinect and ros. I need to get distance of [x,y] pixel in real world.
This is my code
When I run it, I get, for variable "int_data", three fields. But I do not know what they are. They seems like a distance, but I am not sure.
[INFO] [WallTime: 1501099449.999255] int_data (0.008157123811542988, 0.006674010306596756, 0.8540000319480896)
Can anyone explain in detail what they are?
Edit: This is the code that is being linked to:
#!/usr/bin/env python
from roslib import message
import rospy
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2, PointField
#listener
def listen():
rospy.init_node('listen', anonymous=True)
rospy.Subscriber("/camera/depth/points", PointCloud2, callback_kinect)
rospy.spin()
def callback_kinect(data) :
# pick a height
height = int (data.height / 2)
# pick x coords near front and center
middle_x = int (data.width / 2)
# examine point
middle = read_depth (middle_x, height, data)
# do stuff with middle
def read_depth(width, height, data) :
# read function
if (height >= data.height) or (width >= data.width) :
return -1
data_out = pc2.read_points(data, field_names=None, skip_nans=False, uvs=[[width, height]])
int_data = next(data_out)
rospy.loginfo("int_data " + str(int_data))
return int_data
if __name__ == '__main__':
try:
listen()
except rospy.ROSInterruptException:
pass
@l4ncelot, @sabruri1 I have faced an issue, i have used the code and insted of depth of camera, i subscribed to /velodyne_points. Now the issue is that when we have uvs=[[width, height]], the output is as follows (x,y,z):
and when i consider uvs=[ ], the output is as follows:
why the values are different? what is the meaning of uvs? what is the difference between uvs=[[width, height]] and uvs=[ ] ???
thanks