extract_head_cloud.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from sensor_msgs.msg import RegionOfInterest, Image, CameraInfo
00005 import message_filters
00006 from cv_bridge import CvBridge, CvBridgeError
00007 from geometry_msgs.msg import PolygonStamped, Point32, PointStamped
00008 import numpy as np
00009 import dynamic_reconfigure.client
00010 from threading import RLock
00011 import contextlib # get rid of this once PR2 moves to python 2.7
00012 from rospy.service import ServiceException
00013 
00014 class Extract(object):
00015         bridge = CvBridge()
00016 
00017         xfilter = dynamic_reconfigure.client.Client('xfilter')
00018         yfilter = dynamic_reconfigure.client.Client('yfilter')
00019         zfilter = dynamic_reconfigure.client.Client('zfilter')
00020         
00021         depth_im = None
00022         roi      = None
00023 
00024         image_lock = RLock()
00025         roi_lock   = RLock()
00026         
00027         K = []
00028 
00029         def __init__(self):
00030                 roi_sub   = rospy.Subscriber('roi', RegionOfInterest, self.roi_cb)
00031                 depth_sub = message_filters.Subscriber('depth_image', Image)
00032                 info_sub  = message_filters.Subscriber('camera_info', CameraInfo)
00033                 
00034                 ts = message_filters.TimeSynchronizer([depth_sub, info_sub], 10)
00035                 ts.registerCallback(self.depth_cb);
00036                 
00037                 r = rospy.Rate(10)
00038                 while not rospy.is_shutdown() and not self.roi and not self.depth_im:
00039                         r.sleep()
00040                 while not rospy.is_shutdown():
00041                         self.update_params()
00042                         r.sleep()
00043 
00044         def roi_cb(self, msg):
00045                 with self.roi_lock:
00046                         self.roi = msg
00047 
00048         def depth_cb(self, depth_msg, info_msg):
00049                 with self.image_lock:
00050                         self.depth_im = self.bridge.imgmsg_to_cv(depth_msg, '32FC1')
00051                         self.K = info_msg.K
00052 
00053         def update_params(self):
00054                 with contextlib.nested(self.roi_lock, self.image_lock):
00055                         if self.depth_im and self.roi:
00056                                 x = self.roi.x_offset
00057                                 y = self.roi.y_offset
00058                                 w = self.roi.width
00059                                 h = self.roi.height
00060                                 
00061                                 depth = self.depth_im[y+h/2,x+w/2]
00062 
00063                                 face_center_img = np.array((x+w/2.0, y+h/2))
00064                                 face_center_pt  = (face_center_img - (self.K[2], self.K[5])) / np.array([self.K[0], self.K[4]])
00065                                 face_center_depth = self.depth_im[face_center_img]
00066                                 
00067                                 try:
00068                                         self.xfilter.update_configuration({
00069                                                 'filter_field_name': 'x',
00070                                                 'filter_limit_min': face_center_pt[0]-0.2,
00071                                                 'filter_limit_max': face_center_pt[0]+0.2
00072                                         })
00073 
00074                                         self.yfilter.update_configuration({
00075                                                 'filter_field_name': 'y',
00076                                                 'filter_limit_min': face_center_pt[1]-0.2,
00077                                                 'filter_limit_max': face_center_pt[1]+0.2
00078                                         })
00079 
00080                                         self.zfilter.update_configuration({
00081                                                 'filter_field_name': 'z',
00082                                                 'filter_limit_min': depth-0.2,
00083                                                 'filter_limit_max': depth+0.2
00084                                         })
00085                                 except ServiceException:
00086                                         rospy.logwarn("Service exception with dynamic reconfigure")
00087                         
00088 
00089 if __name__ == '__main__':
00090         rospy.init_node('extract_head_cloud')
00091         Extract()
00092         rospy.spin()


head_pose_estimation
Author(s): Dan Lazewatsky
autogenerated on Thu Feb 11 2016 23:06:59