capture_sequential_pointcloud.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('zyonz_apps')
import rospy
import smach
import smach_ros

# Import the parent directory for common
import os, sys
_root_dir = os.path.dirname(os.path.dirname(os.path.realpath(__file__)))
sys.path.insert(0, _root_dir)

from smach_ros import ServiceState
from geometry_msgs.msg                   import PoseStamped
from iri_pointcloud_to_pcd.srv           import StorePointcloud, StorePointcloudRequest
from iri_wam_smach.sm_wam_move_from_pose import SM_WAM_MoveFromPose
from iri_common_smach.st_get_pcl         import GetPCL
from iri_common_smach.st_transform_pose  import TransformPoseStamped
from iri_common_smach.st_sleep           import WaitSeconds
from zyonz_apps_base.move                import SM_ZYONZ_GoHome

class StorePCDService(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['success','fail'],
                                   input_keys  = ['filename','pose_st','pcl'])
    def execute(self, userdata):
        handler = rospy.ServiceProxy('/zyonz/pointcloud_to_pcd/store_pointcloud', StorePointcloud)

        analysis_request = StorePointcloudRequest()
        analysis_request.file_name.data   = userdata.filename
        analysis_request.sensor_pose = userdata.pose_st
        analysis_request.point_cloud = userdata.pcl

        try:
            response = handler(analysis_request)
        except rospy.ServiceException, e:
            return 'fail'

        if (not response):
            return 'fail'
        
        return 'success'

class SM_ZYONZ_StorePCD():
    def __init__(self):
        pass

    def build_sm(self):
        sm = smach.StateMachine(outcomes   = ['succeeded','aborted'],
                                input_keys = ['pcl_topic',
                                              'store_service',
                                              'pose_st',
                                              'target_frame',
                                              'filename'])
        with sm:
            smach.StateMachine.add('CAPTURE_PCL',  GetPCL(),
                            transitions = {'success' : 'GET_POSE',
                                           'fail'    : 'aborted'},
                            remapping   = {'pcl_topic' : 'pcl_topic',
                                           'pcl_RGB'   : 'sm_pcl'})

            smach.StateMachine.add('GET_POSE', TransformPoseStamped(),
                            transitions = {'success' : 'CALL_STORE_SERVICE',
                                           'fail'    : 'aborted'},
                            remapping   = {'pose_st'             : 'pose_st',
                                           'target_frame'        : 'target_frame',
                                           'transformed_pose_st' : 'sm_pose_st'})

            smach.StateMachine.add('CALL_STORE_SERVICE', StorePCDService(),
                            transitions = {'success' : 'succeeded',
                                           'fail' : 'aborted'},
                            remapping   = {'filename' : 'filename',
                                           'pose_st'  : 'sm_pose_st',
                                           'pcl'      : 'sm_pcl'})

        return sm

