image_base_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
from std_msgs.msg import String, Int8, Int32

from iri_common_smach.get_keystroke import GetKeyStroke
from zyonz_apps_base.probing        import SM_ZYONZ_FullProbing
from zyonz_apps_base.move           import SM_ZYONZ_GoHome

from smach import StateMachine, CBState, Iterator

class LogResult(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['ok'])

    def execute(self, userdata):
        keyserver = GetKeyStroke()
        c = -1
        print "Press 0 -> fail or 1 -> success"
        while ((c < 0) or (c > 1)):
            c = keyserver()
            try:
                c = int(c)
            except ValueError:
                continue

        print "Your value was: " + str(c)
        pub = rospy.Publisher('/log/result', Int8, None, False, True)
        pub.publish(c)

        return 'ok'

def construct_main_sm():
    # MAIN STATE MACHINE
    sm = StateMachine(outcomes = ['continue','aborted','succeeded'])
    sm.userdata.rounds = 10

    factory         = SM_ZYONZ_FullProbing()
    sm_full_probing = factory.build_sm()

    with sm:
        round_iterator = Iterator(outcomes = ['succeeded','aborted'],
                                   input_keys = [],
                                   it = range(0, sm.userdata.rounds),
                                   output_keys = [],
                                   it_label = 'index',
                                   exhausted_outcome = 'succeeded')
        with round_iterator:
            container_sm = StateMachine(outcomes = ['succeeded','aborted','continue'])

            with container_sm:
                smach.StateMachine.add('FULL_PROBING_ACTION', sm_full_probing,
                         transitions={'succeeded' : 'LOG_RESULT',
                                      'aborted'   : 'aborted',
                                      'no_kinematic_solution' : 'LOG_RESULT'})
                smach.StateMachine.add('LOG_RESULT', LogResult(),
                        transitions={'ok' : 'continue'})

            #close container_sm
            Iterator.set_contained_state('CONTAINER_STATE',
                                         container_sm,
                                         loop_outcomes=['continue'])
        #close the round_iterator
        StateMachine.add('ROUND_ITERATOR',round_iterator,
                     {'succeeded':'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_image_based_leaf_probing')

    sis.start()

    outcome = sm_main.execute()

    rospy.spin()
    sis.stop()

if __name__ == "__main__":
    main()