probing.py

Go to the documentation of this file.

import roslib; roslib.load_manifest('zyonz_apps_base')
import rospy
import smach
import smach_ros
from pprint import pprint

from smach             import Iterator
from geometry_msgs.msg import Pose, PoseStamped, Transform, TransformStamped

from zyonz_msgs.srv import GetProbingSteps

from iri_common_smach.st_get_pcl         import GetPCL
from iri_wam_smach.sm_wam_move_from_pose import SM_WAM_MoveFromPose
from zyonz_apps_base.spadmeter           import SM_ZYONZ_CloseGripper
from zyonz_apps_base.move                import SM_ZYONZ_GoHome

class ZyonzParseProbingMsg(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['finish'],
                                   input_keys  = ['probing_item'],
                                   output_keys = ['pre_pose_st','probing_pose_st','post_pose_st'])

    def execute(self, userdata):
        pre_pose_st = None
        if (len(userdata.probing_item.pre_probing.poses) > 0):
            pre_pose_st          = PoseStamped()
            pre_pose_st.header   = userdata.probing_item.pre_probing.header
            pre_pose_st.pose     = userdata.probing_item.pre_probing.poses[0]

        post_pose_st = None
        if (len(userdata.probing_item.post_probing.poses) > 0):
            post_pose_st = PoseStamped()
            post_pose_st.header   = userdata.probing_item.post_probing.header
            post_pose_st.pose     = userdata.probing_item.post_probing.poses[0]

        pprint(pre_pose_st)
        pprint(post_pose_st)
        pprint("probing:")
        pprint(userdata.probing_item.probing)

        userdata.pre_pose_st = pre_pose_st
        userdata.probing_pose_st = userdata.probing_item.probing
        userdata.post_pose_st = post_pose_st

        return 'finish'

class ZyonzSelectProbingItem(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['finish','continue'],
                                   input_keys  = ['probing_steps','index'],
                                   output_keys = ['probing_item'])

    def execute(self, userdata):
        steps = userdata.probing_steps.steps
        if (userdata.index >= len(steps)):
            return 'finish'

        userdata.probing_item = userdata.probing_steps.steps[userdata.index]
        return 'continue'

class ZyonzParseProbingMsg(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['finish'],
                                   input_keys  = ['probing_item'],
                                   output_keys = ['pre_pose_st','probing_pose_st','post_pose_st'])

    def execute(self, userdata):
        pre_pose_st = None
        if (len(userdata.probing_item.pre_probing.poses) > 0):
            pre_pose_st          = PoseStamped()
            pre_pose_st.header   = userdata.probing_item.pre_probing.header
            pre_pose_st.pose     = userdata.probing_item.pre_probing.poses[0]

        post_pose_st = None
        if (len(userdata.probing_item.post_probing.poses) > 0):
            post_pose_st = PoseStamped()
            post_pose_st.header   = userdata.probing_item.post_probing.header
            post_pose_st.pose     = userdata.probing_item.post_probing.poses[0]

        pprint(pre_pose_st)
        pprint(post_pose_st)
        pprint("probing:")
        pprint(userdata.probing_item.probing)

        userdata.pre_pose_st = pre_pose_st
        userdata.probing_pose_st = userdata.probing_item.probing
        userdata.post_pose_st = post_pose_st

        return 'finish'

