online_test_image_based_leaf_probing.py

Go to the documentation of this file.

#!/usr/bin/env python

import roslib; 
roslib.load_manifest('zyonz_image_based_leaf_probing')
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 iri_common_smach.st_get_pcl    import GetPCL
from zyonz_apps_base.probing        import FindProbingPoint
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

    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('CAPTURE_PCL', GetPCL("/zyonz/average_point_cloud_node/output"),
                                  transitions = {'success' : 'FIND_PROBING_POINT',
                                                 'fail'    : 'aborted'},
                                  remapping   = {'pcl_RGB' : 'sm_pcl'})

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

                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()