Go to the documentation of this file.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 import roslib; roslib.load_manifest('pr2_clutter_helper')
00032 import rospy
00033 from std_msgs.msg import String
00034 from sensor_msgs.msg import PointCloud
00035 from sensor_msgs.msg import ChannelFloat32
00036 from geometry_msgs.msg import Point32
00037 from random import *
00038 from pr2_lcs_helper.srv import *
00039
00040 '''
00041 client to test broadcaster.
00042
00043 NOT FINISHED YET, have not tested -- hope to soon!
00044
00045 '''
00046
00047
00048 def local_rand_cloud():
00049
00050
00051 ROS_pointcloud = PointCloud()
00052 x = random()*5
00053
00054
00055
00056 intensity_channel = ChannelFloat32()
00057 intensity_channel.name = 'intensities'
00058 for i in range(3000):
00059 intensity_channel.values += [random()]
00060
00061 for i in range(3000):
00062 ROS_pointcloud.points += [Point32(random()*5-x,random(), intensity_channel.values[i])]
00063
00064 rgb_channel = ChannelFloat32()
00065 rgb_channel.name = 'rgb'
00066 rgb_channel.values = normList(intensity_channel.values, 0xFF)
00067
00068
00069 r, g, b = [], [], []
00070 for pt in ROS_pointcloud.points:
00071 b += [pt.y];
00072 r += [pt.z];
00073 g += [0];
00074 r_channel = ChannelFloat32(name='r', values = normList(r) )
00075 g_channel = ChannelFloat32(name='g', values = normList(g) )
00076 b_channel = ChannelFloat32(name='b', values = b)
00077
00078 ROS_pointcloud.channels = (intensity_channel, rgb_channel, r_channel, g_channel, b_channel)
00079 return ROS_pointcloud
00080
00081 def normList(L, normalizeTo=1):
00082 '''normalize values of a list to make its max = normalizeTo'''
00083 vMax = max(L)
00084 if vMax == 0: vMax = 1;
00085 return [ x/(vMax*1.0)*normalizeTo for x in L]
00086
00087 def labeled_cloud_pub_client(cloud, frame):
00088 rospy.wait_for_service('pub_cloud_service')
00089 try:
00090 service = rospy.ServiceProxy('pub_cloud_service', CloudPub)
00091 cloud.header = rospy.Header(frame_id=frame)
00092
00093 resp = service(cloud, frame)
00094 return resp.success
00095 except rospy.ServiceException, e:
00096 print "Service call failed: %s"%e
00097
00098
00099 if __name__== "__main__":
00100 print "creating local random pointcloud"
00101
00102
00103 cloud = local_rand_cloud()
00104
00105 print "sending cloud, receiving success/fail"
00106
00107 frame_id = 'my_frame'
00108 success = labeled_cloud_pub_client(cloud, frame_id)
00109 print "status of success: ", success
00110 n =1
00111 print 'loop'
00112 while True:
00113 cloud = local_rand_cloud()
00114 rospy.sleep(.1)
00115 success = labeled_cloud_pub_client(cloud, frame_id)
00116 print "status of success: ", success, n
00117 n+=1
00118
00119
00120
00121
00122
00123