Go to the documentation of this file.00001
00002
00003 import roslib;
00004 roslib.load_manifest('zyonz_image_based_leaf_probing')
00005 import rospy
00006 import smach
00007 import smach_ros
00008 import tf
00009 from pprint import pprint
00010 from std_msgs.msg import String, Int8, Int32
00011
00012 from iri_common_smach.get_keystroke import GetKeyStroke
00013 from iri_common_smach.st_get_pcl import GetPCL
00014 from zyonz_apps_base.probing import FindProbingPoint
00015 from zyonz_apps_base.move import SM_ZYONZ_GoHome
00016
00017 from smach import StateMachine, CBState, Iterator
00018
00019 class LogResult(smach.State):
00020 def __init__(self):
00021 smach.State.__init__(self, outcomes = ['ok'])
00022
00023 def execute(self, userdata):
00024 keyserver = GetKeyStroke()
00025 c = -1
00026 print "Press 0 -> fail or 1 -> success"
00027 while ((c < 0) or (c > 1)):
00028 c = keyserver()
00029 try:
00030 c = int(c)
00031 except ValueError:
00032 continue
00033
00034 print "Your value was: " + str(c)
00035 pub = rospy.Publisher('/log/result', Int8, None, False, True)
00036 pub.publish(c)
00037
00038 return 'ok'
00039
00040
00041 def construct_main_sm():
00042
00043 sm = StateMachine(outcomes = ['continue','aborted','succeeded'])
00044 sm.userdata.rounds = 10
00045
00046 with sm:
00047 round_iterator = Iterator(outcomes = ['succeeded','aborted'],
00048 input_keys = [],
00049 it = range(0, sm.userdata.rounds),
00050 output_keys = [],
00051 it_label = 'index',
00052 exhausted_outcome = 'succeeded')
00053 with round_iterator:
00054 container_sm = StateMachine(outcomes = ['succeeded','aborted','continue'])
00055
00056 with container_sm:
00057 smach.StateMachine.add('CAPTURE_PCL', GetPCL("/zyonz/average_point_cloud_node/output"),
00058 transitions = {'success' : 'FIND_PROBING_POINT',
00059 'fail' : 'aborted'},
00060 remapping = {'pcl_RGB' : 'sm_pcl'})
00061
00062 smach.StateMachine.add('FIND_PROBING_POINT', FindProbingPoint(),
00063 transitions = {'success' : 'LOG_RESULT',
00064 'fail' : 'aborted'},
00065 remapping = {'pcl' : 'sm_pcl' ,
00066 'probing_steps' : 'sm_probing_msg' })
00067
00068 smach.StateMachine.add('LOG_RESULT', LogResult(),
00069 transitions={'ok' : 'continue'})
00070
00071
00072
00073 Iterator.set_contained_state('CONTAINER_STATE',
00074 container_sm,
00075 loop_outcomes=['continue'])
00076
00077 StateMachine.add('ROUND_ITERATOR',round_iterator,
00078 {'succeeded':'succeeded',
00079 'aborted':'aborted'})
00080 return sm
00081
00082 def main():
00083 rospy.init_node("base_probing_sm")
00084 sm_main = construct_main_sm()
00085
00086
00087 sis = smach_ros.IntrospectionServer('zyonz_smach_base_probing', sm_main,
00088 '/zyonz_image_based_leaf_probing')
00089
00090 sis.start()
00091
00092 outcome = sm_main.execute()
00093
00094 rospy.spin()
00095 sis.stop()
00096
00097 if __name__ == "__main__":
00098 main()