capture_lateral_pcl_sequence.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('zyonz_apps')
00003 import rospy
00004 import smach
00005 import smach_ros
00006 
00007 # Import the parent directory for common
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         #pprint(analysis_request)
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         #pprint(response.camera_pose)
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 # This cover the action of moving the WAM to a given pose and get the point cloud from there.
00116 # Together with PointCloud, the transform from camera to world will be retrieve and include in
00117 # storing the PCD file.
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     #sm.userdata.pcl_topic      = '/zyonz/zyonz_obtain_two_clustered_leaves/cluster_a'
00390     #sm.userdata.pcl_topic      = '/zyonz/outlier_removal/output'
00391     sm.userdata.pcl_topic      = '/zyonz/camboard/pointcloud/cloud_raw'
00392     #sm.userdata.pcl_topic      = '/zyonz/iri_filter_jump_edge/filtered_pc'
00393     #sm.userdata.pcl_topic      = '/zyonz/outlier_removal/output'
00394     sm.userdata.store_service  = '/zyonz/pointcloud_to_pcd/store_pointcloud'
00395     #sm.userdata.pose_st        = build_home_pose();
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     # Run state machine introspection server for smach viewer
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 


zyonz_apps
Author(s): sfoix
autogenerated on Fri Dec 6 2013 23:27:41