00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 import roslib; roslib.load_manifest('point_cloud_ros')
00034
00035 import rospy
00036
00037 from sensor_msgs.msg import PointCloud
00038 from geometry_msgs.msg import Point32
00039
00040 import numpy as np, math
00041 from numpy import pi
00042 import sys, os, optparse, time
00043 import copy
00044
00045 import hrl_lib.util as ut
00046 import point_cloud_ros.point_cloud_utils as pcu
00047
00048 from enthought.mayavi import mlab
00049
00050
00051 def SphereToCart(rho, theta, phi):
00052 x = rho * np.sin(phi) * np.cos(theta)
00053 y = rho * np.sin(phi) * np.sin(theta)
00054 z = rho * np.cos(phi)
00055 return (x,y,z)
00056
00057 def generate_sphere():
00058 pts = 4e3
00059 theta = np.random.rand(pts) * 2*pi
00060 phi = np.random.rand(pts) * pi
00061 rho = 1*np.ones(len(theta))
00062
00063 x,y,z = SphereToCart(rho,theta,phi)
00064
00065 pts = np.matrix(np.row_stack((x,y,z)))
00066 return pcu.np_points_to_ros(pts)
00067
00068 def plot_cloud(pts):
00069 x = pts[0,:].A1
00070 y = pts[1,:].A1
00071 z = pts[2,:].A1
00072 mlab.points3d(x,y,z,mode='point')
00073 mlab.show()
00074
00075 def plot_normals(pts,normals,curvature=None):
00076
00077 x = pts[0,:].A1
00078 y = pts[1,:].A1
00079 z = pts[2,:].A1
00080
00081 u = normals[0,:].A1
00082 v = normals[1,:].A1
00083 w = normals[2,:].A1
00084
00085 if curvature != None:
00086
00087 mlab.points3d(x,y,z,curvature,mode='sphere',scale_factor=0.1,mask_points=1)
00088 mlab.colorbar()
00089 else:
00090 mlab.points3d(x,y,z,mode='point')
00091 mlab.quiver3d(x,y,z,u,v,w,mask_points=16,scale_factor=0.1)
00092
00093 mlab.show()
00094
00095 def downsample_cb(cloud_down):
00096 print 'downsample_cb got called.'
00097 pts = ros_pts_to_np(cloud_down.pts)
00098 x = pts[0,:].A1
00099 y = pts[1,:].A1
00100 z = pts[2,:].A1
00101 mlab.points3d(x,y,z,mode='point')
00102 mlab.show()
00103
00104 def normals_cb(normals_cloud):
00105 print 'normals_cb got called.'
00106 d = {}
00107 t0 = time.time()
00108 pts = ros_pts_to_np(normals_cloud.pts)
00109 t1 = time.time()
00110 print 'time to go from ROS point cloud to np matrx:', t1-t0
00111 d['pts'] = pts
00112
00113 if normals_cloud.chan[0].name != 'nx':
00114 print '################################################################################'
00115 print 'synthetic_point_clouds.normals_cloud: DANGER DANGER normals_cloud.chan[0] is NOT nx, it is:', normals_cloud.chan[0].name
00116 print 'Exiting...'
00117 print '################################################################################'
00118 sys.exit()
00119
00120 normals_list = []
00121 for i in range(3):
00122 normals_list.append(normals_cloud.chan[i].vals)
00123
00124 d['normals'] = np.matrix(normals_list)
00125 d['curvature'] = normals_cloud.chan[3].vals
00126
00127 print 'd[\'pts\'].shape:', d['pts'].shape
00128 print 'd[\'normals\'].shape:', d['normals'].shape
00129 ut.save_pickle(d, 'normals_cloud_'+ut.formatted_time()+'.pkl')
00130
00131
00132
00133
00134 if __name__ == '__main__':
00135 p = optparse.OptionParser()
00136 p.add_option('--sphere', action='store_true', dest='sphere',
00137 help='sample a sphere and publish the point cloud')
00138 p.add_option('--plot', action='store_true', dest='plot',
00139 help='plot the result')
00140 p.add_option('-f', action='store', type='string',dest='fname',
00141 default=None, help='pkl file with the normals.')
00142 p.add_option('--pc', action='store', type='string',dest='pc_fname',
00143 default=None, help='pkl file with 3xN numpy matrix (numpy point cloud).')
00144
00145 opt, args = p.parse_args()
00146 sphere_flag = opt.sphere
00147 plot_flag = opt.plot
00148 fname = opt.fname
00149 pc_fname = opt.pc_fname
00150
00151
00152 if sphere_flag or pc_fname!=None:
00153 rospy.init_node('point_cloud_tester', anonymous=True)
00154 pub = rospy.Publisher("tilt_laser_cloud", PointCloud)
00155 rospy.Subscriber("cloud_normals", PointCloud, normals_cb)
00156 rospy.Subscriber("cloud_downsampled", PointCloud, downsample_cb)
00157
00158 time.sleep(1)
00159
00160 if sphere_flag:
00161 pc = generate_sphere()
00162
00163 if pc_fname != None:
00164 pts = ut.load_pickle(pc_fname)
00165 print 'before np_points_to_ros'
00166 t0 = time.time()
00167 pc = pcu.np_points_to_ros(pts)
00168 t1 = time.time()
00169 print 'time to go from numpy to ros:', t1-t0
00170
00171 t0 = time.time()
00172 pcu.ros_pointcloud_to_np(pc)
00173 t1 = time.time()
00174 print 'time to go from ros to numpy:', t1-t0
00175
00176
00177 pub.publish(pc)
00178 rospy.spin()
00179
00180
00181 if plot_flag:
00182 if fname == None:
00183 print 'Please give a pkl file for plotting (-f option)'
00184 print 'Exiting...'
00185 sys.exit()
00186
00187 d = ut.load_pickle(fname)
00188 plot_normals(d['pts'],d['normals'],d['curvature'])
00189
00190