Advertisement
Guest User

listener.py

a guest
Jul 26th, 2017
406
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 1.02 KB | None | 0 0
  1. #!/usr/bin/env python
  2. from roslib import message
  3. import rospy
  4. import sensor_msgs.point_cloud2 as pc2
  5. from sensor_msgs.msg import PointCloud2, PointField
  6.  
  7. #listener
  8. def listen():
  9.     rospy.init_node('listen', anonymous=True)
  10.     rospy.Subscriber("/camera/depth/points", PointCloud2, callback_kinect)
  11.     rospy.spin()
  12.  
  13. def callback_kinect(data) :
  14.     # pick a height
  15.     height =  int (data.height / 2)
  16.     # pick x coords near front and center
  17.     middle_x = int (data.width / 2)
  18.     # examine point
  19.     middle = read_depth (middle_x, height, data)
  20.     # do stuff with middle
  21.  
  22.  
  23. def read_depth(width, height, data) :
  24.     # read function
  25.     if (height >= data.height) or (width >= data.width) :
  26.         return -1
  27.     data_out = pc2.read_points(data, field_names=None, skip_nans=False, uvs=[[width, height]])
  28.     int_data = next(data_out)
  29.     rospy.loginfo("int_data " + str(int_data))
  30.     return int_data
  31.  
  32.  
  33. if __name__ == '__main__':
  34.     try:
  35.         listen()
  36.     except rospy.ROSInterruptException:
  37.         pass
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement