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()
