Go to the documentation of this file.00001 import roslib; roslib.load_manifest('hai_sandbox')
00002
00003 import rospy
00004 import feature_extractor_fpfh.msg as fmsg
00005 import hrl_lib.rutils as ru
00006 from cv_bridge import CvBridge, CvBridgeError
00007 import numpy as np
00008
00009
00010 class KinectListener:
00011 def __init__(self, topic=None):
00012 if topic == None:
00013 topic = 'fpfh_hist'
00014 rate = .2
00015 self.listener = ru.GenericListener('kinect_client', fmsg.FPFHHist, topic, rate)
00016 self.bridge = CvBridge()
00017
00018 def read(self):
00019 cur_time = rospy.Time.now().to_sec()
00020 not_fresh = True
00021 while not_fresh:
00022 fpfh_hist = self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False, quiet=True)
00023 if not (fpfh_hist.header.stamp.to_sec() < cur_time):
00024 not_fresh = False
00025 else:
00026 rospy.loginfo("fpfh message time is in the past by %.2f secs"% (cur_time - fpfh_hist.header.stamp.to_sec()))
00027
00028 histogram = np.matrix(fpfh_hist.histograms).reshape((fpfh_hist.hist_npoints, 33)).T
00029 hist_points = np.matrix(fpfh_hist.hpoints3d).reshape((fpfh_hist.hist_npoints, 3)).T
00030 points3d = np.matrix(fpfh_hist.origpoints).reshape((fpfh_hist.original_npoints, 3)).T
00031 points3d = points3d[:, np.where(1-np.isnan(points3d))[1].A1]
00032 cvimage_mat = self.bridge.imgmsg_to_cv(fpfh_hist.image, 'bgr8')
00033 return {'histogram': histogram, 'hpoints3d': hist_points, 'points3d': points3d, 'image': cvimage_mat}
00034
00035