00001
00002 import roslib; roslib.load_manifest('zyonz_apps')
00003 import rospy
00004 import smach
00005 import smach_ros
00006
00007
00008 import os, sys
00009 _root_dir = os.path.dirname(os.path.dirname(os.path.realpath(__file__)))
00010 sys.path.insert(0, _root_dir)
00011
00012 from smach_ros import ServiceState
00013 from geometry_msgs.msg import PoseStamped
00014 from iri_pointcloud_to_pcd.srv import StorePointcloud, StorePointcloudRequest
00015 from iri_wam_smach.sm_wam_move_from_pose import SM_WAM_MoveFromPose
00016 from iri_common_smach.st_get_pcl import GetPCL
00017 from iri_common_smach.st_transform_pose import TransformPoseStamped
00018 from iri_common_smach.st_sleep import WaitSeconds
00019 from zyonz_apps_base.move import SM_ZYONZ_GoHome
00020 from zyonz_camera_pose_around_point.srv import GetPose, GetPoseRequest
00021 from pprint import pprint
00022
00023 class SM_ZYONZ_ComputePYPose(smach.State):
00024
00025 def __init__(self, dist_m, rad_x_rot, rad_y_rot):
00026 smach.State.__init__(self, outcomes = ['success','fail'],
00027 input_keys = ['pose_st'],
00028 output_keys = ['res_pose_st'])
00029 self.dist_m = dist_m
00030 self.rad_x_rot = rad_x_rot
00031 self.rad_y_rot = rad_y_rot
00032
00033 def execute(self, userdata):
00034 handler = rospy.ServiceProxy('/zyonz/camera_pose_around_point/get_camera_pose', GetPose)
00035
00036 analysis_request = GetPoseRequest()
00037 analysis_request.view_point = userdata.pose_st
00038 analysis_request.dist_m = self.dist_m
00039 analysis_request.rad_x_rot = self.rad_x_rot
00040 analysis_request.rad_y_rot = self.rad_y_rot
00041
00042
00043
00044 try:
00045 response = handler(analysis_request)
00046 except rospy.ServiceException, e:
00047 return 'fail'
00048
00049 if (not response):
00050 return 'fail'
00051
00052 userdata.res_pose_st = response.camera_pose
00053
00054
00055
00056 return 'success'
00057
00058
00059 class StorePCDService(smach.State):
00060 def __init__(self):
00061 smach.State.__init__(self, outcomes = ['success','fail'],
00062 input_keys = ['filename','pose_st','pcl'])
00063 def execute(self, userdata):
00064 handler = rospy.ServiceProxy('/zyonz/pointcloud_to_pcd/store_pointcloud', StorePointcloud)
00065
00066 analysis_request = StorePointcloudRequest()
00067 analysis_request.file_name.data = userdata.filename
00068 analysis_request.sensor_pose = userdata.pose_st
00069 analysis_request.point_cloud = userdata.pcl
00070
00071 try:
00072 response = handler(analysis_request)
00073 except rospy.ServiceException, e:
00074 return 'fail'
00075
00076 if (not response):
00077 return 'fail'
00078
00079 return 'success'
00080
00081 class SM_ZYONZ_StorePCD():
00082 def __init__(self):
00083 pass
00084
00085 def build_sm(self):
00086 sm = smach.StateMachine(outcomes = ['succeeded','aborted'],
00087 input_keys = ['pcl_topic',
00088 'store_service',
00089 'pose_st',
00090 'target_frame',
00091 'filename'])
00092 with sm:
00093 smach.StateMachine.add('CAPTURE_PCL', GetPCL(),
00094 transitions = {'success' : 'GET_POSE',
00095 'fail' : 'aborted'},
00096 remapping = {'pcl_topic' : 'pcl_topic',
00097 'pcl_RGB' : 'sm_pcl'})
00098
00099 smach.StateMachine.add('GET_POSE', TransformPoseStamped(),
00100 transitions = {'success' : 'CALL_STORE_SERVICE',
00101 'fail' : 'aborted'},
00102 remapping = {'pose_st' : 'pose_st',
00103 'target_frame' : 'target_frame',
00104 'transformed_pose_st' : 'sm_pose_st'})
00105
00106 smach.StateMachine.add('CALL_STORE_SERVICE', StorePCDService(),
00107 transitions = {'success' : 'succeeded',
00108 'fail' : 'aborted'},
00109 remapping = {'filename' : 'filename',
00110 'pose_st' : 'sm_pose_st',
00111 'pcl' : 'sm_pcl'})
00112
00113 return sm
00114
00115
00116
00117
00118 class SM_ZYONZ_CapturePoint():
00119 def __init__(self):
00120 pass
00121
00122 def build_sm(self):
00123 sm_approching_factory = SM_WAM_MoveFromPose("/zyonz/wam_wrapper/joints_move", "/zyonz/iri_wam_camboard_ik/get_wam_ik")
00124 sm_approching = sm_approching_factory.build_sm()
00125 sm_store_factory = SM_ZYONZ_StorePCD()
00126 sm_store_pcd = sm_store_factory.build_sm()
00127
00128 sm = smach.StateMachine(outcomes = ['succeeded', 'aborted'],
00129 input_keys = ['pcl_topic', 'store_service',
00130 'dist_m','rad_x_rot','rad_y_rot','ref_pose',
00131 'pose_st_00','pose_st_01',
00132 'pose_st_02','pose_st_03',
00133 'pose_st_04','pose_st_05',
00134 'pose_st_06','pose_st_07',
00135 'filename_00','filename_01',
00136 'filename_02','filename_03',
00137 'filename_04','filename_05',
00138 'filename_06','filename_07',
00139 'target_frame'])
00140
00141 with sm:
00142 smach.StateMachine.add('COMPUTE_POSE_00', SM_ZYONZ_ComputePYPose(0.3, -3.14/2, 0.0),
00143 transitions = {'success': 'GO_TO_POSE_00',
00144 'fail' : 'aborted' },
00145 remapping = {'pose_st' : 'ref_pose',
00146 'res_pose_st' : 'pose_st_00'})
00147
00148 smach.StateMachine.add('GO_TO_POSE_00', sm_approching,
00149 transitions={'success': 'WAIT_00',
00150 'no_kinematic_solution': 'aborted',
00151 'aborted': 'aborted' },
00152 remapping = {'pose_st': 'pose_st_00'})
00153
00154 smach.StateMachine.add('WAIT_00', WaitSeconds(2),
00155 transitions = {'finish': 'STORE_PCD_00'})
00156
00157 smach.StateMachine.add('STORE_PCD_00', sm_store_pcd,
00158 transitions = {'succeeded' : 'COMPUTE_POSE_01',
00159 'aborted' : 'aborted' },
00160 remapping = {'pcl_topic' : 'pcl_topic',
00161 'store_service' : 'store_service',
00162 'pose_st' : 'pose_st_00',
00163 'target_frame' : 'target_frame',
00164 'filename' : 'filename_00'})
00165
00166 smach.StateMachine.add('COMPUTE_POSE_01', SM_ZYONZ_ComputePYPose(0.3, -3.14/2+1.57/9, 0.0),
00167 transitions = {'success': 'GO_TO_POSE_01',
00168 'fail' : 'aborted' },
00169 remapping = {'pose_st' : 'ref_pose',
00170 'res_pose_st' : 'pose_st_01'})
00171
00172 smach.StateMachine.add('GO_TO_POSE_01', sm_approching,
00173 transitions={'success': 'WAIT_01',
00174 'no_kinematic_solution': 'aborted',
00175 'aborted': 'aborted' },
00176 remapping = {'pose_st': 'pose_st_01'})
00177
00178 smach.StateMachine.add('WAIT_01', WaitSeconds(2),
00179 transitions = {'finish': 'STORE_PCD_01'})
00180
00181 smach.StateMachine.add('STORE_PCD_01', sm_store_pcd,
00182 transitions = {'succeeded': 'COMPUTE_POSE_02',
00183 'aborted': 'aborted' },
00184 remapping = {'pcl_topic' : 'pcl_topic',
00185 'store_service' : 'store_service',
00186 'pose_st' : 'pose_st_01',
00187 'target_frame' : 'target_frame',
00188 'filename' : 'filename_01'})
00189
00190 smach.StateMachine.add('COMPUTE_POSE_02', SM_ZYONZ_ComputePYPose(0.3, -3.14/2+2*(1.57/9), 0.0),
00191 transitions = {'success': 'GO_TO_POSE_02',
00192 'fail' : 'aborted' },
00193 remapping = {'pose_st' : 'ref_pose',
00194 'res_pose_st' : 'pose_st_02'})
00195
00196 smach.StateMachine.add('GO_TO_POSE_02', sm_approching,
00197 transitions={'success': 'WAIT_02',
00198 'no_kinematic_solution': 'aborted',
00199 'aborted': 'aborted' },
00200 remapping = {'pose_st': 'pose_st_02'})
00201
00202 smach.StateMachine.add('WAIT_02', WaitSeconds(2),
00203 transitions = {'finish': 'STORE_PCD_02'})
00204
00205 smach.StateMachine.add('STORE_PCD_02', sm_store_pcd,
00206 transitions = {'succeeded': 'COMPUTE_POSE_03',
00207 'aborted': 'aborted' },
00208 remapping = {'pcl_topic' : 'pcl_topic',
00209 'store_service' : 'store_service',
00210 'pose_st' : 'pose_st_02',
00211 'target_frame' : 'target_frame',
00212 'filename' : 'filename_02'})
00213
00214 smach.StateMachine.add('COMPUTE_POSE_03', SM_ZYONZ_ComputePYPose(0.3, -3.14/2+3*(1.57/9), 0.0),
00215 transitions = {'success': 'GO_TO_POSE_03',
00216 'fail' : 'aborted' },
00217 remapping = {'pose_st' : 'ref_pose',
00218 'res_pose_st' : 'pose_st_03'})
00219
00220 smach.StateMachine.add('GO_TO_POSE_03', sm_approching,
00221 transitions={'success': 'WAIT_03',
00222 'no_kinematic_solution': 'aborted',
00223 'aborted': 'aborted' },
00224 remapping = {'pose_st': 'pose_st_03'})
00225
00226 smach.StateMachine.add('WAIT_03', WaitSeconds(2),
00227 transitions = {'finish': 'STORE_PCD_03'})
00228
00229 smach.StateMachine.add('STORE_PCD_03', sm_store_pcd,
00230 transitions = {'succeeded': 'COMPUTE_POSE_04',
00231 'aborted': 'aborted' },
00232 remapping = {'pcl_topic' : 'pcl_topic',
00233 'store_service' : 'store_service',
00234 'pose_st' : 'pose_st_03',
00235 'target_frame' : 'target_frame',
00236 'filename' : 'filename_03'})
00237
00238 smach.StateMachine.add('COMPUTE_POSE_04', SM_ZYONZ_ComputePYPose(0.3, -3.14/2+4*(1.57/9), 0.0),
00239 transitions = {'success': 'GO_TO_POSE_04',
00240 'fail' : 'aborted' },
00241 remapping = {'pose_st' : 'ref_pose',
00242 'res_pose_st' : 'pose_st_04'})
00243
00244 smach.StateMachine.add('GO_TO_POSE_04', sm_approching,
00245 transitions={'success': 'WAIT_04',
00246 'no_kinematic_solution': 'aborted',
00247 'aborted': 'aborted' },
00248 remapping = {'pose_st': 'pose_st_04'})
00249
00250 smach.StateMachine.add('WAIT_04', WaitSeconds(2),
00251 transitions = {'finish': 'STORE_PCD_04'})
00252
00253 smach.StateMachine.add('STORE_PCD_04', sm_store_pcd,
00254 transitions = {'succeeded': 'COMPUTE_POSE_05',
00255 'aborted': 'aborted' },
00256 remapping = {'pcl_topic' : 'pcl_topic',
00257 'store_service' : 'store_service',
00258 'pose_st' : 'pose_st_04',
00259 'target_frame' : 'target_frame',
00260 'filename' : 'filename_04'})
00261
00262 smach.StateMachine.add('COMPUTE_POSE_05', SM_ZYONZ_ComputePYPose(0.3, -3.14/2+5*(1.57/9), 0.0),
00263 transitions = {'success': 'GO_TO_POSE_05',
00264 'fail' : 'aborted' },
00265 remapping = {'pose_st' : 'ref_pose',
00266 'res_pose_st' : 'pose_st_05'})
00267
00268 smach.StateMachine.add('GO_TO_POSE_05', sm_approching,
00269 transitions={'success': 'WAIT_05',
00270 'no_kinematic_solution': 'aborted',
00271 'aborted': 'aborted' },
00272 remapping = {'pose_st': 'pose_st_05'})
00273
00274 smach.StateMachine.add('WAIT_05', WaitSeconds(2),
00275 transitions = {'finish': 'STORE_PCD_05'})
00276
00277 smach.StateMachine.add('STORE_PCD_05', sm_store_pcd,
00278 transitions = {'succeeded': 'COMPUTE_POSE_06',
00279 'aborted': 'aborted' },
00280 remapping = {'pcl_topic' : 'pcl_topic',
00281 'store_service' : 'store_service',
00282 'pose_st' : 'pose_st_05',
00283 'target_frame' : 'target_frame',
00284 'filename' : 'filename_05'})
00285
00286 smach.StateMachine.add('COMPUTE_POSE_06', SM_ZYONZ_ComputePYPose(0.3, -3.14/2+6*(1.57/9), 0.0),
00287 transitions = {'success': 'GO_TO_POSE_06',
00288 'fail' : 'aborted' },
00289 remapping = {'pose_st' : 'ref_pose',
00290 'res_pose_st' : 'pose_st_06'})
00291
00292 smach.StateMachine.add('GO_TO_POSE_06', sm_approching,
00293 transitions={'success': 'WAIT_06',
00294 'no_kinematic_solution': 'aborted',
00295 'aborted': 'aborted' },
00296 remapping = {'pose_st': 'pose_st_06'})
00297
00298 smach.StateMachine.add('WAIT_06', WaitSeconds(2),
00299 transitions = {'finish': 'STORE_PCD_06'})
00300
00301 smach.StateMachine.add('STORE_PCD_06', sm_store_pcd,
00302 transitions = {'succeeded': 'COMPUTE_POSE_07',
00303 'aborted': 'aborted' },
00304 remapping = {'pcl_topic' : 'pcl_topic',
00305 'store_service' : 'store_service',
00306 'pose_st' : 'pose_st_06',
00307 'target_frame' : 'target_frame',
00308 'filename' : 'filename_06'})
00309
00310 smach.StateMachine.add('COMPUTE_POSE_07', SM_ZYONZ_ComputePYPose(0.3, -3.14/2+7*(1.57/9), 0.0),
00311 transitions = {'success': 'GO_TO_POSE_07',
00312 'fail' : 'aborted' },
00313 remapping = {'pose_st' : 'ref_pose',
00314 'res_pose_st' : 'pose_st_07'})
00315
00316 smach.StateMachine.add('GO_TO_POSE_07', sm_approching,
00317 transitions={'success': 'WAIT_07',
00318 'no_kinematic_solution': 'aborted',
00319 'aborted': 'aborted' },
00320 remapping = {'pose_st': 'pose_st_07'})
00321
00322 smach.StateMachine.add('WAIT_07', WaitSeconds(2),
00323 transitions = {'finish': 'STORE_PCD_07'})
00324
00325 smach.StateMachine.add('STORE_PCD_07', sm_store_pcd,
00326 transitions = {'succeeded': 'COMPUTE_POSE_08',
00327 'aborted': 'aborted' },
00328 remapping = {'pcl_topic' : 'pcl_topic',
00329 'store_service' : 'store_service',
00330 'pose_st' : 'pose_st_07',
00331 'target_frame' : 'target_frame',
00332 'filename' : 'filename_07'})
00333
00334 smach.StateMachine.add('COMPUTE_POSE_08', SM_ZYONZ_ComputePYPose(0.3, -3.14/2+8*(1.57/9), 0.0),
00335 transitions = {'success': 'GO_TO_POSE_08',
00336 'fail' : 'aborted' },
00337 remapping = {'pose_st' : 'ref_pose',
00338 'res_pose_st' : 'pose_st_08'})
00339
00340 smach.StateMachine.add('GO_TO_POSE_08', sm_approching,
00341 transitions={'success': 'WAIT_08',
00342 'no_kinematic_solution': 'aborted',
00343 'aborted': 'aborted' },
00344 remapping = {'pose_st': 'pose_st_08'})
00345
00346 smach.StateMachine.add('WAIT_08', WaitSeconds(2),
00347 transitions = {'finish': 'STORE_PCD_08'})
00348
00349 smach.StateMachine.add('STORE_PCD_08', sm_store_pcd,
00350 transitions = {'succeeded': 'COMPUTE_POSE_09',
00351 'aborted': 'aborted' },
00352 remapping = {'pcl_topic' : 'pcl_topic',
00353 'store_service' : 'store_service',
00354 'pose_st' : 'pose_st_08',
00355 'target_frame' : 'target_frame',
00356 'filename' : 'filename_08'})
00357
00358 smach.StateMachine.add('COMPUTE_POSE_09', SM_ZYONZ_ComputePYPose(0.3, -3.14/2+9*(1.57/9), 0.0),
00359 transitions = {'success': 'GO_TO_POSE_09',
00360 'fail' : 'aborted' },
00361 remapping = {'pose_st' : 'ref_pose',
00362 'res_pose_st' : 'pose_st_09'})
00363
00364 smach.StateMachine.add('GO_TO_POSE_09', sm_approching,
00365 transitions={'success': 'WAIT_09',
00366 'no_kinematic_solution': 'aborted',
00367 'aborted': 'aborted' },
00368 remapping = {'pose_st': 'pose_st_09'})
00369
00370 smach.StateMachine.add('WAIT_09', WaitSeconds(2),
00371 transitions = {'finish': 'STORE_PCD_09'})
00372
00373 smach.StateMachine.add('STORE_PCD_09', sm_store_pcd,
00374 transitions = {'succeeded': 'succeeded',
00375 'aborted': 'aborted' },
00376 remapping = {'pcl_topic' : 'pcl_topic',
00377 'store_service' : 'store_service',
00378 'pose_st' : 'pose_st_07',
00379 'target_frame' : 'target_frame',
00380 'filename' : 'filename_09'})
00381
00382 return sm
00383
00384 def construct_main_sm():
00385 sm_factory = SM_ZYONZ_CapturePoint()
00386 sm_capture = sm_factory.build_sm()
00387
00388 sm = smach.StateMachine(outcomes = ['succeeded','aborted'])
00389
00390
00391 sm.userdata.pcl_topic = '/zyonz/camboard/pointcloud/cloud_raw'
00392
00393
00394 sm.userdata.store_service = '/zyonz/pointcloud_to_pcd/store_pointcloud'
00395
00396 sm.userdata.ref_pose = PoseStamped();
00397 sm.userdata.ref_pose.header.frame_id = '/wam_link0';
00398 sm.userdata.ref_pose.pose.position.x = 0.5;
00399 sm.userdata.ref_pose.pose.position.y = 0.0;
00400 sm.userdata.ref_pose.pose.position.z = 0.0;
00401 sm.userdata.ref_pose.pose.orientation.x = 0.0;
00402 sm.userdata.ref_pose.pose.orientation.y = 0.0;
00403 sm.userdata.ref_pose.pose.orientation.z = 0.0;
00404 sm.userdata.ref_pose.pose.orientation.w = 1.0;
00405 sm.userdata.pose_st_00 = PoseStamped();
00406 sm.userdata.pose_st_00.header.frame_id = '/wam_link0';
00407 sm.userdata.pose_st_00.pose.position.x = 0.581;
00408 sm.userdata.pose_st_00.pose.position.y = 0.267;
00409 sm.userdata.pose_st_00.pose.position.z = -0.144;
00410 sm.userdata.pose_st_00.pose.orientation.x = 0.442;
00411 sm.userdata.pose_st_00.pose.orientation.y = -0.525;
00412 sm.userdata.pose_st_00.pose.orientation.z = 0.545;
00413 sm.userdata.pose_st_00.pose.orientation.w = 0.481;
00414 sm.userdata.pose_st_01 = PoseStamped();
00415 sm.userdata.pose_st_01.header.frame_id = '/wam_link0';
00416 sm.userdata.pose_st_01.pose.position.x = 0.581;
00417 sm.userdata.pose_st_01.pose.position.y = 0.258;
00418 sm.userdata.pose_st_01.pose.position.z = -0.096;
00419 sm.userdata.pose_st_01.pose.orientation.x = -0.515;
00420 sm.userdata.pose_st_01.pose.orientation.y = 0.551;
00421 sm.userdata.pose_st_01.pose.orientation.z = -0.506;
00422 sm.userdata.pose_st_01.pose.orientation.w = -0.419;
00423 sm.userdata.pose_st_02 = PoseStamped();
00424 sm.userdata.pose_st_02.header.frame_id = '/wam_link0';
00425 sm.userdata.pose_st_02.pose.position.x = 0.583;
00426 sm.userdata.pose_st_02.pose.position.y = 0.208;
00427 sm.userdata.pose_st_02.pose.position.z = -0.042;
00428 sm.userdata.pose_st_02.pose.orientation.x = -0.572;
00429 sm.userdata.pose_st_02.pose.orientation.y = 0.625;
00430 sm.userdata.pose_st_02.pose.orientation.z = -0.412;
00431 sm.userdata.pose_st_02.pose.orientation.w = -0.334;
00432 sm.userdata.pose_st_03 = PoseStamped();
00433 sm.userdata.pose_st_03.header.frame_id = '/wam_link0';
00434 sm.userdata.pose_st_03.pose.position.x = 0.569;
00435 sm.userdata.pose_st_03.pose.position.y = 0.175;
00436 sm.userdata.pose_st_03.pose.position.z = 0.020;
00437 sm.userdata.pose_st_03.pose.orientation.x = -0.572;
00438 sm.userdata.pose_st_03.pose.orientation.y = 0.686;
00439 sm.userdata.pose_st_03.pose.orientation.z = -0.341;
00440 sm.userdata.pose_st_03.pose.orientation.w = -0.294;
00441 sm.userdata.pose_st_04 = PoseStamped();
00442 sm.userdata.pose_st_04.header.frame_id = '/wam_link0';
00443 sm.userdata.pose_st_04.pose.position.x = 0.559;
00444 sm.userdata.pose_st_04.pose.position.y = 0.123;
00445 sm.userdata.pose_st_04.pose.position.z = 0.053;
00446 sm.userdata.pose_st_04.pose.orientation.x = -0.594;
00447 sm.userdata.pose_st_04.pose.orientation.y = 0.725;
00448 sm.userdata.pose_st_04.pose.orientation.z = -0.270;
00449 sm.userdata.pose_st_04.pose.orientation.w = -0.221;
00450 sm.userdata.pose_st_05 = PoseStamped();
00451 sm.userdata.pose_st_05.header.frame_id = '/wam_link0';
00452 sm.userdata.pose_st_05.pose.position.x = 0.557;
00453 sm.userdata.pose_st_05.pose.position.y = 0.077;
00454 sm.userdata.pose_st_05.pose.position.z = 0.064;
00455 sm.userdata.pose_st_05.pose.orientation.x = -0.640;
00456 sm.userdata.pose_st_05.pose.orientation.y = 0.727;
00457 sm.userdata.pose_st_05.pose.orientation.z = -0.174;
00458 sm.userdata.pose_st_05.pose.orientation.w = -0.180;
00459 sm.userdata.pose_st_06 = PoseStamped();
00460 sm.userdata.pose_st_06.header.frame_id = '/wam_link0';
00461 sm.userdata.pose_st_06.pose.position.x = 0.545;
00462 sm.userdata.pose_st_06.pose.position.y = 0.028;
00463 sm.userdata.pose_st_06.pose.position.z = 0.078;
00464 sm.userdata.pose_st_06.pose.orientation.x = -0.638;
00465 sm.userdata.pose_st_06.pose.orientation.y = 0.761;
00466 sm.userdata.pose_st_06.pose.orientation.z = -0.085;
00467 sm.userdata.pose_st_06.pose.orientation.w = -0.081;
00468 sm.userdata.pose_st_07 = PoseStamped();
00469 sm.userdata.pose_st_07.header.frame_id = '/wam_link0';
00470 sm.userdata.pose_st_07.pose.position.x = 0.547;
00471 sm.userdata.pose_st_07.pose.position.y = -0.023;
00472 sm.userdata.pose_st_07.pose.position.z = 0.099;
00473 sm.userdata.pose_st_07.pose.orientation.x = -0.644;
00474 sm.userdata.pose_st_07.pose.orientation.y = 0.764;
00475 sm.userdata.pose_st_07.pose.orientation.z = 0.001;
00476 sm.userdata.pose_st_07.pose.orientation.w = -0.048;
00477 sm.userdata.filename_00 = '/tmp/plant00.pcd'
00478 sm.userdata.filename_01 = '/tmp/plant01.pcd'
00479 sm.userdata.filename_02 = '/tmp/plant02.pcd'
00480 sm.userdata.filename_03 = '/tmp/plant03.pcd'
00481 sm.userdata.filename_04 = '/tmp/plant04.pcd'
00482 sm.userdata.filename_05 = '/tmp/plant05.pcd'
00483 sm.userdata.filename_06 = '/tmp/plant06.pcd'
00484 sm.userdata.filename_07 = '/tmp/plant07.pcd'
00485 sm.userdata.filename_08 = '/tmp/plant08.pcd'
00486 sm.userdata.filename_09 = '/tmp/plant09.pcd'
00487 sm.userdata.target_frame = '/wam_link0'
00488
00489 with sm:
00490 smach.StateMachine.add('CAPTURE_APP', sm_capture)
00491 return sm
00492
00493 def main():
00494 rospy.init_node("base_probing_sm")
00495 sm_main = construct_main_sm()
00496
00497
00498 sis = smach_ros.IntrospectionServer('zyonz_capture_sequential_pointcloud', sm_main,
00499 '/zyonz_capture_sequential_pointcloud')
00500
00501 sis.start()
00502
00503 outcome = sm_main.execute()
00504
00505 rospy.spin()
00506 sis.stop()
00507
00508 if __name__ == "__main__":
00509 main()
00510