# This cover the action of moving the WAM to a given pose and get the point cloud from there.
# Together with PointCloud, the transform from camera to world will be retrieve and include in
# storing the PCD file.
class SM_ZYONZ_CapturePoint():
    def __init__(self):
        pass

    def build_sm(self):
        sm_approching_factory = SM_WAM_MoveFromPose("/zyonz/wam_wrapper/joints_move", "/zyonz/iri_wam_camboard_ik/get_wam_ik")
        sm_approching         = sm_approching_factory.build_sm()
        sm_store_factory      = SM_ZYONZ_StorePCD()
        sm_store_pcd          = sm_store_factory.build_sm()

        sm = smach.StateMachine(outcomes   = ['succeeded', 'aborted'],
                                input_keys = ['pcl_topic', 'store_service',
                                              'pose_st_00','pose_st_01',
                                              'pose_st_02','pose_st_03',
                                              'pose_st_04','pose_st_05',
                                              'pose_st_06','pose_st_07',
                                              'filename_00','filename_01',
                                              'filename_02','filename_03',
                                              'filename_04','filename_05',
                                              'filename_06','filename_07',
                                              'target_frame'])

        with sm:
           smach.StateMachine.add('GO_TO_POSE_00', sm_approching,
                           transitions={'success': 'STORE_PCD_00',
                                        'no_kinematic_solution': 'aborted',
                                        'aborted': 'aborted' },
                           remapping = {'pose_st': 'pose_st_00'})

           smach.StateMachine.add('STORE_PCD_00', sm_store_pcd,
                          transitions = {'succeeded' : 'GO_TO_POSE_01',
                                         'aborted'   : 'aborted' },
                          remapping   = {'pcl_topic'     : 'pcl_topic',
                                         'store_service' : 'store_service',
                                         'pose_st'       : 'pose_st_00',
                                         'target_frame'  : 'target_frame',
                                         'filename'      : 'filename_00'})

           smach.StateMachine.add('GO_TO_POSE_01', sm_approching,
                           transitions={'success': 'STORE_PCD_01',
                                        'no_kinematic_solution': 'aborted',
                                        'aborted': 'aborted' },
                           remapping = {'pose_st': 'pose_st_01'})

           smach.StateMachine.add('STORE_PCD_01', sm_store_pcd,
                          transitions = {'succeeded': 'GO_TO_POSE_02',
                                         'aborted': 'aborted' },
                          remapping   = {'pcl_topic'     : 'pcl_topic',
                                         'store_service' : 'store_service',
                                         'pose_st'       : 'pose_st_01',
                                         'target_frame'  : 'target_frame',
                                         'filename'      : 'filename_01'})

           smach.StateMachine.add('GO_TO_POSE_02', sm_approching,
                           transitions={'success': 'STORE_PCD_02',
                                        'no_kinematic_solution': 'aborted',
                                        'aborted': 'aborted' },
                           remapping = {'pose_st': 'pose_st_02'})

           smach.StateMachine.add('STORE_PCD_02', sm_store_pcd,
                          transitions = {'succeeded': 'GO_TO_POSE_03',
                                         'aborted': 'aborted' },
                          remapping   = {'pcl_topic'     : 'pcl_topic',
                                         'store_service' : 'store_service',
                                         'pose_st'       : 'pose_st_02',
                                         'target_frame'  : 'target_frame',
                                         'filename'      : 'filename_02'})

           smach.StateMachine.add('GO_TO_POSE_03', sm_approching,
                           transitions={'success': 'STORE_PCD_03',
                                        'no_kinematic_solution': 'aborted',
                                        'aborted': 'aborted' },
                           remapping = {'pose_st': 'pose_st_03'})

           smach.StateMachine.add('STORE_PCD_03', sm_store_pcd,
                          transitions = {'succeeded': 'GO_TO_POSE_04',
                                         'aborted': 'aborted' },
                          remapping   = {'pcl_topic'     : 'pcl_topic',
                                         'store_service' : 'store_service',
                                         'pose_st'       : 'pose_st_03',
                                         'target_frame'  : 'target_frame',
                                         'filename'      : 'filename_03'})

           smach.StateMachine.add('GO_TO_POSE_04', sm_approching,
                           transitions={'success': 'STORE_PCD_04',
                                        'no_kinematic_solution': 'aborted',
                                        'aborted': 'aborted' },
                           remapping = {'pose_st': 'pose_st_04'})

           smach.StateMachine.add('STORE_PCD_04', sm_store_pcd,
                          transitions = {'succeeded': 'GO_TO_POSE_05',
                                         'aborted': 'aborted' },
                          remapping   = {'pcl_topic'     : 'pcl_topic',
                                         'store_service' : 'store_service',
                                         'pose_st'       : 'pose_st_04',
                                         'target_frame'  : 'target_frame',
                                         'filename'      : 'filename_04'})

           smach.StateMachine.add('GO_TO_POSE_05', sm_approching,
                           transitions={'success': 'STORE_PCD_05',
                                        'no_kinematic_solution': 'aborted',
                                        'aborted': 'aborted' },
                           remapping = {'pose_st': 'pose_st_05'})

           smach.StateMachine.add('STORE_PCD_05', sm_store_pcd,
                          transitions = {'succeeded': 'GO_TO_POSE_06',
                                         'aborted': 'aborted' },
                          remapping   = {'pcl_topic'     : 'pcl_topic',
                                         'store_service' : 'store_service',
                                         'pose_st'       : 'pose_st_05',
                                         'target_frame'  : 'target_frame',
                                         'filename'      : 'filename_05'})

           smach.StateMachine.add('GO_TO_POSE_06', sm_approching,
                           transitions={'success': 'STORE_PCD_06',
                                        'no_kinematic_solution': 'aborted',
                                        'aborted': 'aborted' },
                           remapping = {'pose_st': 'pose_st_06'})

           smach.StateMachine.add('STORE_PCD_06', sm_store_pcd,
                          transitions = {'succeeded': 'GO_TO_POSE_07',
                                         'aborted': 'aborted' },
                          remapping   = {'pcl_topic'     : 'pcl_topic',
                                         'store_service' : 'store_service',
                                         'pose_st'       : 'pose_st_06',
                                         'target_frame'  : 'target_frame',
                                         'filename'      : 'filename_06'})

           smach.StateMachine.add('GO_TO_POSE_07', sm_approching,
                           transitions={'success': 'STORE_PCD_07',
                                        'no_kinematic_solution': 'aborted',
                                        'aborted': 'aborted' },
                           remapping = {'pose_st': 'pose_st_07'})

           smach.StateMachine.add('STORE_PCD_07', sm_store_pcd,
                          transitions = {'succeeded': 'succeeded',
                                         'aborted': 'aborted' },
                          remapping   = {'pcl_topic'     : 'pcl_topic',
                                         'store_service' : 'store_service',
                                         'pose_st'       : 'pose_st_07',
                                         'target_frame'  : 'target_frame',
                                         'filename'      : 'filename_07'})

        return sm

