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..."
