Go to the documentation of this file.00001
00002
00003 import roslib
00004 roslib.load_manifest( 'rfid_people_following' )
00005 import rospy
00006
00007 from scipy.spatial import KDTree
00008 import numpy as np, math
00009 import pickle as pkl
00010 import markers
00011 import point_cloud_utils as pcu
00012 import time
00013
00014 rospy.init_node( 'traj_pub' )
00015
00016 print 'Loading data'
00017
00018 f = open( 'trajectory_caps/resutls.pkl' , 'r' )
00019 dset = pkl.load( f )
00020 f.close()
00021
00022 captures = dset['captures']
00023
00024 def Linf_norm( arr ):
00025
00026 rv = np.max( np.abs( arr - np.array([7.0, 3.0]) ))
00027
00028 return rv
00029
00030 def filt( dist_min, dist_max ):
00031
00032 pts = np.array([]).reshape([4,0])
00033 for i in range(len(captures)):
00034 lp = load_points( captures[i][3] )
00035 if Linf_norm( lp[0:2,0] ) >= dist_min and Linf_norm( lp[0:2,0] ) <= dist_max:
00036 pts = np.column_stack([ pts, lp ])
00037 fpts_3d = np.row_stack([ pts[0], pts[1], np.zeros(pts.shape[1]) ])
00038 print fpts_3d.shape
00039 return np.matrix( fpts_3d )
00040
00041
00042 def load_points( fname ):
00043 print 'Loading %s' % fname
00044 f = open( fname, 'r' )
00045 d = pkl.load( f )
00046 f.close()
00047 return np.array(d).T
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057 if __name__ == '__main__':
00058 import optparse
00059 p = optparse.OptionParser()
00060 p.add_option('-x', action='store', type='float', dest='dist_max', default='20.0', help='max distance')
00061 p.add_option('-n', action='store', type='float', dest='dist_min', default='0.0', help='min distance')
00062 opt, args = p.parse_args()
00063
00064 pub = rospy.Publisher( 'traj_pub_pts', pcu.PointCloud )
00065
00066 ros_pts = pcu.np_points_to_ros( filt( opt.dist_min, opt.dist_max ))
00067 ros_pts.header.frame_id = '/odom_combined'
00068
00069 rate = rospy.Rate( 2 )
00070
00071 print 'Publishing PointCloud'
00072 while not rospy.is_shutdown():
00073 ros_pts.header.stamp = rospy.Time.now()
00074 pub.publish( ros_pts )
00075 rate.sleep()
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096