def construct_main_sm():
    sm_factory = SM_ZYONZ_CapturePoint()
    sm_capture = sm_factory.build_sm()

    sm = smach.StateMachine(outcomes = ['succeeded','aborted'])
    #sm.userdata.pcl_topic      = '/zyonz/zyonz_obtain_two_clustered_leaves/cluster_a'
    sm.userdata.pcl_topic      = '/zyonz/outlier_removal/output'
    sm.userdata.store_service  = '/zyonz/pointcloud_to_pcd/store_pointcloud'
    #sm.userdata.pose_st        = build_home_pose();
    sm.userdata.pose_st_00 = PoseStamped();
    sm.userdata.pose_st_00.header.frame_id = '/wam_link0';
    sm.userdata.pose_st_00.pose.position.x =  0.778;
    sm.userdata.pose_st_00.pose.position.y = -0.165;
    sm.userdata.pose_st_00.pose.position.z =  0.350;
    sm.userdata.pose_st_00.pose.orientation.x =  0.822;
    sm.userdata.pose_st_00.pose.orientation.y = -0.532;
    sm.userdata.pose_st_00.pose.orientation.z =  0.114;
    sm.userdata.pose_st_00.pose.orientation.w = -0.171;
    sm.userdata.pose_st_01 = PoseStamped();
    sm.userdata.pose_st_01.header.frame_id = '/wam_link0';
    sm.userdata.pose_st_01.pose.position.x =  0.742;
    sm.userdata.pose_st_01.pose.position.y = -0.224;
    sm.userdata.pose_st_01.pose.position.z =  0.329;
    sm.userdata.pose_st_01.pose.orientation.x =  0.818;
    sm.userdata.pose_st_01.pose.orientation.y = -0.538;
    sm.userdata.pose_st_01.pose.orientation.z =  0.097;
    sm.userdata.pose_st_01.pose.orientation.w = -0.176;
    sm.userdata.pose_st_02 = PoseStamped();
    sm.userdata.pose_st_02.header.frame_id = '/wam_link0';
    sm.userdata.pose_st_02.pose.position.x =  0.686;
    sm.userdata.pose_st_02.pose.position.y = -0.267;
    sm.userdata.pose_st_02.pose.position.z =  0.297;
    sm.userdata.pose_st_02.pose.orientation.x =  0.832;
    sm.userdata.pose_st_02.pose.orientation.y = -0.500;
    sm.userdata.pose_st_02.pose.orientation.z =  0.073;
    sm.userdata.pose_st_02.pose.orientation.w = -0.229;
    sm.userdata.pose_st_03 = PoseStamped();
    sm.userdata.pose_st_03.header.frame_id = '/wam_link0';
    sm.userdata.pose_st_03.pose.position.x =  0.610;
    sm.userdata.pose_st_03.pose.position.y = -0.241;
    sm.userdata.pose_st_03.pose.position.z =  0.259;
    sm.userdata.pose_st_03.pose.orientation.x =  0.837;
    sm.userdata.pose_st_03.pose.orientation.y = -0.471;
    sm.userdata.pose_st_03.pose.orientation.z =  0.141;
    sm.userdata.pose_st_03.pose.orientation.w = -0.242;
    sm.userdata.pose_st_04 = PoseStamped();
    sm.userdata.pose_st_04.header.frame_id = '/wam_link0';
    sm.userdata.pose_st_04.pose.position.x =  0.556;
    sm.userdata.pose_st_04.pose.position.y = -0.221;
    sm.userdata.pose_st_04.pose.position.z =  0.209;
    sm.userdata.pose_st_04.pose.orientation.x =  0.813;
    sm.userdata.pose_st_04.pose.orientation.y = -0.478;
    sm.userdata.pose_st_04.pose.orientation.z =  0.186;
    sm.userdata.pose_st_04.pose.orientation.w = -0.273;
    sm.userdata.pose_st_05 = PoseStamped();
    sm.userdata.pose_st_05.header.frame_id = '/wam_link0';
    sm.userdata.pose_st_05.pose.position.x =  0.478;
    sm.userdata.pose_st_05.pose.position.y = -0.215;
    sm.userdata.pose_st_05.pose.position.z =  0.143;
    sm.userdata.pose_st_05.pose.orientation.x =  0.768;
    sm.userdata.pose_st_05.pose.orientation.y = -0.516;
    sm.userdata.pose_st_05.pose.orientation.z =  0.211;
    sm.userdata.pose_st_05.pose.orientation.w = -0.317;
    sm.userdata.pose_st_06 = PoseStamped();
    sm.userdata.pose_st_06.header.frame_id = '/wam_link0';
    sm.userdata.pose_st_06.pose.position.x =  0.416;
    sm.userdata.pose_st_06.pose.position.y = -0.236;
    sm.userdata.pose_st_06.pose.position.z =  0.084;
    sm.userdata.pose_st_06.pose.orientation.x =  0.746;
    sm.userdata.pose_st_06.pose.orientation.y = -0.475;
    sm.userdata.pose_st_06.pose.orientation.z =  0.278;
    sm.userdata.pose_st_06.pose.orientation.w = -0.375;
    sm.userdata.pose_st_07 = PoseStamped();
    sm.userdata.pose_st_07.header.frame_id = '/wam_link0';
    sm.userdata.pose_st_07.pose.position.x =  0.407;
    sm.userdata.pose_st_07.pose.position.y = -0.273;
    sm.userdata.pose_st_07.pose.position.z =  0.025;
    sm.userdata.pose_st_07.pose.orientation.x =  0.675;
    sm.userdata.pose_st_07.pose.orientation.y = -0.481;
    sm.userdata.pose_st_07.pose.orientation.z =  0.361;
    sm.userdata.pose_st_07.pose.orientation.w = -0.427;
    sm.userdata.filename_00    = '/tmp/plant00.pcd'
    sm.userdata.filename_01    = '/tmp/plant01.pcd'
    sm.userdata.filename_02    = '/tmp/plant02.pcd'
    sm.userdata.filename_03    = '/tmp/plant03.pcd'
    sm.userdata.filename_04    = '/tmp/plant04.pcd'
    sm.userdata.filename_05    = '/tmp/plant05.pcd'
    sm.userdata.filename_06    = '/tmp/plant06.pcd'
    sm.userdata.filename_07    = '/tmp/plant07.pcd'
    sm.userdata.target_frame   = '/wam_link0'

    with sm:
        smach.StateMachine.add('CAPTURE_APP', sm_capture)
    return sm

def main():
    rospy.init_node("base_probing_sm")
    sm_main = construct_main_sm()

    # Run state machine introspection server for smach viewer
    sis = smach_ros.IntrospectionServer('zyonz_capture_sequential_pointcloud', sm_main,
                                        '/zyonz_capture_sequential_pointcloud')

    sis.start()

    outcome = sm_main.execute()

    rospy.spin()
    sis.stop()

if __name__ == "__main__":
    main()