def generate_sphere(): pts = 4e3 theta = np.random.rand(pts) * 2 * pi phi = np.random.rand(pts) * pi rho = 1 * np.ones(len(theta)) x, y, z = SphereToCart(rho, theta, phi) pts = np.matrix(np.row_stack((x, y, z))) return pcu.np_points_to_ros(pts)
def generate_sphere(): pts = 4e3 theta = np.random.rand(pts) * 2 * pi phi = np.random.rand(pts) * pi rho = 1 * np.ones(len(theta)) x, y, z = SphereToCart(rho, theta, phi) pts = np.matrix(np.row_stack((x, y, z))) return pcu.np_points_to_ros(pts)
if sphere_flag or pc_fname != None: rospy.init_node('point_cloud_tester', anonymous=True) pub = rospy.Publisher("tilt_laser_cloud", PointCloud) rospy.Subscriber("cloud_normals", PointCloud, normals_cb) rospy.Subscriber("cloud_downsampled", PointCloud, downsample_cb) time.sleep(1) if sphere_flag: pc = generate_sphere() if pc_fname != None: pts = ut.load_pickle(pc_fname) print 'before np_points_to_ros' t0 = time.time() pc = pcu.np_points_to_ros(pts) t1 = time.time() print 'time to go from numpy to ros:', t1 - t0 t0 = time.time() pcu.ros_pointcloud_to_np(pc) t1 = time.time() print 'time to go from ros to numpy:', t1 - t0 pub.publish(pc) rospy.spin() if plot_flag: if fname == None: print 'Please give a pkl file for plotting (-f option)' print 'Exiting...'
if sphere_flag or pc_fname != None: rospy.init_node("point_cloud_tester", anonymous=True) pub = rospy.Publisher("tilt_laser_cloud", PointCloud) rospy.Subscriber("cloud_normals", PointCloud, normals_cb) rospy.Subscriber("cloud_downsampled", PointCloud, downsample_cb) time.sleep(1) if sphere_flag: pc = generate_sphere() if pc_fname != None: pts = ut.load_pickle(pc_fname) print "before np_points_to_ros" t0 = time.time() pc = pcu.np_points_to_ros(pts) t1 = time.time() print "time to go from numpy to ros:", t1 - t0 t0 = time.time() pcu.ros_pointcloud_to_np(pc) t1 = time.time() print "time to go from ros to numpy:", t1 - t0 pub.publish(pc) rospy.spin() if plot_flag: if fname == None: print "Please give a pkl file for plotting (-f option)" print "Exiting..."