pionner_probing.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 copy

from smach import Iterator
from smach_ros import ServiceState
from iri_wam_smach.sm_wam_move_from_pose import SM_WAM_MoveFromPose
from geometry_msgs.msg import PoseStamped
from iri_common_smach.st_sleep import WaitSeconds

class SM_ESTIRABOT_ApproachAndCatch():
    def __init_(self):
        pass

    def build_internal_sm(self):
        sm_move_factory = SM_WAM_MoveFromPose('/estirabot/wam_wrapper/joints_move',
                                              '/estirabot/wam_ik/pose_move')
        sm_move         = sm_move_factory.build_sm()

        sm = smach.StateMachine(outcomes   = ['aborted','continue'],
                                input_keys = ['approach_pose_st','catch_pose_st','lift_pose_st'])

        with sm:
             smach.StateMachine.add('GO_TO_APPROACH', sm_move,
                     transitions = {'success' : 'WAIT_AFTER_APPROACH',
                                    'aborted': 'aborted'},
                     remapping   = {'pose_st': 'approach_pose_st'})

             smach.StateMachine.add('WAIT_AFTER_APPROACH', WaitSeconds(1),
                     transitions = {'finish' : 'GO_TO_CATCH'})

             smach.StateMachine.add('GO_TO_CATCH', sm_move,
                     transitions = {'success' : 'WAIT_AFTER_CATCH',
                                    'aborted'   : 'aborted'},
                     remapping   = {'pose_st': 'catch_pose_st'})

             smach.StateMachine.add('WAIT_AFTER_CATCH', WaitSeconds(1),
                     transitions = {'finish' : 'GO_TO_LIFT'})

             smach.StateMachine.add('GO_TO_LIFT', sm_move,
                     transitions = {'success' : 'WAIT_AFTER_LIFT',
                                    'aborted'   : 'aborted'},
                     remapping   = {'pose_st': 'lift_pose_st'})

             smach.StateMachine.add('WAIT_AFTER_LIFT', WaitSeconds(1),
                     transitions = {'finish' : 'GO_TO_CATCH_RETURN'})

             smach.StateMachine.add('GO_TO_CATCH_RETURN', sm_move,
                     transitions = {'success' : 'WAIT_AFTER_CATCH_RETURN',
                                    'aborted'   : 'aborted'},
                     remapping   = {'pose_st': 'catch_pose_st'})

             smach.StateMachine.add('WAIT_AFTER_CATCH_RETURN', WaitSeconds(1),
                     transitions = {'finish' : 'FINAL_POSITION'})

             smach.StateMachine.add('FINAL_POSITION', sm_move,
                     transitions = {'success' : 'continue',
                                    'aborted': 'aborted'},
                     remapping   = {'pose_st': 'approach_pose_st'})

        return sm

    def build_iterator(self):
        iterator = Iterator(outcomes = ['success','aborted'],
                         input_keys = ['approach_pose_st','catch_pose_st','lift_pose_st'],
                         it = range(0, 3),
                         output_keys = [],
                         it_label = 'index',
                         exhausted_outcome = 'success')
        with iterator:
            iterator.set_contained_state('RUN_STEPS',
                                        self.build_internal_sm(),
                                        loop_outcomes=['continue'])
        return iterator

    def build_sm(self):
        sm = smach.StateMachine(outcomes = ['succeeded','aborted'],
                                input_keys = [])

        catch = PoseStamped()
        catch.header.frame_id = 'wam_link0'
        catch.pose.position.x = 0.825
        catch.pose.position.y = 0.052
        catch.pose.position.z = -0.210
        catch.pose.orientation.x = 0.680
        catch.pose.orientation.y = -0.014
        catch.pose.orientation.z = 0.731
        catch.pose.orientation.w = -0.024

        app = copy.deepcopy(catch)
        app.pose.position.x = catch.pose.position.x - 0.07

        lift = copy.deepcopy(catch)
        lift.pose.position.z = catch.pose.position.z + 0.05

        sm.userdata.approach_pose_st = app
        sm.userdata.catch_pose_st    = catch
        sm.userdata.lift_pose_st     = lift

        sm_iterator = self.build_iterator()

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

        return sm

def main():
    rospy.init_node("base_probing_sm")
    sm_grab_factory = SM_ESTIRABOT_ApproachAndCatch()
    sm_main = sm_grab_factory.build_sm()

    sis = smach_ros.IntrospectionServer('estirabot_smach_probing', sm_main,
                                      '/estirabot_smach_probing')
    sis.start()

    outcome = sm_main.execute()

    rospy.spin()
    sis.stop()

if __name__ == "__main__":
    main()