view_trajectories.py
Go to the documentation of this file.
00001 #!/usr/bin/python
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     # arr is (2,)
00026     rv = np.max( np.abs( arr - np.array([7.0, 3.0]) ))
00027     #print 'arr: ', arr, ' rv: ', rv
00028     return rv
00029 
00030 def filt( dist_min, dist_max ):
00031     # pts = 2xN array
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  # 4xN
00048 
00049 # kd = KDTree( np.array([ [x,y] for x,y,ang,fname in captures ]))
00050 
00051 #ind = kd.query_ball_point( np.array([ 3.0, 3.0 ]), r=0.5 )
00052 #pts = np.column_stack([ load_points( captures[i][3] ) for i in ind ])
00053 
00054 # pts = np.column_stack([ load_points( captures[i][3] ) for i in range(len(captures)) ])
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     # ros_pts = pcu.np_points_to_ros( np.matrix(fpts_3d) )
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 #     arr = np.array( res ).T
00080 #     kd = KDTree( arr[0:2].T ) # xy-only.
00081 
00082 #     X,Y = np.meshgrid( range(0,11), range(0,7) )
00083 #     #xy = np.row_stack([ X.flatten(), Y.flatten() ])
00084 #     Z = np.zeros( X.shape )
00085 
00086 #     for i in xrange( X.shape[0] ):
00087 #         for j in xrange( X.shape[1] ):
00088 #             ind = kd.query_ball_point( np.array([ X[i,j], Y[i,j] ]), r=0.5 )
00089 #             if ind:
00090 #                 Z[i,j] = np.mean( arr[3,ind] )
00091 #             else:
00092 #                 #print i,j
00093 #                 Z[i,j] = np.nan
00094 
00095 
00096 


rfid_people_following
Author(s): Travis Deyle (Healthcare Robotics Lab, Georgia Tech)
autogenerated on Wed Nov 27 2013 11:38:30