f.close() print 'Done. Re-run.' exit() # data will be 4xN => x, y, RSSI, positive finish (bool) # Calculate Useful Values xy = data[0:2,:] xyz = np.row_stack([ xy, np.zeros( xy.shape[1] )]) rssi = data[2] pos_read = data[3] pp = pcu.np_points_to_ros( np.matrix(xyz[:,np.where(pos_read > 0.5)[0]]) ) pp.header.frame_id = '/map' pn = pcu.np_points_to_ros( np.matrix(xyz[:,np.where(pos_read < 0.5)[0]]) ) pn.header.frame_id = '/map' rospy.init_node( 'traj_pub_node' ) time.sleep( 0.3 ) print 'PUBLISHING' pubp = rospy.Publisher( 'traj_pub_pos', PointCloud ) pubn = rospy.Publisher( 'traj_pub_neg', PointCloud ) while not rospy.is_shutdown(): pp.header.stamp = rospy.Time.now() pn.header.stamp = rospy.Time.now()
#ind = kd.query_ball_point( np.array([ 3.0, 3.0 ]), r=0.5 ) #pts = np.column_stack([ load_points( captures[i][3] ) for i in ind ]) # pts = np.column_stack([ load_points( captures[i][3] ) for i in range(len(captures)) ]) if __name__ == '__main__': import optparse p = optparse.OptionParser() p.add_option('-x', action='store', type='float', dest='dist_max', default='20.0', help='max distance') p.add_option('-n', action='store', type='float', dest='dist_min', default='0.0', help='min distance') opt, args = p.parse_args() pub = rospy.Publisher( 'traj_pub_pts', pcu.PointCloud ) # ros_pts = pcu.np_points_to_ros( np.matrix(fpts_3d) ) ros_pts = pcu.np_points_to_ros( filt( opt.dist_min, opt.dist_max )) ros_pts.header.frame_id = '/odom_combined' rate = rospy.Rate( 2 ) print 'Publishing PointCloud' while not rospy.is_shutdown(): ros_pts.header.stamp = rospy.Time.now() pub.publish( ros_pts ) rate.sleep() # arr = np.array( res ).T # kd = KDTree( arr[0:2].T ) # xy-only.
action='store', type='float', dest='dist_max', default='20.0', help='max distance') p.add_option('-n', action='store', type='float', dest='dist_min', default='0.0', help='min distance') opt, args = p.parse_args() pub = rospy.Publisher('traj_pub_pts', pcu.PointCloud) # ros_pts = pcu.np_points_to_ros( np.matrix(fpts_3d) ) ros_pts = pcu.np_points_to_ros(filt(opt.dist_min, opt.dist_max)) ros_pts.header.frame_id = '/odom_combined' rate = rospy.Rate(2) print 'Publishing PointCloud' while not rospy.is_shutdown(): ros_pts.header.stamp = rospy.Time.now() pub.publish(ros_pts) rate.sleep() # arr = np.array( res ).T # kd = KDTree( arr[0:2].T ) # xy-only. # X,Y = np.meshgrid( range(0,11), range(0,7) ) # #xy = np.row_stack([ X.flatten(), Y.flatten() ])
pkl.dump(data, f, -1) f.close() print 'Done. Re-run.' exit() # data will be 4xN => x, y, RSSI, positive finish (bool) # Calculate Useful Values xy = data[0:2, :] xyz = np.row_stack([xy, np.zeros(xy.shape[1])]) rssi = data[2] pos_read = data[3] pp = pcu.np_points_to_ros(np.matrix(xyz[:, np.where(pos_read > 0.5)[0]])) pp.header.frame_id = '/map' pn = pcu.np_points_to_ros(np.matrix(xyz[:, np.where(pos_read < 0.5)[0]])) pn.header.frame_id = '/map' rospy.init_node('traj_pub_node') time.sleep(0.3) print 'PUBLISHING' pubp = rospy.Publisher('traj_pub_pos', PointCloud) pubn = rospy.Publisher('traj_pub_neg', PointCloud) while not rospy.is_shutdown(): pp.header.stamp = rospy.Time.now() pn.header.stamp = rospy.Time.now()