geometric_based_probing_garnics_demo.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 tf
from pprint import pprint
import copy

from smach import StateMachine, CBState, Iterator
from smach_ros import ServiceState
from actionlib import *
from actionlib.msg import *
from std_msgs.msg import String, Int8, Int32
from sensor_msgs.msg import PointCloud2, JointState
from geometry_msgs.msg import Transform, PoseStamped, Pose

from iri_common_smach.st_get_pcl import GetPCL
from zyonz_msgs.msg              import chlorophyll_sampleAction, chlorophyll_sampleGoal
from zyonz_msgs.srv              import GetProbingSteps
from zyonz_apps_base.probing     import ZyonzProbingSMFactory
from zyonz_apps_base.move        import SM_ZYONZ_GoHome

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/geometric_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

        return 'success'

class TransformProbingPose(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['success','fail'],
                                   input_keys  = ['pose_st','pre_pose_st'],
                                   output_keys = ['joints_to_position','pre_joints'])

    def execute(self, userdata):
        try: 
          handler = rospy.ServiceProxy('/zyonz/iri_wam_spad_ik/get_wam_ik', 
                                     wamInverseKinematics)
          response = handler(userdata.pose_st)

          if (response):
              userdata.joints_to_position = response.joints

          response = handler(userdata.pre_pose_st)

          if (response):
              userdata.pre_joints = response.joints

              return 'success'
        except rospy.ServiceException, e:
              return 'fail'

        return 'fail'


class MoveApproachingPose(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['success','fail'],
                                   input_keys  = ['pre_joints'])

    def execute(self, userdata):
        return wam_joints_move(userdata.pre_joints)


class MovePosition(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes    = ['success','fail'],
                                   input_keys  = ['joints_to_position'])

    def execute(self, userdata):
        return wam_joints_move(userdata.joints_to_position)

def construct_main_sm():
    sm = StateMachine(outcomes = ['succeeded','aborted'])
    f_probing  = ZyonzProbingSMFactory()
    sm_probing = f_probing.build_sm()
    f_move     = SM_ZYONZ_GoHome()
    sm_home    = f_move.build_sm()

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

        smach.StateMachine.add('CAPTURE_PCL',
                         GetPCL("/zyonz/euclidean_cluster/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' : 'aborted',
                                          'aborted'               : 'aborted'},
                          remapping    = {'probing_steps' : 'sm_probing_msg'})

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

    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_smach_base_probing', sm_main,
                                        '/zyonz_geometric_based_leaf_probing')
    sis.start()

    outcome = sm_main.execute()

    rospy.spin()
    sis.stop()

if __name__ == "__main__":
    main()