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('laser_camera_segmentation')
00032 import rospy
00033
00034
00035 import laser_camera_segmentation.processor as processor
00036 import laser_camera_segmentation.configuration as configuration
00037
00038
00039 from labeling.label_object import label_object
00040 from labeling.scan_dataset import scan_dataset
00041
00042 import numpy as np
00043 import opencv as cv
00044
00045
00046 from laser_camera_segmentation.ROS_interface_helper_functions import *
00047
00048 from laser_camera_segmentation.srv._Segmentation import *
00049 import sensor_msgs
00050
00051
00052
00053 def get_test_data():
00054 cfg = configuration.configuration('../data/ROS_test_client/', 'dummyScanner')
00055 pc = processor.processor(cfg)
00056 pc.scan_dataset = scan_dataset()
00057 pc.scan_dataset.image_artag_filename = ''
00058 pc.scan_dataset.table_plane_translation = np.matrix([0,0,0]).T
00059
00060 pc.scan_dataset.ground_plane_translation = np.matrix([0,0,1.32]).T
00061 pc.scan_dataset.ground_plane_rotation = ''
00062
00063 pc.scan_dataset.is_labeled = True
00064 pc.scan_dataset.id = 'ROStest'
00065 pc.load_raw_data(pc.scan_dataset.id)
00066 image_size = cv.cvGetSize(pc.img)
00067
00068
00069
00070 (pc.pts3d, pc.scan_indices, pc.intensities) = pc.create_pointcloud(pc.laserscans, True, True)
00071
00072
00073 polygon = label_object()
00074 polygon.set_label('surface')
00075 width_min = image_size.width * 0.4
00076 width_max = image_size.width * 0.95
00077 height_min = image_size.height * 0.3
00078 height_max = image_size.height * 0.95
00079 polygon.add_point((width_min,height_min))
00080 polygon.add_point((width_min,height_max))
00081 polygon.add_point((width_max,height_max))
00082 polygon.add_point((width_max,height_min))
00083
00084 return pc.pts3d, pc.intensities, None, pc.img, pc.image_angle, polygon
00085
00086
00087 def test_segmentation():
00088 rospy.wait_for_service('segment_pointcloud')
00089 try:
00090 pts3d, intensities, labels, image, image_angle, polygon = get_test_data()
00091 ROS_pointcloud = convert_pointcloud_to_ROS(pts3d, intensities, labels)
00092 ROS_image = convert_cvimage_to_ROS(image)
00093 imageSize = cv.cvGetSize(image)
00094
00095 ROS_polygon = convert_polygon_to_ROS(polygon)
00096
00097
00098 segment_pointcloud = rospy.ServiceProxy('segment_pointcloud', Segmentation)
00099 request = SegmentationRequest()
00100 request.imageWidth = 41
00101 request.pointcloud = ROS_pointcloud
00102 request.image = ROS_image
00103 request.imageWidth = imageSize.width
00104 request.imageHeight = imageSize.height
00105 request.imageAngle = image_angle
00106 request.regionOfInterest2D = ROS_polygon
00107 request.laserHeightAboveGround = 1.32
00108 request.numberOfPointsToClassify = 1000
00109
00110 response = segment_pointcloud(request)
00111
00112 pts3d_bound, intensities, labels = convert_ROS_pointcloud_to_pointcloud(response.pointcloud)
00113 cfg = configuration.configuration('../data/ROS_test_client/', 'dummyScanner')
00114 pc = processor.processor(cfg)
00115
00116 pc.scan_dataset = scan_dataset()
00117 pc.scan_dataset.image_artag_filename = ''
00118 pc.scan_dataset.table_plane_translation = np.matrix([0,0,0]).T
00119
00120 pc.scan_dataset.ground_plane_translation = np.matrix([0,0,request.laserHeightAboveGround]).T
00121 pc.scan_dataset.ground_plane_rotation = ''
00122 pc.scan_dataset.is_labeled = True
00123 pc.scan_dataset.id = 'ROStest'
00124 pc.image_angle = ''
00125 pc.pts3d_bound = pts3d_bound
00126 pc.map_polys = labels
00127 pc.scan_dataset.ground_plane_normal = np.matrix([0.,0.,1.]).T
00128
00129 from enthought.mayavi import mlab
00130 pc.display_3d('labels')
00131 mlab.show()
00132
00133
00134 return response
00135 except rospy.ServiceException, e:
00136 print "Service call failed: %s"%e
00137
00138
00139
00140
00141 if __name__ == "__main__":
00142 rospy.init_node('segmentation_client')
00143
00144
00145
00146
00147
00148
00149
00150 print 'segmentation_client'
00151 test_segmentation()