class ZyonzSelectProbingItem(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['finish','continue'],
                                   input_keys  = ['probing_steps','index'],
                                   output_keys = ['probing_item'])

    def execute(self, userdata):
        steps = userdata.probing_steps.steps
        if (userdata.index >= len(steps)):
            return 'finish'

        userdata.probing_item = userdata.probing_steps.steps[userdata.index]
        return 'continue'

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

    def build_internal_sm(self):
        sm_approching_factory = SM_WAM_MoveFromPose('/zyonz/wam_wrapper/joints_move',
                                                    '/zyonz/iri_wam_spad_ik/get_wam_ik')
        sm_approching         = sm_approching_factory.build_sm()
        sm_spad_factory       = SM_ZYONZ_CloseGripper()
        sm_spad               = sm_spad_factory.build_sm()

        sm = smach.StateMachine(outcomes   = ['success','aborted','continue','no_kinematic_solution'],
                                input_keys = ['probing_steps','index'])

        with sm:
           smach.StateMachine.add('SELECT_PROBING_ITEM', ZyonzSelectProbingItem(),
                          transitions = {'finish'   : 'success',
                                         'continue' : 'PARSE_PROBING_MSG' },
                          remapping   = {'probing_item' : 'sm_probing_item'})

           smach.StateMachine.add('PARSE_PROBING_MSG', ZyonzParseProbingMsg(),
                          transitions = {'finish': 'PRE_PROBING_STEPS'},
                          remapping   = {'probing_item' : 'sm_probing_item'})

           # Approaching steps
           # TODO: This should be an iterator over number of approaching steps
           smach.StateMachine.add('PRE_PROBING_STEPS', sm_approching,
                           transitions={'success': 'PROBING_POSE',
                                        'no_kinematic_solution': 'no_kinematic_solution',
                                        'aborted': 'aborted' },
                           remapping = {'pose_st':'pre_pose_st'})

           # Probing pose
           smach.StateMachine.add('PROBING_POSE', sm_approching,
                           transitions={'success': 'SPADMETER_ACTION',
                                        'no_kinematic_solution': 'no_kinematic_solution',
                                        'aborted': 'aborted' },
                           remapping = {'pose_st':'probing_pose_st'})

           # Spadmeter action
           # TODO: this machine be converted to some type of parameter so probing does not
           # imply using the spad all times.
           smach.StateMachine.add('SPADMETER_ACTION', sm_spad,
                           transitions={'succeeded' : 'POST_PROBING_STEPS',
                                        'preempted' : 'aborted',
                                        'aborted'   : 'aborted' })

           # Exiting steps
           # TODO: This should be an iterator over number of exiting steps
           smach.StateMachine.add('POST_PROBING_STEPS', sm_approching,
                           transitions={'success': 'continue',
                                        'no_kinematic_solution': 'no_kinematic_solution',
                                        'aborted': 'aborted' },
                           remapping = {'pose_st': 'post_pose_st'})

        return sm

    def build_iterator(self):
        # Iterator allows maximum of 10 rounds. It should use the size of probing_steps
        # but seems impossible to pass this as input.
        iterator = Iterator(outcomes = ['success','aborted','no_kinematic_solution'],
                            input_keys = ['probing_steps','rounds'],
                            it = range(0, 10), 
                            output_keys = [],
                            it_label = 'index',
                            exhausted_outcome = 'success')
        with iterator:
            iterator.set_contained_state('RUN_PROBING_ROUND',
                                         self.build_internal_sm(),
                                         loop_outcomes=['continue'])
        return iterator

    def build_sm(self):

        sm = smach.StateMachine(outcomes   = ['succeeded','aborted','no_kinematic_solution'],
                                input_keys = ['probing_steps'])

        # Dirty as hell until SMACH allows to use input keys as iterator items
        sm.userdata.rounds = 10
        sm_iterator = self.build_iterator()

        with sm:
            smach.StateMachine.add('PROBING_ITERATOR', sm_iterator,
                    transitions = {'success' : 'succeeded'})

        return sm

class FindProbingPoint(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['success','fail'],
                                   input_keys  = ['pcl'],
                                   output_keys = ['probing_steps'])
    def execute(self, userdata):
        handler = rospy.ServiceProxy('/zyonz/skills/image_based_leaf_probing/leaf_probing_srv', GetProbingSteps)

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

        if (not response):
            return 'fail'

        pub = rospy.Publisher('/debug/probing/grasp', PoseStamped, None, False, True)
        pub.publish(response.steps[0].probing)
        pprint(response.steps[0].probing)

        pre_pose_st = PoseStamped()
        pre_pose_st.header = response.steps[0].pre_probing.header
        pre_pose_st.pose   = response.steps[0].pre_probing.poses[0]
        pub = rospy.Publisher('/debug/probing/pre_grasp', PoseStamped, None, False, True)
        pub.publish(pre_pose_st)
        pprint(pre_pose_st)

        #userdata.probing_steps = response.steps[0]
        userdata.probing_steps = response

 #        return 'fail'
        return 'success'

class SM_ZYONZ_FullProbing():
    # Build a whole SMACH machine that will do:
    # 1. Start in home position
    # 2. Capture a PCL from the cam
    # 3. Find the probing point
    # 4. Perform the Probing action
    # 5. Back to home again
    def __init__(self):
        pass

    def build_sm(self):
        factory    = ZyonzProbingSMFactory()
        sm_probing = factory.build_sm()
        factory    = SM_ZYONZ_GoHome()
        sm_goHome  = factory.build_sm()

        sm = smach.StateMachine(outcomes = ['succeeded','aborted','no_kinematic_solution'])

        with sm:
            smach.StateMachine.add('GO_HOME', sm_goHome,
                              transitions={'success': 'CAPTURE_PCL',
                                           'aborted'   : 'aborted'})

            smach.StateMachine.add('CAPTURE_PCL',
                             GetPCL("/zyonz/apply_tof_calibration/output"),
                                transitions = {'success' : 'FIND_PROBING_POINT',
                                               'fail'    : 'aborted'},
                                remapping   = {'pcl_RGB'  : 'sm_pcl'})

            smach.StateMachine.add('FIND_PROBING_POINT', FindProbingPoint(),
                    transitions = {'success' : 'PROBING_ACTION',
                                   'fail'    : 'FIND_PROBING_POINT'},
                    remapping   = {'pcl'           : 'sm_pcl' ,
                                   'probing_steps' : 'sm_probing_msg' })

            smach.StateMachine.add('PROBING_ACTION', sm_probing,
                              transitions  = {'succeeded'     : 'GO_END',
                                              'no_kinematic_solution' : 'no_kinematic_solution',
                                              'aborted' : 'aborted'},
                              remapping    = {'probing_steps' : 'sm_probing_msg'})

            smach.StateMachine.add('GO_END', sm_goHome,
                              transitions={'success': 'succeeded',
                                           'aborted'   : 'aborted'})

        return sm