def create(self): # x:711 y:84, x:836 y:453, x:709 y:633 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Find","cup"] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:67 y:70 OperatableStateMachine.add('cond', CheckConditionState(predicate=lambda x: x[1] is not None and x[1] != ''), transitions={'true': 'ReadParameters', 'false': 'say no object given'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'Action'}) # x:325 y:70 OperatableStateMachine.add('say no object given', SaraSay(sentence="You didn't told me what to find.", input_keys=[], emotion=1, block=True), transitions={'done': 'Cause1'}, autonomy={'done': Autonomy.Off}) # x:68 y:363 OperatableStateMachine.add('Action_find', self.use_behavior(sara_flexbe_behaviors__Action_findSM, 'Action_find'), transitions={'done': 'Say_FInded_Object', 'failed': 'Say_Not_Found'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'className': 'name', 'entity': 'entity'}) # x:77 y:159 OperatableStateMachine.add('ReadParameters', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'SAy_Find_Object'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Action', 'output_value': 'name'}) # x:334 y:279 OperatableStateMachine.add('Get Time', CalculationState(calculation=lambda x: x.lastUpdateTime.secs), transitions={'done': 'Get Id'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity', 'output_value': 'currentTime'}) # x:405 y:146 OperatableStateMachine.add('Get Id', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'Set Time'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity', 'output_value': 'id'}) # x:502 y:268 OperatableStateMachine.add('Set Time', SetRosParam(ParamName="/behavior/FoundEntity/lastUpdate"), transitions={'done': 'Set Id'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'currentTime'}) # x:645 y:323 OperatableStateMachine.add('Set Id', SetRosParam(ParamName="/behavior/FoundEntity/Id"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'id'}) # x:835 y:14 OperatableStateMachine.add('Cause1', SetKey(Value="You didn't told me what to find."), transitions={'done': 'set cause in rosparam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:646 y:460 OperatableStateMachine.add('cause2', SetKey(Value="I didn't find the object"), transitions={'done': 'set cause in rosparam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:769 y:347 OperatableStateMachine.add('set cause in rosparam', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:47 y:258 OperatableStateMachine.add('SAy_Find_Object', SaraSay(sentence=lambda x: "I'm now looking for the " + x, input_keys=[], emotion=0, block=True), transitions={'done': 'Action_find'}, autonomy={'done': Autonomy.Off}) # x:296 y:403 OperatableStateMachine.add('Say_Not_Found', SaraSay(sentence=lambda x: "I did not find the " + x + ".", input_keys=[], emotion=0, block=True), transitions={'done': 'cause2'}, autonomy={'done': Autonomy.Off}) # x:212 y:130 OperatableStateMachine.add('Say_FInded_Object', SaraSay(sentence=lambda x: "I just find the " + x.name, input_keys=[], emotion=0, block=True), transitions={'done': 'Get Time'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:746 y:544, x:65 y:361 _state_machine = OperatableStateMachine( outcomes=['finished', 'not found'], input_keys=['Position']) _state_machine.userdata.Position = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365 _sm_verifie_presence_0 = OperatableStateMachine(outcomes=['not found']) with _sm_verifie_presence_0: # x:90 y:107 OperatableStateMachine.add( 'getID', GetRosParam(ParamName="behavior/Operator/Id"), transitions={ 'done': 'getOperator', 'failed': 'not found' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'ID'}) # x:297 y:176 OperatableStateMachine.add('getOperator', GetEntityByID(), transitions={ 'found': 'getID', 'not_found': 'not found' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Operator' }) # x:208 y:123, x:289 y:206, x:75 y:376, x:330 y:365, x:430 y:365 _sm_deplacement_et_verification_de_presence_1 = ConcurrencyContainer( outcomes=['arrived', 'failed'], input_keys=['Position'], conditions=[('failed', [('verifie presence', 'not found')]), ('arrived', [('saramove', 'arrived')]), ('failed', [('saramove', 'failed')])]) with _sm_deplacement_et_verification_de_presence_1: # x:70 y:106 OperatableStateMachine.add('saramove', SaraMoveBase(reference="map"), transitions={ 'arrived': 'arrived', 'failed': 'failed' }, autonomy={ 'arrived': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'pose': 'Position'}) # x:255 y:90 OperatableStateMachine.add( 'verifie presence', _sm_verifie_presence_0, transitions={'not found': 'failed'}, autonomy={'not found': Autonomy.Inherit}) with _state_machine: # x:55 y:57 OperatableStateMachine.add( 'Get_operator', self.use_behavior(sara_flexbe_behaviors__Get_operatorSM, 'Get_operator'), transitions={ 'Found': 'foundyou!', 'NotFound': 'not found' }, autonomy={ 'Found': Autonomy.Inherit, 'NotFound': Autonomy.Inherit }, remapping={'Operator': 'Operator'}) # x:162 y:210 OperatableStateMachine.add( 'foundyou!', SaraSay(sentence="Operator, please follow me", emotion=1, block=True), transitions={ 'done': 'deplacement et verification de presence' }, autonomy={'done': Autonomy.Off}) # x:398 y:500 OperatableStateMachine.add( 'destinationreached', SaraSay(sentence="We have reached our destination", emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:247 y:319 OperatableStateMachine.add( 'deplacement et verification de presence', _sm_deplacement_et_verification_de_presence_1, transitions={ 'arrived': 'destinationreached', 'failed': 'lostyou' }, autonomy={ 'arrived': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'Position': 'Position'}) # x:599 y:180 OperatableStateMachine.add( 'lostyou', SaraSay(sentence="I lost you, please stay where you are", emotion=1, block=True), transitions={'done': 'Get_operator'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:310 y:409, x:130 y:365 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:17 y:314 _sm_group_0 = OperatableStateMachine(outcomes=['done'], output_keys=['Words']) with _sm_group_0: # x:84 y:76 OperatableStateMachine.add('speech', GetSpeech(watchdog=8), transitions={ 'done': 'wait', 'nothing': 'speech', 'fail': 'speech' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'Words'}) # x:92 y:221 OperatableStateMachine.add('wait', WaitState(wait_time=1), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:30 y:365 _sm_group_2_1 = OperatableStateMachine(outcomes=['done']) with _sm_group_2_1: # x:32 y:131 OperatableStateMachine.add('test_sound', LookAtSound(moveBase=True), transitions={'done': 'test_sound'}, autonomy={'done': Autonomy.Off}) # x:30 y:365, x:337 y:100, x:230 y:365 _sm_look_and_wait_2 = ConcurrencyContainer( outcomes=['done'], output_keys=['Words'], conditions=[('done', [('Group', 'done')]), ('done', [('Group_2', 'done')])]) with _sm_look_and_wait_2: # x:75 y:82 OperatableStateMachine.add('Group_2', _sm_group_2_1, transitions={'done': 'done'}, autonomy={'done': Autonomy.Inherit}) # x:28 y:173 OperatableStateMachine.add('Group', _sm_group_0, transitions={'done': 'done'}, autonomy={'done': Autonomy.Inherit}, remapping={'Words': 'Words'}) with _state_machine: # x:60 y:65 OperatableStateMachine.add('say marco!', SaraSay(sentence="Marco?", input_keys=[], emotion=1, block=True), transitions={'done': 'Look and wait'}, autonomy={'done': Autonomy.Off}) # x:335 y:191 OperatableStateMachine.add('Look and wait', _sm_look_and_wait_2, transitions={'done': 'is polo!'}, autonomy={'done': Autonomy.Inherit}, remapping={'Words': 'Words'}) # x:281 y:85 OperatableStateMachine.add( 'hey', SaraSay(sentence="Hey! You need to say polo!", input_keys=[], emotion=1, block=True), transitions={'done': 'say marco!'}, autonomy={'done': Autonomy.Off}) # x:520 y:36 OperatableStateMachine.add( 'is polo!', RegexTester(regex="[^o]*o[^o\ ]*o[^o]*"), transitions={ 'true': 'say marco!', 'false': 'hey' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'Words', 'result': 'result' }) return _state_machine
def create(self): # x:832 y:260, x:728 y:256 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['ID']) _state_machine.userdata.ID = 0 _state_machine.userdata.Closed_Gripper_Width = 1 _state_machine.userdata.Open_Gripper_Width = 255 _state_machine.userdata.Relative = False _state_machine.userdata.Pose_Init = "IdlePose" _state_machine.userdata.dropPose = "DropBagPose" _state_machine.userdata.EntryDoor = "door1/enter" _state_machine.userdata.ExitDoor = "door1/exit" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:700 y:231, x:418 y:463 _sm_ecoute_getpose_0 = OperatableStateMachine(outcomes=['arrete', 'fail']) with _sm_ecoute_getpose_0: # x:53 y:35 OperatableStateMachine.add('Ecoute', GetSpeech(watchdog=5), transitions={'done': 'stop', 'nothing': 'Ecoute', 'fail': 'fail'}, autonomy={'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'words': 'words'}) # x:373 y:215 OperatableStateMachine.add('stop', RegexTester(regex=".*stop.*"), transitions={'true': 'arrete', 'false': 'Ecoute'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'text': 'words', 'result': 'result'}) # x:350 y:105, x:261 y:311 _sm_get_operator_id_1 = OperatableStateMachine(outcomes=['not_found', 'done'], output_keys=['ID']) with _sm_get_operator_id_1: # x:42 y:30 OperatableStateMachine.add('nom', SetKey(Value="person"), transitions={'done': 'FindId'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:37 y:347 OperatableStateMachine.add('setID', SetRosParam(ParamName="OperatorID"), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'ID'}) # x:30 y:112 OperatableStateMachine.add('FindId', list_entities_by_name(frontality_level=0.5, distance_max=10), transitions={'found': 'GetID', 'none_found': 'not_found'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'name', 'entity_list': 'Entities_list', 'number': 'number'}) # x:38 y:238 OperatableStateMachine.add('GetID', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'setID'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Entities_list', 'output_value': 'ID'}) # x:346 y:367, x:610 y:472, x:230 y:365, x:628 y:425, x:619 y:318, x:54 y:367 _sm_follow_2 = ConcurrencyContainer(outcomes=['done', 'failed', 'not_found'], input_keys=['ID', 'distance'], output_keys=['Position'], conditions=[ ('not_found', [('Action_follow', 'failed')]), ('done', [('Ecoute_getPose', 'arrete')]), ('failed', [('Ecoute_getPose', 'fail')]) ]) with _sm_follow_2: # x:107 y:103 OperatableStateMachine.add('Action_follow', self.use_behavior(sara_flexbe_behaviors__Action_followSM, 'Getting ID Operator and follow /Follow/Action_follow'), transitions={'failed': 'not_found'}, autonomy={'failed': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:294 y:146 OperatableStateMachine.add('Ecoute_getPose', _sm_ecoute_getpose_0, transitions={'arrete': 'done', 'fail': 'failed'}, autonomy={'arrete': Autonomy.Inherit, 'fail': Autonomy.Inherit}) # x:57 y:371, x:419 y:55 _sm_waiting_for_operator_3 = OperatableStateMachine(outcomes=['done', 'failed']) with _sm_waiting_for_operator_3: # x:57 y:67 OperatableStateMachine.add('getSpeech', GetSpeech(watchdog=15), transitions={'done': 'UnderstandingOpe', 'nothing': 'failed', 'fail': 'failed'}, autonomy={'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'words': 'words'}) # x:53 y:249 OperatableStateMachine.add('start', SaraSay(sentence="I will follow you. Tell me when to stop.", input_keys=[], emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:203 y:180 OperatableStateMachine.add('UnderstandingOpe', RegexTester(regex=".*follow.*"), transitions={'true': 'start', 'false': 'failed'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'text': 'words', 'result': 'result'}) # x:130 y:365 _sm_look_at_4 = OperatableStateMachine(outcomes=['failed']) with _sm_look_at_4: # x:105 y:160 OperatableStateMachine.add('LookAtClosest', self.use_behavior(sara_flexbe_behaviors__LookAtClosestSM, 'Recevoir sac/Look at/LookAtClosest'), transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:280 y:296, x:248 y:549 _sm_receive_bag_5 = OperatableStateMachine(outcomes=['failed', 'done'], input_keys=['Closed_Gripper_Width', 'Open_Gripper_Width']) with _sm_receive_bag_5: # x:42 y:326 OperatableStateMachine.add('PutBAg', SaraSay(sentence="Please put the grocery bag in my hand", input_keys=[], emotion=1, block=False), transitions={'done': 'Action_Receive_Bag'}, autonomy={'done': Autonomy.Off}) # x:18 y:427 OperatableStateMachine.add('Action_Receive_Bag', self.use_behavior(sara_flexbe_behaviors__Action_Receive_BagSM, 'Recevoir sac/Receive bag/Action_Receive_Bag'), transitions={'finished': 'bringit', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'Closed_Gripper_Width': 'Closed_Gripper_Width', 'Open_Gripper_Width': 'Open_Gripper_Width', 'Closed_Gripper_Width': 'Closed_Gripper_Width'}) # x:66 y:125 OperatableStateMachine.add('getspeech2', GetSpeech(watchdog=5), transitions={'done': 'takebag', 'nothing': 'getspeech2', 'fail': 'failed'}, autonomy={'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'words': 'words'}) # x:64 y:218 OperatableStateMachine.add('takebag', RegexTester(regex=".*((take)|(bag)|(ready)).*"), transitions={'true': 'PutBAg', 'false': 'getspeech2'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'text': 'words', 'result': 'result'}) # x:42 y:531 OperatableStateMachine.add('bringit', SaraSay(sentence="I will bring it inside", input_keys=[], emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:77 y:40 OperatableStateMachine.add('sac', SaraSay(sentence="Ok. Tell me, when you are ready", input_keys=[], emotion=1, block=True), transitions={'done': 'getspeech2'}, autonomy={'done': Autonomy.Off}) # x:147 y:579 _sm_look_at_closest_6 = OperatableStateMachine(outcomes=['failed'], output_keys=['Entity']) with _sm_look_at_closest_6: # x:40 y:39 OperatableStateMachine.add('set person', SetKey(Value="person"), transitions={'done': 'head angle'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Person'}) # x:36 y:119 OperatableStateMachine.add('head angle', SaraSetHeadAngle(pitch=-0.3, yaw=0), transitions={'done': 'get persons'}, autonomy={'done': Autonomy.Off}) # x:271 y:149 OperatableStateMachine.add('get persons', list_entities_by_name(frontality_level=0.5, distance_max=2), transitions={'found': 'set person', 'none_found': 'get persons'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'Person', 'entity_list': 'entity_list', 'number': 'number'}) # x:30 y:365 _sm_get_op_7 = OperatableStateMachine(outcomes=['true']) with _sm_get_op_7: # x:76 y:40 OperatableStateMachine.add('Say Joke', SaraSay(sentence="Can someone come help me, I only have one arm.", input_keys=[], emotion=1, block=True), transitions={'done': 'ecouteNewPerson'}, autonomy={'done': Autonomy.Off}) # x:208 y:255 OperatableStateMachine.add('listen', RegexTester(regex=".*((i)|(come)|(help)).*"), transitions={'true': 'true', 'false': 'ecouteNewPerson'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'text': 'words', 'result': 'result'}) # x:32 y:106 OperatableStateMachine.add('ecouteNewPerson', GetSpeech(watchdog=5), transitions={'done': 'listen', 'nothing': 'ecouteNewPerson', 'fail': 'ecouteNewPerson'}, autonomy={'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'words': 'words'}) # x:65 y:357, x:130 y:365, x:230 y:365 _sm_get_ope_8 = ConcurrencyContainer(outcomes=['true'], output_keys=['Entity'], conditions=[ ('true', [('get op', 'true')]), ('true', [('look at closest', 'failed')]) ]) with _sm_get_ope_8: # x:37 y:35 OperatableStateMachine.add('get op', _sm_get_op_7, transitions={'true': 'true'}, autonomy={'true': Autonomy.Inherit}) # x:238 y:109 OperatableStateMachine.add('look at closest', _sm_look_at_closest_6, transitions={'failed': 'true'}, autonomy={'failed': Autonomy.Inherit}, remapping={'Entity': 'Entity'}) # x:37 y:417, x:236 y:263 _sm_init_sara_9 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['Pose_Init'], output_keys=['Origin']) with _sm_init_sara_9: # x:30 y:40 OperatableStateMachine.add('get_pose', Get_Robot_Pose(), transitions={'done': 'SETHEAD'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'Origin'}) # x:24 y:259 OperatableStateMachine.add('Init Arm', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={'done': 'done', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'target': 'Pose_Init'}) # x:23 y:183 OperatableStateMachine.add('SETHEAD', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'Init Arm'}, autonomy={'done': Autonomy.Off}) # x:92 y:291, x:242 y:133 _sm_exitarena_10 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['ExitDoor']) with _sm_exitarena_10: # x:38 y:119 OperatableStateMachine.add('Action_Pass_Door', self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'ExitArena/Action_Pass_Door'), transitions={'Done': 'finished', 'Fail': 'failed'}, autonomy={'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit}, remapping={'DoorName': 'ExitDoor'}) # x:353 y:529, x:369 y:296 _sm_enter_arena_11 = OperatableStateMachine(outcomes=['done', 'fail'], input_keys=['EntryDoor']) with _sm_enter_arena_11: # x:132 y:147 OperatableStateMachine.add('Action_Pass_Door', self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'Enter arena/Action_Pass_Door'), transitions={'Done': 'say start', 'Fail': 'fail'}, autonomy={'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit}, remapping={'DoorName': 'EntryDoor'}) # x:124 y:501 OperatableStateMachine.add('say help', SaraSay(sentence="Hi, i will help you carry some bags. LEt me know when you need me", input_keys=[], emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:146 y:256 OperatableStateMachine.add('say start', SaraSay(sentence="", input_keys=[], emotion=1, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:121 y:375 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'Enter arena/Action_Move'), transitions={'finished': 'say help', 'failed': 'fail'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'EntryDoor'}) # x:372 y:428, x:389 y:477 _sm_getting_id_operator_and_follow__12 = OperatableStateMachine(outcomes=['failed', 'done'], output_keys=['Position']) with _sm_getting_id_operator_and_follow__12: # x:70 y:115 OperatableStateMachine.add('Waiting for operator', _sm_waiting_for_operator_3, transitions={'done': 'Get operator ID', 'failed': 'Waiting for operator'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:71 y:504 OperatableStateMachine.add('get pose', Get_Robot_Pose(), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'Position'}) # x:69 y:415 OperatableStateMachine.add('Follow', _sm_follow_2, transitions={'done': 'get pose', 'failed': 'failed', 'not_found': 'initial'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'not_found': Autonomy.Inherit}, remapping={'ID': 'ID', 'distance': 'distance', 'Position': 'Position'}) # x:60 y:222 OperatableStateMachine.add('Get operator ID', _sm_get_operator_id_1, transitions={'not_found': 'Get operator ID', 'done': 'set follow distance'}, autonomy={'not_found': Autonomy.Inherit, 'done': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:60 y:324 OperatableStateMachine.add('set follow distance', SetKey(Value="0.5"), transitions={'done': 'Follow'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distance'}) # x:286 y:327 OperatableStateMachine.add('initial', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'Get operator ID'}, autonomy={'done': Autonomy.Off}) # x:247 y:123, x:282 y:200, x:230 y:365 _sm_drop_le_sac_13 = OperatableStateMachine(outcomes=['failed', 'no_object', 'done'], input_keys=['Idle', 'dropPose']) with _sm_drop_le_sac_13: # x:22 y:114 OperatableStateMachine.add('dropbag', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={'done': 'Open', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'target': 'dropPose'}) # x:5 y:344 OperatableStateMachine.add('close', SetGripperState(width=0, effort=1), transitions={'object': 'returnIdl', 'no_object': 'returnIdl'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:19 y:268 OperatableStateMachine.add('wait', WaitState(wait_time=2), transitions={'done': 'close'}, autonomy={'done': Autonomy.Off}) # x:8 y:433 OperatableStateMachine.add('returnIdl', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={'done': 'done', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'target': 'Idle'}) # x:10 y:190 OperatableStateMachine.add('Open', SetGripperState(width=0.1, effort=1), transitions={'object': 'wait', 'no_object': 'no_object'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:555 y:54, x:568 y:190 _sm_retour_maison_14 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['PoseOrigin', 'Relative']) with _sm_retour_maison_14: # x:54 y:63 OperatableStateMachine.add('keydistance', SetKey(Value=0), transitions={'done': 'adapt the end pose'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distance'}) # x:258 y:51 OperatableStateMachine.add('Arrived', SaraSay(sentence="I have food, people. I will drop the bags on the floor.", input_keys=[], emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:43 y:141 OperatableStateMachine.add('adapt the end pose', Get_Reacheable_Waypoint(), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'pose_in': 'PoseOrigin', 'distance': 'distance', 'pose_out': 'pose_out'}) # x:236 y:182 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'Retour maison/Action_Move'), transitions={'finished': 'Arrived', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'pose_out'}) # x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365 _sm_recevoir_sac_15 = ConcurrencyContainer(outcomes=['failed', 'done'], input_keys=['Closed_Gripper_Width', 'Open_Gripper_Width'], conditions=[ ('failed', [('Receive bag', 'failed')]), ('done', [('Receive bag', 'done')]), ('failed', [('Look at', 'failed')]) ]) with _sm_recevoir_sac_15: # x:84 y:156 OperatableStateMachine.add('Receive bag', _sm_receive_bag_5, transitions={'failed': 'failed', 'done': 'done'}, autonomy={'failed': Autonomy.Inherit, 'done': Autonomy.Inherit}, remapping={'Closed_Gripper_Width': 'Closed_Gripper_Width', 'Open_Gripper_Width': 'Open_Gripper_Width'}) # x:262 y:154 OperatableStateMachine.add('Look at', _sm_look_at_4, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:728 y:533, x:722 y:468 _sm_getnewperson_16 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['Position']) with _sm_getnewperson_16: # x:52 y:77 OperatableStateMachine.add('Get ope', _sm_get_ope_8, transitions={'true': 'set name'}, autonomy={'true': Autonomy.Inherit}, remapping={'Entity': 'Entity'}) # x:46 y:508 OperatableStateMachine.add('SET HEAD', SaraSetHeadAngle(pitch=-0.2, yaw=0), transitions={'done': 'Action_Guide2'}, autonomy={'done': Autonomy.Off}) # x:39 y:297 OperatableStateMachine.add('list', list_entities_by_name(frontality_level=0.5, distance_max=2), transitions={'found': 'get closest ID', 'none_found': 'list'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'name', 'entity_list': 'entity_list', 'number': 'number'}) # x:70 y:194 OperatableStateMachine.add('set name', SetKey(Value="person"), transitions={'done': 'list'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:151 y:413 OperatableStateMachine.add('get closest ID', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'SET HEAD'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity_list', 'output_value': 'ID'}) # x:293 y:534 OperatableStateMachine.add('Action_Guide2', self.use_behavior(sara_flexbe_behaviors__Action_Guide2SM, 'GetNewPerson/Action_Guide2'), transitions={'finished': 'done', 'failed': 'failed', 'critical_fail': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit}, remapping={'Position': 'Position', 'ID': 'ID'}) # x:57 y:399 OperatableStateMachine.add('set 0', SetKey(Value=0), transitions={'done': 'SET HEAD'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'ID'}) with _state_machine: # x:188 y:35 OperatableStateMachine.add('say ready', SaraSay(sentence="I'm ready for the help me carry scenario. I will follow when you ask me.", input_keys=[], emotion=1, block=True), transitions={'done': 'INIT SARA'}, autonomy={'done': Autonomy.Off}) # x:827 y:439 OperatableStateMachine.add('finish', SaraSay(sentence="I am done for the day", input_keys=[], emotion=2, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:53 y:641 OperatableStateMachine.add('GetNewPerson', _sm_getnewperson_16, transitions={'done': 'say leave', 'failed': 'failed'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'Position': 'Position'}) # x:48 y:349 OperatableStateMachine.add('Recevoir sac', _sm_recevoir_sac_15, transitions={'failed': 'failed', 'done': 'Retour maison'}, autonomy={'failed': Autonomy.Inherit, 'done': Autonomy.Inherit}, remapping={'Closed_Gripper_Width': 'Closed_Gripper_Width', 'Open_Gripper_Width': 'Open_Gripper_Width'}) # x:36 y:448 OperatableStateMachine.add('Retour maison', _sm_retour_maison_14, transitions={'done': 'drop le sac', 'failed': 'failed'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'PoseOrigin': 'Origin', 'Relative': 'Relative'}) # x:49 y:556 OperatableStateMachine.add('drop le sac', _sm_drop_le_sac_13, transitions={'failed': 'failed', 'no_object': 'failed', 'done': 'GetNewPerson'}, autonomy={'failed': Autonomy.Inherit, 'no_object': Autonomy.Inherit, 'done': Autonomy.Inherit}, remapping={'Idle': 'Pose_Init', 'dropPose': 'dropPose'}) # x:150 y:260 OperatableStateMachine.add('Getting ID Operator and follow ', _sm_getting_id_operator_and_follow__12, transitions={'failed': 'failed', 'done': 'Recevoir sac'}, autonomy={'failed': Autonomy.Inherit, 'done': Autonomy.Inherit}, remapping={'Position': 'Position'}) # x:148 y:177 OperatableStateMachine.add('Enter arena', _sm_enter_arena_11, transitions={'done': 'Getting ID Operator and follow ', 'fail': 'failed'}, autonomy={'done': Autonomy.Inherit, 'fail': Autonomy.Inherit}, remapping={'EntryDoor': 'EntryDoor'}) # x:498 y:617 OperatableStateMachine.add('ExitArena', _sm_exitarena_10, transitions={'finished': 'finish', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'ExitDoor': 'ExitDoor'}) # x:389 y:14 OperatableStateMachine.add('ContinueButton', ContinueButton(), transitions={'true': 'say ready', 'false': 'say ready'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}) # x:329 y:645 OperatableStateMachine.add('say leave', SaraSay(sentence="I will leave now. Goodbye", input_keys=[], emotion=1, block=True), transitions={'done': 'ExitArena'}, autonomy={'done': Autonomy.Off}) # x:319 y:101 OperatableStateMachine.add('wait1', WaitState(wait_time=5), transitions={'done': 'say ready'}, autonomy={'done': Autonomy.Off}) # x:35 y:78 OperatableStateMachine.add('INIT SARA', _sm_init_sara_9, transitions={'done': 'Getting ID Operator and follow ', 'failed': 'failed'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'Pose_Init': 'Pose_Init', 'Origin': 'Origin'}) return _state_machine
def create(self): # x:814 y:45, x:514 y:274 _state_machine = OperatableStateMachine(outcomes=['Found', 'NotFound'], output_keys=['Operator']) _state_machine.userdata.Operator = None _state_machine.userdata.Name = "person" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:506 y:393, x:515 y:462 _sm_move_to_person_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['Operator']) with _sm_move_to_person_0: # x:30 y:83 OperatableStateMachine.add( 'Getpos', CalculationState(calculation=lambda x: x.position), transitions={'done': 'setDistance'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Operator', 'output_value': 'pose_in' }) # x:35 y:450 OperatableStateMachine.add('Action_Move', self.use_behavior( Action_MoveSM, 'Move to person/Action_Move'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'Pose'}) # x:47 y:368 OperatableStateMachine.add('set not rel', SetKey(Value=False), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'relative'}) # x:41 y:179 OperatableStateMachine.add('setDistance', SetKey(Value=1.5), transitions={'done': 'Close position'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distance'}) # x:27 y:280 OperatableStateMachine.add('Close position', Get_Reacheable_Waypoint(), transitions={'done': 'set not rel'}, autonomy={'done': Autonomy.Off}, remapping={ 'pose_in': 'pose_in', 'distance': 'distance', 'pose_out': 'Pose' }) with _state_machine: # x:64 y:35 OperatableStateMachine.add( 'Get previous ID', GetRosParam(ParamName="behavior/Operator/Id"), transitions={ 'done': 'Get Operator', 'failed': 'for 3' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'ID'}) # x:271 y:37 OperatableStateMachine.add('Get Operator', GetEntityByID(), transitions={ 'found': 'Found', 'not_found': 'Say lost operator' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Operator' }) # x:263 y:155 OperatableStateMachine.add('Say lost operator', SaraSay(sentence="I lost my operator", input_keys=[], emotion=1, block=True), transitions={'done': 'for 3'}, autonomy={'done': Autonomy.Off}) # x:780 y:517 OperatableStateMachine.add('ask if operator', SaraSay(sentence="Are you my operator?", input_keys=[], emotion=1, block=True), transitions={'done': 'get speech'}, autonomy={'done': Autonomy.Off}) # x:70 y:273 OperatableStateMachine.add('for 3', ForLoop(repeat=3), transitions={ 'do': 'for 3_2', 'end': 'set None' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:249 y:357 OperatableStateMachine.add('say where are you', SaraSay( sentence="Operator. Where are you?", input_keys=[], emotion=1, block=True), transitions={'done': 'for 3'}, autonomy={'done': Autonomy.Off}) # x:281 y:265 OperatableStateMachine.add('set None', SetKey(Value=None), transitions={'done': 'NotFound'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Operator'}) # x:49 y:511 OperatableStateMachine.add('Get persons', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'get next closest', 'none_found': 'say where are you' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'Name', 'entity_list': 'entity_list', 'number': 'number' }) # x:461 y:475 OperatableStateMachine.add('Move to person', _sm_move_to_person_0, transitions={ 'finished': 'ask if operator', 'failed': 'NotFound' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'Operator': 'Operator'}) # x:783 y:161 OperatableStateMachine.add( 'set new ID', SetRosParam(ParamName="behavior/Operator/Id"), transitions={'done': 'Found'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'ID'}) # x:775 y:269 OperatableStateMachine.add( 'get ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'set new ID'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Operator', 'output_value': 'ID' }) # x:784 y:433 OperatableStateMachine.add('get speech', GetSpeech(watchdog=5), transitions={ 'done': 'Yes ?', 'nothing': 'for 3_2', 'fail': 'NotFound' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) # x:69 y:402 OperatableStateMachine.add('for 3_2', ForLoop(repeat=3), transitions={ 'do': 'Get persons', 'end': 'set None' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index2'}) # x:744 y:332 OperatableStateMachine.add( 'Yes ?', CheckConditionState(predicate=lambda x: "yes" in x), transitions={ 'true': 'get ID', 'false': 'for 3_2' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'words'}) # x:263 y:535 OperatableStateMachine.add( 'get next closest', FlexibleCalculationState(calculation=lambda x: x[0][x[1]], input_keys=["entity_list", "index"]), transitions={'done': 'ask if operator'}, autonomy={'done': Autonomy.Off}, remapping={ 'entity_list': 'entity_list', 'index': 'index', 'output_value': 'Operator' }) return _state_machine
def create(self): # x:1070 y:493, x:687 y:155 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.MoveToPeople = ["move","crowd"] _state_machine.userdata.CupboardName = "cupboard" _state_machine.userdata.MoveToTable = ["move","table"] _state_machine.userdata.PickObject = ["pick", "bottle"] _state_machine.userdata.PlaceObject = ["place", "table"] _state_machine.userdata.rotation = 1.5 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365 _sm_salute_people_0 = OperatableStateMachine(outcomes=['done']) with _sm_salute_people_0: # x:105 y:52 OperatableStateMachine.add('wave', RunTrajectory(file="Wave"), transitions={'done': 'hi'}, autonomy={'done': Autonomy.Off}) # x:95 y:152 OperatableStateMachine.add('hi', SaraSay(sentence="Hi i will show you how great i am.", input_keys=[], emotion=1, block=True), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:107 y:335 OperatableStateMachine.add('say pick', SaraSay(sentence="I will take the bottle from this cupboard and place it on the table.", input_keys=[], emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:116 y:243 OperatableStateMachine.add('w1', WaitState(wait_time=1), transitions={'done': 'say pick'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:70 y:37 OperatableStateMachine.add('Init_Sequence', self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'Init_Sequence'), transitions={'finished': 'Action_MovePeople', 'failed': 'Action_MovePeople'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:975 y:587 OperatableStateMachine.add('bye', SaraSay(sentence="I am done for the day. Thank you for visiting me!", input_keys=[], emotion=1, block=True), transitions={'done': 'Init_Sequence'}, autonomy={'done': Autonomy.Off}) # x:48 y:551 OperatableStateMachine.add('ActionWrapper_Pick', self.use_behavior(sara_flexbe_behaviors__ActionWrapper_PickSM, 'ActionWrapper_Pick'), transitions={'finished': 'Action_MoveTable', 'failed': 'try', 'critical_fail': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit}, remapping={'Action': 'PickObject'}) # x:58 y:343 OperatableStateMachine.add('Action_OpenCupboard', self.use_behavior(sara_flexbe_behaviors__Action_OpenCupboardSM, 'Action_OpenCupboard'), transitions={'finished': 'say looking', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'name': 'CupboardName'}) # x:60 y:653 OperatableStateMachine.add('try', SaraSay(sentence="I will try again", input_keys=[], emotion=1, block=True), transitions={'done': 'Action_OpenCupboard'}, autonomy={'done': Autonomy.Off}) # x:70 y:152 OperatableStateMachine.add('Action_MovePeople', self.use_behavior(sara_flexbe_behaviors__ActionWrapper_MoveSM, 'Action_MovePeople'), transitions={'finished': 'Salute people', 'failed': 'Action_MovePeople', 'critical_fail': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit}, remapping={'Action': 'MoveToPeople'}) # x:621 y:485 OperatableStateMachine.add('ActionWrapper_Place', self.use_behavior(sara_flexbe_behaviors__ActionWrapper_PlaceSM, 'ActionWrapper_Place'), transitions={'finished': 'wave', 'failed': 'failed', 'critical_fail': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit}, remapping={'Action': 'PlaceObject'}) # x:75 y:247 OperatableStateMachine.add('Salute people', _sm_salute_people_0, transitions={'done': 'Action_OpenCupboard'}, autonomy={'done': Autonomy.Inherit}) # x:772 y:588 OperatableStateMachine.add('action_turn', self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'action_turn'), transitions={'finished': 'bye', 'failed': 'bye'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'rotation': 'rotation'}) # x:349 y:485 OperatableStateMachine.add('Action_MoveTable', self.use_behavior(sara_flexbe_behaviors__ActionWrapper_MoveSM, 'Action_MoveTable'), transitions={'finished': 'ActionWrapper_Place', 'failed': 'Action_MoveTable', 'critical_fail': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit}, remapping={'Action': 'MoveToTable'}) # x:70 y:445 OperatableStateMachine.add('say looking', SaraSay(sentence="I'm now looking for the bottle.", input_keys=[], emotion=1, block=True), transitions={'done': 'ActionWrapper_Pick'}, autonomy={'done': Autonomy.Off}) # x:833 y:462 OperatableStateMachine.add('wave', RunTrajectory(file="Wave"), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:660 y:509, x:880 y:203, x:715 y:440 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Pick","bottle"] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365 _sm_group_0 = ConcurrencyContainer(outcomes=['done'], input_keys=['pose_out'], conditions=[ ('done', [('move', 'arrived')]), ('done', [('move', 'failed')]), ('done', [('3', 'done')]) ]) with _sm_group_0: # x:30 y:40 OperatableStateMachine.add('move', SaraMoveBase(reference="map"), transitions={'arrived': 'done', 'failed': 'done'}, autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'pose': 'pose_out'}) # x:179 y:91 OperatableStateMachine.add('3', WaitState(wait_time=2), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:40 y:700 _sm_get_closer_1 = OperatableStateMachine(outcomes=['done'], input_keys=['Object']) with _sm_get_closer_1: # x:59 y:36 OperatableStateMachine.add('set targetpose', SetKey(Value="PreGripPose"), transitions={'done': 'say closer'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:88 y:374 OperatableStateMachine.add('set dist', SetKey(Value=0.8), transitions={'done': 'get close pos'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distance'}) # x:26 y:448 OperatableStateMachine.add('get close pos', Get_Reacheable_Waypoint(), transitions={'done': 'Group'}, autonomy={'done': Autonomy.Off}, remapping={'pose_in': 'Pos', 'distance': 'distance', 'pose_out': 'pose_out'}) # x:47 y:213 OperatableStateMachine.add('get pos', CalculationState(calculation=lambda x: x.position), transitions={'done': 'move head'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Object', 'output_value': 'Pos'}) # x:88 y:290 OperatableStateMachine.add('move head', SaraSetHeadAngle(pitch=0.7, yaw=0), transitions={'done': 'set dist'}, autonomy={'done': Autonomy.Off}) # x:201 y:156 OperatableStateMachine.add('move arm', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={'done': 'get pos', 'failed': 'get pos'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'target': 'target'}) # x:60 y:106 OperatableStateMachine.add('say closer', SaraSay(sentence="I need to get a bit closer.", input_keys=[], emotion=1, block=False), transitions={'done': 'move arm'}, autonomy={'done': Autonomy.Off}) # x:26 y:541 OperatableStateMachine.add('Group', _sm_group_0, transitions={'done': 'wait'}, autonomy={'done': Autonomy.Inherit}, remapping={'pose_out': 'pose_out'}) # x:33 y:625 OperatableStateMachine.add('wait', WaitState(wait_time=2), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:59 y:308, x:447 y:59, x:384 y:162 _sm_check_form_2 = OperatableStateMachine(outcomes=['done', 'fail_full', 'full_no_object'], input_keys=['Action']) with _sm_check_form_2: # x:31 y:40 OperatableStateMachine.add('check if gripper full', GetRosParam(ParamName="behavior/Gripper_Content"), transitions={'done': 'Say_Full', 'failed': 'cond'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'Value': 'ObjectInGripper'}) # x:30 y:121 OperatableStateMachine.add('cond', CheckConditionState(predicate=lambda x: x[1] == ''), transitions={'true': 'not told', 'false': 'done'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'Action'}) # x:222 y:119 OperatableStateMachine.add('not told', SaraSay(sentence="Hum! They didn't told me what to pick", input_keys=[], emotion=1, block=True), transitions={'done': 'full_no_object'}, autonomy={'done': Autonomy.Off}) # x:242 y:31 OperatableStateMachine.add('Say_Full', SaraSay(sentence=lambda x: "Wait. There is already a "+ x + "in my gripper.", input_keys=[], emotion=0, block=True), transitions={'done': 'fail_full'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:84 y:30 OperatableStateMachine.add('Check Form', _sm_check_form_2, transitions={'done': 'get name', 'fail_full': 'cause1', 'full_no_object': 'cause2'}, autonomy={'done': Autonomy.Inherit, 'fail_full': Autonomy.Inherit, 'full_no_object': Autonomy.Inherit}, remapping={'Action': 'Action'}) # x:28 y:452 OperatableStateMachine.add('Action_pick', self.use_behavior(sara_flexbe_behaviors__Action_pickSM, 'Action_pick'), transitions={'success': 'Got_It', 'unreachable': 'for 1', 'not found': 'Say_lost', 'dropped': 'say missed'}, autonomy={'success': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'not found': Autonomy.Inherit, 'dropped': Autonomy.Inherit}, remapping={'objectID': 'ID'}) # x:261 y:239 OperatableStateMachine.add('Get closer', _sm_get_closer_1, transitions={'done': 'Action_find'}, autonomy={'done': Autonomy.Inherit}, remapping={'Object': 'Object'}) # x:275 y:333 OperatableStateMachine.add('for 1', ForLoop(repeat=1), transitions={'do': 'Get closer', 'end': 'say giveup'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:416 y:264 OperatableStateMachine.add('say giveup', SaraSay(sentence="I give up", input_keys=[], emotion=1, block=True), transitions={'done': 'cause4'}, autonomy={'done': Autonomy.Off}) # x:284 y:496 OperatableStateMachine.add('say missed', SaraSay(sentence="Oops! I missed.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause4'}, autonomy={'done': Autonomy.Off}) # x:469 y:495 OperatableStateMachine.add('set param', SetRosParam(ParamName="behavior/GripperContent"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'ObjectName'}) # x:82 y:115 OperatableStateMachine.add('get name', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Action_find'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Action', 'output_value': 'ObjectName'}) # x:511 y:20 OperatableStateMachine.add('cause1', SetKey(Value="My gripper was already full."), transitions={'done': 'setrosparam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:512 y:81 OperatableStateMachine.add('cause2', SetKey(Value="I didn't know what to pick."), transitions={'done': 'setrosparam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:511 y:143 OperatableStateMachine.add('cause3', SetKey(Value="I didn't found the object."), transitions={'done': 'setrosparam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:690 y:197 OperatableStateMachine.add('setrosparam', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:605 y:312 OperatableStateMachine.add('cause4', SetKey(Value="I was unable to pick the object."), transitions={'done': 'setrosparam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:30 y:188 OperatableStateMachine.add('Action_find', self.use_behavior(sara_flexbe_behaviors__Action_findSM, 'Action_find'), transitions={'done': 'getID', 'failed': 'cause3'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'className': 'ObjectName', 'entity': 'Object'}) # x:49 y:322 OperatableStateMachine.add('getID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'Action_pick'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Object', 'output_value': 'ID'}) # x:284 y:422 OperatableStateMachine.add('Say_lost', SaraSay(sentence=lambda x: "Hum! I lost sight of the "+x, input_keys=[], emotion=0, block=True), transitions={'done': 'cause4'}, autonomy={'done': Autonomy.Off}) # x:281 y:572 OperatableStateMachine.add('Got_It', SaraSay(sentence=lambda x: "I have the "+x, input_keys=[], emotion=0, block=True), transitions={'done': 'set param'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:860 y:152, x:755 y:568 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['pose']) _state_machine.userdata.pose = "crowd" _state_machine.userdata.name = "destination" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:130 y:365 _sm_look_around_0 = OperatableStateMachine(outcomes=['failed']) with _sm_look_around_0: # x:78 y:40 OperatableStateMachine.add('set pitch', SetKey(Value=0.6), transitions={'done': 'get angle'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'pitch'}) # x:348 y:210 OperatableStateMachine.add('set head', SaraSetHeadAngleKey(), transitions={'done': 'get angle'}, autonomy={'done': Autonomy.Off}, remapping={ 'yaw': 'yaw', 'pitch': 'pitch' }) # x:204 y:248 OperatableStateMachine.add( 'limit yaw', CalculationState(calculation=lambda x: max(min(x, 1), -1)), transitions={'done': 'set head'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'yaw', 'output_value': 'yaw' }) # x:191 y:126 OperatableStateMachine.add('get angle', GetClosestObstacle(topic="/scan", maximumDistance=2), transitions={'done': 'limit yaw'}, autonomy={'done': Autonomy.Off}, remapping={ 'Angle': 'yaw', 'distance': 'distance', 'position': 'position' }) # x:30 y:365, x:130 y:365 _sm_move_1 = OperatableStateMachine(outcomes=['arrived', 'failed'], input_keys=['pose']) with _sm_move_1: # x:95 y:122 OperatableStateMachine.add('move', SaraMoveBase(reference="map"), transitions={ 'arrived': 'arrived', 'failed': 'failed' }, autonomy={ 'arrived': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'pose': 'pose'}) # x:259 y:573, x:491 y:362, x:491 y:453 _sm_manage_name_2 = OperatableStateMachine( outcomes=['done', 'too much', 'not found'], input_keys=['pose'], output_keys=['pose', 'name']) with _sm_manage_name_2: # x:39 y:48 OperatableStateMachine.add( 'check if Pose', CheckConditionState(predicate=lambda x: type(x) is type([])), transitions={ 'true': 'getname', 'false': 'check if string' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'pose'}) # x:257 y:264 OperatableStateMachine.add( 'getcontainers', CalculationState(calculation=lambda x: x[1:]), transitions={'done': 'get wonderland entity'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'pose', 'output_value': 'containers' }) # x:232 y:354 OperatableStateMachine.add('get wonderland entity', WonderlandGetEntityVerbal(), transitions={ 'one': 'get waypoint', 'multiple': 'too much', 'none': 'not found', 'error': 'not found' }, autonomy={ 'one': Autonomy.Off, 'multiple': Autonomy.Off, 'none': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'name': 'name', 'containers': 'containers', 'entities': 'entities', 'firstEntity': 'firstEntity' }) # x:238 y:442 OperatableStateMachine.add('get waypoint', GetAttribute(attributes=["waypoint"]), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={ 'object': 'firstEntity', 'waypoint': 'pose' }) # x:256 y:166 OperatableStateMachine.add( 'getname', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'getcontainers'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'pose', 'output_value': 'name' }) # x:42 y:180 OperatableStateMachine.add( 'check if string', CheckConditionState(predicate=lambda x: type(x) is type("")), transitions={ 'true': 'remap to name', 'false': 'done' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'pose'}) # x:40 y:251 OperatableStateMachine.add( 'remap to name', CalculationState(calculation=lambda x: x), transitions={'done': 'set containers empty'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'pose', 'output_value': 'name' }) # x:30 y:326 OperatableStateMachine.add( 'set containers empty', SetKey(Value=[]), transitions={'done': 'get wonderland entity'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'containers'}) # x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365 _sm_move_concurent_3 = ConcurrencyContainer( outcomes=['arrived', 'failed'], input_keys=['pose'], conditions=[('arrived', [('Move', 'arrived')]), ('failed', [('Move', 'failed')]), ('failed', [('Look around', 'failed')])]) with _sm_move_concurent_3: # x:30 y:40 OperatableStateMachine.add('Move', _sm_move_1, transitions={ 'arrived': 'arrived', 'failed': 'failed' }, autonomy={ 'arrived': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'pose'}) # x:268 y:74 OperatableStateMachine.add('Look around', _sm_look_around_0, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) with _state_machine: # x:54 y:27 OperatableStateMachine.add('SetCount', SetKey(Value=2), transitions={'done': 'manage name'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Count'}) # x:258 y:250 OperatableStateMachine.add('stuck', SaraSay(sentence="I'm getting stuck.", input_keys=[], emotion=2, block=True), transitions={'done': 'Count--'}, autonomy={'done': Autonomy.Off}) # x:49 y:251 OperatableStateMachine.add('try again', SaraSay(sentence="But I'm still going.", input_keys=[], emotion=1, block=False), transitions={'done': 'Move concurent'}, autonomy={'done': Autonomy.Off}) # x:360 y:508 OperatableStateMachine.add( 'sorry', SaraSay(sentence="Well. It seem's I can't go there.", input_keys=[], emotion=2, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:672 y:147 OperatableStateMachine.add('set blink', SetExpression(emotion=6, brightness=-1), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:252 y:146 OperatableStateMachine.add('Move concurent', _sm_move_concurent_3, transitions={ 'arrived': 'reset head', 'failed': 'stuck' }, autonomy={ 'arrived': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'pose'}) # x:254 y:353 OperatableStateMachine.add( 'Count--', CalculationState(calculation=lambda x: x - 1), transitions={'done': 'check count'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Count', 'output_value': 'Count' }) # x:37 y:351 OperatableStateMachine.add( 'check count', CheckConditionState(predicate=lambda x: x >= 0), transitions={ 'true': 'try again', 'false': 'sorry' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'Count'}) # x:497 y:149 OperatableStateMachine.add('reset head', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'set blink'}, autonomy={'done': Autonomy.Off}) # x:269 y:21 OperatableStateMachine.add('manage name', _sm_manage_name_2, transitions={ 'done': 'set head', 'too much': 'say too much', 'not found': 'say not known' }, autonomy={ 'done': Autonomy.Inherit, 'too much': Autonomy.Inherit, 'not found': Autonomy.Inherit }, remapping={ 'pose': 'pose', 'name': 'name' }) # x:46 y:147 OperatableStateMachine.add('set head', SaraSetHeadAngle(pitch=0.8, yaw=0), transitions={'done': 'Move concurent'}, autonomy={'done': Autonomy.Off}) # x:477 y:333 OperatableStateMachine.add( 'say too much', SaraSay( sentence=lambda x: "There is more than one " + x[0] + ".", input_keys=["poseName"], emotion=3, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'poseName': 'name'}) # x:445 y:418 OperatableStateMachine.add( 'say not known', SaraSay(sentence=lambda x: "I don't know where the " + x[0] + " is.", input_keys=["poseName"], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'poseName': 'name'}) return _state_machine
def create(self): # x:194 y:348, x:15 y:144 _state_machine = OperatableStateMachine(outcomes=['Done', 'Fail'], input_keys=['DoorName']) _state_machine.userdata.DoorName = "door" _state_machine.userdata.otherSide = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:324 _sm_manage_door_opening_0 = OperatableStateMachine( outcomes=['done'], output_keys=['otherSide']) with _sm_manage_door_opening_0: # x:41 y:40 OperatableStateMachine.add('Wait for door 1', DoorDetector(timeout=1), transitions={ 'done': 'genfront', 'failed': 'door closed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }) # x:199 y:43 OperatableStateMachine.add( 'door closed', SaraSay(sentence="This door is closed. I need an human.", input_keys=[], emotion=1, block=True), transitions={'done': 'wait for door 2'}, autonomy={'done': Autonomy.Off}) # x:30 y:151 OperatableStateMachine.add('genfront', GenPoseEuler(x=1.5, y=0, z=0, roll=0, pitch=0, yaw=0), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'otherSide'}) # x:357 y:44 OperatableStateMachine.add('wait for door 2', DoorDetector(timeout=5), transitions={ 'done': 'Thanks', 'failed': 'callhelp' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }) # x:534 y:45 OperatableStateMachine.add( 'callhelp', SaraSay(sentence="Can someone open this door for me. Please.", input_keys=[], emotion=1, block=True), transitions={'done': 'wait for door 2'}, autonomy={'done': Autonomy.Off}) # x:348 y:156 OperatableStateMachine.add('Thanks', SaraSay(sentence="Thank you!", input_keys=[], emotion=1, block=True), transitions={'done': 'genfront'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:155 y:30 OperatableStateMachine.add( 'Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'Action_Move'), transitions={ 'finished': 'Manage door opening', 'failed': 'Fail' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'DoorName'}) # x:154 y:132 OperatableStateMachine.add( 'Manage door opening', _sm_manage_door_opening_0, transitions={'done': 'move to the other side'}, autonomy={'done': Autonomy.Inherit}, remapping={'otherSide': 'otherSide'}) # x:160 y:234 OperatableStateMachine.add('move to the other side', SaraMoveBase(reference="base_link"), transitions={ 'arrived': 'Done', 'failed': 'Fail' }, autonomy={ 'arrived': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'pose': 'otherSide'}) return _state_machine
def create(self): # x:783 y:268, x:448 y:352, x:564 y:354 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = [ "say", "coucou Philippe commit ca va" ] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:55 y:97 OperatableStateMachine.add( 'calc', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'rosparamkey'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Action', 'output_value': 'sentence' }) # x:271 y:123 OperatableStateMachine.add('rosparamkey', GetRosParamKey(), transitions={ 'done': 'Sara_Speak', 'failed': 'Sara_Failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'ParamName': 'sentence', 'Value': 'sentenceToSay' }) # x:483 y:96 OperatableStateMachine.add('Sara_Speak', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:473 y:212 OperatableStateMachine.add('Sara_Failed', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:763 y:423, x:866 y:181, x:896 y:268 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Follow", "rachel"] _state_machine.userdata.distance = 1 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:368 _sm_wait_stop_0 = OperatableStateMachine(outcomes=['finished']) with _sm_wait_stop_0: # x:112 y:116 OperatableStateMachine.add('Get Command', GetSpeech(watchdog=1000), transitions={ 'done': 'Command Stop', 'nothing': 'Get Command', 'fail': 'Get Command' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) # x:115 y:270 OperatableStateMachine.add( 'Command Stop', CheckConditionState(predicate=lambda x: True in [ True for match in ['stop', 'arrived', 'reached', 'here', 'ok'] if match in x ]), transitions={ 'true': 'finished', 'false': 'Get Command' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'words'}) # x:395 y:153, x:390 y:47, x:359 y:362, x:304 y:368 _sm_follow_loop_1 = ConcurrencyContainer( outcomes=['finished', 'error'], input_keys=['ID', 'distance'], conditions=[('finished', [('Wait Stop', 'finished')]), ('error', [('Action_follow', 'failed')])]) with _sm_follow_loop_1: # x:190 y:37 OperatableStateMachine.add( 'Action_follow', self.use_behavior(sara_flexbe_behaviors__Action_followSM, 'Follow Loop/Action_follow'), transitions={'failed': 'error'}, autonomy={'failed': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:190 y:139 OperatableStateMachine.add('Wait Stop', _sm_wait_stop_0, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}) with _state_machine: # x:85 y:51 OperatableStateMachine.add( 'GetName', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Get Person ID'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Action', 'output_value': 'name' }) # x:97 y:141 OperatableStateMachine.add( 'Get Person ID', GetRosParam(ParamName="/behavior/FoundPerson/Id"), transitions={ 'done': 'Get Entity Location', 'failed': 'Say_Lost' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'personId'}) # x:92 y:225 OperatableStateMachine.add('Get Entity Location', GetEntityByID(), transitions={ 'found': 'Tell_Follow', 'not_found': 'Say_Lost' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'personId', 'Entity': 'Entity' }) # x:231 y:389 OperatableStateMachine.add('Tell Way', SaraSay(sentence="Show me the way !", input_keys=[], emotion=1, block=True), transitions={'done': 'Follow Loop'}, autonomy={'done': Autonomy.Off}) # x:391 y:374 OperatableStateMachine.add('Follow Loop', _sm_follow_loop_1, transitions={ 'finished': 'Stop_Follow', 'error': 'Cant_Follow' }, autonomy={ 'finished': Autonomy.Inherit, 'error': Autonomy.Inherit }, remapping={ 'ID': 'personId', 'distance': 'distance' }) # x:533 y:140 OperatableStateMachine.add( 'cause1', SetKey(Value="I lost the person I was following."), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:540 y:219 OperatableStateMachine.add( 'cause2', SetKey(Value="I was unable to follow the person."), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:697 y:178 OperatableStateMachine.add( 'setrosparamcause', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:96 y:341 OperatableStateMachine.add( 'Tell_Follow', SaraSay(sentence=lambda x: "I will follow you, " + x[0] + " !", input_keys=["name"], emotion=0, block=True), transitions={'done': 'Tell Way'}, autonomy={'done': Autonomy.Off}, remapping={'name': 'name'}) # x:571 y:411 OperatableStateMachine.add( 'Stop_Follow', SaraSay(sentence=lambda x: "Ok " + x[0] + ", I will stop to follow you !", input_keys=["name"], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'name': 'name'}) # x:359 y:262 OperatableStateMachine.add( 'Cant_Follow', SaraSay( sentence=lambda x: "I can't follow you, " + x[0] + " !", input_keys=["name"], emotion=0, block=True), transitions={'done': 'cause2'}, autonomy={'done': Autonomy.Off}, remapping={'name': 'name'}) # x:345 y:149 OperatableStateMachine.add( 'Say_Lost', SaraSay(sentence=lambda x: "I have lost " + x[0] + " !", input_keys=["name"], emotion=0, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}, remapping={'name': 'name'}) return _state_machine
def create(self): # x:391 y:586, x:438 y:304 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.StartPosition = "Operator" _state_machine.userdata.PositionBras = "IdlePose" _state_machine.userdata.title = "GPSR" _state_machine.userdata.EntryName = "door1/enter" _state_machine.userdata.ExitName = "door2/exit" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:135 y:542, x:527 y:395 _sm_validate_0 = OperatableStateMachine(outcomes=['done', 'bad'], input_keys=['sentence']) with _sm_validate_0: # x:56 y:62 OperatableStateMachine.add( 'Say_Command', SaraSay( sentence=lambda x: "I heard. " + x + ". Is that correct?", input_keys=[], emotion=0, block=True), transitions={'done': 'get speech'}, autonomy={'done': Autonomy.Off}) # x:44 y:165 OperatableStateMachine.add('get speech', GetSpeech(watchdog=10), transitions={ 'done': 'is yes', 'nothing': 'say repeate', 'fail': 'say repeate' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) # x:91 y:307 OperatableStateMachine.add( 'is yes', CheckConditionState(predicate=lambda x: "yes" in x or "right" in x or "sure" in x), transitions={ 'true': 'done', 'false': 'say repeate' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'words'}) # x:423 y:332 OperatableStateMachine.add( 'say repeate', SaraSay(sentence="Please, repeat your command.", input_keys=[], emotion=1, block=True), transitions={'done': 'bad'}, autonomy={'done': Autonomy.Off}) # x:30 y:324 _sm_look_at_op_1 = OperatableStateMachine(outcomes=['fail']) with _sm_look_at_op_1: # x:61 y:31 OperatableStateMachine.add('set name', SetKey(Value="person"), transitions={'done': 'list persons'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:44 y:110 OperatableStateMachine.add('list persons', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'get closest', 'none_found': 'list persons' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'name', 'entity_list': 'entity_list', 'number': 'number' }) # x:43 y:211 OperatableStateMachine.add( 'get closest', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'look at'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'ID' }) # x:210 y:198 OperatableStateMachine.add('look at', KeepLookingAt(), transitions={'failed': 'list persons'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:307 y:35, x:335 y:491 _sm_get_commands_2 = OperatableStateMachine( outcomes=['fail', 'understood'], output_keys=['ActionForms', 'sentence']) with _sm_get_commands_2: # x:50 y:48 OperatableStateMachine.add( 'say ready', SaraSay(sentence="I'm ready for your commands.", input_keys=[], emotion=1, block=False), transitions={'done': 'GetSpeech'}, autonomy={'done': Autonomy.Off}) # x:32 y:408 OperatableStateMachine.add('SaraNLUgpsr', SaraNLUgpsr(), transitions={ 'understood': 'say understood', 'not_understood': 'say sorry', 'fail': 'say sorry' }, autonomy={ 'understood': Autonomy.Off, 'not_understood': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'sentence': 'sentence', 'ActionForms': 'ActionForms' }) # x:597 y:223 OperatableStateMachine.add( 'say sorry', SaraSay( sentence="Sorry, I could not understand what you said.", input_keys=[], emotion=1, block=True), transitions={'done': 'GetSpeech'}, autonomy={'done': Autonomy.Off}) # x:30 y:491 OperatableStateMachine.add('say understood', SaraSay(sentence="Ok", input_keys=[], emotion=1, block=True), transitions={'done': 'understood'}, autonomy={'done': Autonomy.Off}) # x:44 y:131 OperatableStateMachine.add('GetSpeech', GetSpeech(watchdog=5), transitions={ 'done': 'validate', 'nothing': 'GetSpeech', 'fail': 'fail' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'sentence'}) # x:29 y:259 OperatableStateMachine.add('validate', _sm_validate_0, transitions={ 'done': 'SaraNLUgpsr', 'bad': 'GetSpeech' }, autonomy={ 'done': Autonomy.Inherit, 'bad': Autonomy.Inherit }, remapping={'sentence': 'sentence'}) # x:122 y:413 _sm_end_3 = OperatableStateMachine(outcomes=['done'], input_keys=['ExitName']) with _sm_end_3: # x:30 y:40 OperatableStateMachine.add( 'win', SaraSay(sentence="Thank you. I'm going now.", input_keys=[], emotion=1, block=True), transitions={'done': 'Action_Pass_Door'}, autonomy={'done': Autonomy.Off}) # x:33 y:163 OperatableStateMachine.add('Action_Pass_Door', self.use_behavior( Action_Pass_DoorSM, 'End/Action_Pass_Door'), transitions={ 'Done': 'say yay', 'Fail': 'done' }, autonomy={ 'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit }, remapping={'DoorName': 'ExitName'}) # x:210 y:262 OperatableStateMachine.add( 'say yay', SaraSay(sentence="I did it. I'm the best robot.", input_keys=[], emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:320 y:82, x:322 y:143, x:265 y:407, x:306 y:225, x:430 y:324 _sm_interact_operator_4 = ConcurrencyContainer( outcomes=['fail', 'understood'], output_keys=['ActionForms', 'sentence'], conditions=[('understood', [('Get Commands', 'understood')]), ('fail', [('Get Commands', 'fail')]), ('fail', [('look at op', 'fail')])]) with _sm_interact_operator_4: # x:95 y:45 OperatableStateMachine.add('Get Commands', _sm_get_commands_2, transitions={ 'fail': 'fail', 'understood': 'understood' }, autonomy={ 'fail': Autonomy.Inherit, 'understood': Autonomy.Inherit }, remapping={ 'ActionForms': 'ActionForms', 'sentence': 'sentence' }) # x:99 y:198 OperatableStateMachine.add('look at op', _sm_look_at_op_1, transitions={'fail': 'fail'}, autonomy={'fail': Autonomy.Inherit}) # x:325 y:387, x:314 y:190 _sm_fail_state_5 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['StartPosition']) with _sm_fail_state_5: # x:36 y:29 OperatableStateMachine.add( 'say failed', SaraSay(sentence="I failed. I'm going back to tell my master.", input_keys=[], emotion=1, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:42 y:301 OperatableStateMachine.add( 'get error', GetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={ 'done': 'Say_Error', 'failed': 'finished' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'Error'}) # x:48 y:414 OperatableStateMachine.add( 'Say_Error', SaraSay(sentence=lambda x: "Sorry, I failed because " + x, input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:25 y:172 OperatableStateMachine.add('Action_Move', self.use_behavior( Action_MoveSM, 'Fail state/Action_Move'), transitions={ 'finished': 'get error', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'StartPosition'}) # x:588 y:141, x:590 y:545, x:642 y:410 _sm_do_the_actions_6 = OperatableStateMachine( outcomes=['finished', 'failed', 'critical fail'], input_keys=['ActionForms', 'OriginalPose']) with _sm_do_the_actions_6: # x:85 y:33 OperatableStateMachine.add('set i', SetKey(Value=0), transitions={'done': 'is form?'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'index'}) # x:48 y:339 OperatableStateMachine.add( 'GetForm', FlexibleCalculationState(calculation=lambda x: x[0][x[1]], input_keys=["ActionForms", "index"]), transitions={'done': 'Action_Executor'}, autonomy={'done': Autonomy.Off}, remapping={ 'ActionForms': 'ActionForms', 'index': 'index', 'output_value': 'ActionForm' }) # x:57 y:131 OperatableStateMachine.add( 'is form?', FlexibleCheckConditionState( predicate=lambda x: x[1] < len(x[0]), input_keys=["ActionForms", "index"]), transitions={ 'true': 'set setp', 'false': 'finished' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'ActionForms': 'ActionForms', 'index': 'index' }) # x:204 y:237 OperatableStateMachine.add( '++i', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'is form?'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'index', 'output_value': 'index' }) # x:72 y:479 OperatableStateMachine.add('Action_Executor', self.use_behavior( Action_ExecutorSM, 'Do the actions/Action_Executor'), transitions={ 'finished': '++i', 'failed': 'failed', 'critical_fail': 'critical fail' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit }, remapping={'Action': 'ActionForm'}) # x:45 y:241 OperatableStateMachine.add('set setp', StoryboardSetStepKey(), transitions={'done': 'GetForm'}, autonomy={'done': Autonomy.Off}, remapping={'step': 'index'}) # x:424 y:175 _sm_initialisation_7 = OperatableStateMachine( outcomes=['done'], input_keys=['PositionBras', 'EntryName']) with _sm_initialisation_7: # x:62 y:79 OperatableStateMachine.add('Action_Pass_Door', self.use_behavior( Action_Pass_DoorSM, 'Initialisation/Action_Pass_Door'), transitions={ 'Done': 'set step', 'Fail': 'done' }, autonomy={ 'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit }, remapping={'DoorName': 'EntryName'}) # x:70 y:306 OperatableStateMachine.add('set story', Set_Story(titre="GPSR", storyline=[]), transitions={'done': 'bras en lair'}, autonomy={'done': Autonomy.Off}) # x:288 y:375 OperatableStateMachine.add( 'say start', SaraSay(sentence="I'm ready to start the GPSR scenario.", input_keys=[], emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:78 y:205 OperatableStateMachine.add('set step', Set_a_step(step=0), transitions={'done': 'set story'}, autonomy={'done': Autonomy.Off}) # x:101 y:395 OperatableStateMachine.add('bras en lair', MoveitMove(move=True, waitForExecution=False, group="RightArm", watchdog=15), transitions={ 'done': 'say start', 'failed': 'say start' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'PositionBras'}) with _state_machine: # x:33 y:103 OperatableStateMachine.add('Initialisation', _sm_initialisation_7, transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'PositionBras': 'PositionBras', 'EntryName': 'EntryName' }) # x:767 y:196 OperatableStateMachine.add('Do the actions', _sm_do_the_actions_6, transitions={ 'finished': 'say succseed', 'failed': 'Fail state', 'critical fail': 'critical' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical fail': Autonomy.Inherit }, remapping={ 'ActionForms': 'ActionForms', 'OriginalPose': 'OriginalPose' }) # x:250 y:126 OperatableStateMachine.add( 'Fail state', _sm_fail_state_5, transitions={ 'finished': 'lift head', 'failed': 'critical' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'StartPosition': 'StartPosition'}) # x:250 y:257 OperatableStateMachine.add( 'critical', SaraSay( sentence="Critical failure! I require medical assistance.", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:52 y:468 OperatableStateMachine.add('for 3', ForLoop(repeat=3), transitions={ 'do': 'GetOriginalPose', 'end': 'End' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:747 y:475 OperatableStateMachine.add('say succseed', SaraSay( sentence="I succeed my mission.", input_keys=[], emotion=1, block=True), transitions={'done': 'for 3'}, autonomy={'done': Autonomy.Off}) # x:204 y:368 OperatableStateMachine.add('Interact operator', _sm_interact_operator_4, transitions={ 'fail': 'critical', 'understood': 'set step' }, autonomy={ 'fail': Autonomy.Inherit, 'understood': Autonomy.Inherit }, remapping={ 'ActionForms': 'ActionForms', 'sentence': 'sentence' }) # x:553 y:316 OperatableStateMachine.add('set story', StoryboardSetStoryFromAction(), transitions={'done': 'Do the actions'}, autonomy={'done': Autonomy.Off}, remapping={ 'titre': 'title', 'actionList': 'ActionForms' }) # x:404 y:377 OperatableStateMachine.add('set step', Set_a_step(step=0), transitions={'done': 'set story'}, autonomy={'done': Autonomy.Off}) # x:41 y:375 OperatableStateMachine.add( 'GetOriginalPose', Get_Robot_Pose(), transitions={'done': 'Interact operator'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'OriginalPose'}) # x:37 y:564 OperatableStateMachine.add('End', _sm_end_3, transitions={'done': 'finished'}, autonomy={'done': Autonomy.Inherit}, remapping={'ExitName': 'ExitName'}) # x:30 y:288 OperatableStateMachine.add('lift head', SaraSetHeadAngle(pitch=-0.3, yaw=0), transitions={'done': 'GetOriginalPose'}, autonomy={'done': Autonomy.Off}) # x:48 y:23 OperatableStateMachine.add('ContinueButton', ContinueButton(), transitions={ 'true': 'Initialisation', 'false': 'Initialisation' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }) # x:21 y:185 OperatableStateMachine.add('Action_Move', self.use_behavior( Action_MoveSM, 'Action_Move'), transitions={ 'finished': 'lift head', 'failed': 'critical' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'StartPosition'}) return _state_machine
def create(self): # x:818 y:151, x:162 y:460 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['object']) _state_machine.userdata.object = "snacks" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:124 y:77 OperatableStateMachine.add('ClosestObject', ClosestObject(), transitions={ 'found': 'CheckAngle', 'not_found': 'failed', 'non_existant': 'failed' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off, 'non_existant': Autonomy.Off }, remapping={ 'object': 'object', 'angle': 'angle', 'closestObject': 'closestObject' }) # x:342 y:132 OperatableStateMachine.add( 'CheckAngle', CheckConditionState(predicate=lambda x: x > 0), transitions={ 'true': 'left', 'false': 'sayright' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'angle'}) # x:552 y:92 OperatableStateMachine.add( 'left', SaraSay(sentence=lambda x: "The " + str(x[1]) + " is on the left of the" + str(x[0].name), input_keys=["object", "closestObject"], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'object': 'object', 'closestObject': 'closestObject' }) # x:441 y:296 OperatableStateMachine.add( 'sayright', SaraSay(sentence=lambda x: "The " + str(x[1]) + " is on the right of the " + str(x[0].name), input_keys=["object", "closestObject"], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'object': 'object', 'closestObject': 'closestObject' }) return _state_machine
def create(self): # x:1023 y:473, x:791 y:291, x:797 y:103 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Guide", 'table', 'kitchen'] _state_machine.userdata.relative = False # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:458 _sm_groupwait_0 = OperatableStateMachine(outcomes=['end']) with _sm_groupwait_0: # x:30 y:40 OperatableStateMachine.add('waitwait', WaitState(wait_time=20), transitions={'done': 'end'}, autonomy={'done': Autonomy.Off}) # x:534 y:319 _sm_move_head_and_base_at_the_end_1 = OperatableStateMachine( outcomes=['failed']) with _sm_move_head_and_base_at_the_end_1: # x:52 y:31 OperatableStateMachine.add('setkeyorientation', SetKey(Value=1.5), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:51 y:114 OperatableStateMachine.add( 'action_turn', self.use_behavior( sara_flexbe_behaviors__action_turnSM, 'operator is still there/Move head and base end /move head and base at the end/action_turn' ), transitions={ 'finished': 'turn right head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:48 y:272 OperatableStateMachine.add('wait while head turn', WaitState(wait_time=4), transitions={'done': 'action_turn_2'}, autonomy={'done': Autonomy.Off}) # x:47 y:199 OperatableStateMachine.add( 'turn right head', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'wait while head turn'}, autonomy={'done': Autonomy.Off}) # x:45 y:348 OperatableStateMachine.add( 'action_turn_2', self.use_behavior( sara_flexbe_behaviors__action_turnSM, 'operator is still there/Move head and base end /move head and base at the end/action_turn_2' ), transitions={ 'finished': 'left to rigth', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:43 y:592 OperatableStateMachine.add('left to rigth', SaraSetHeadAngle(pitch=0, yaw=-1.57), transitions={'done': 'waitwait1'}, autonomy={'done': Autonomy.Off}) # x:420 y:602 OperatableStateMachine.add('right to left', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'waitwait2'}, autonomy={'done': Autonomy.Off}) # x:266 y:541 OperatableStateMachine.add('waitwait1', WaitState(wait_time=8), transitions={'done': 'right to left'}, autonomy={'done': Autonomy.Off}) # x:236 y:674 OperatableStateMachine.add('waitwait2', WaitState(wait_time=8), transitions={'done': 'left to rigth'}, autonomy={'done': Autonomy.Off}) # x:231 y:538 _sm_find_a_human_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID']) with _sm_find_a_human_2: # x:78 y:275 OperatableStateMachine.add('find the human', GetEntityByID(), transitions={ 'found': 'finished', 'not_found': 'find the human' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:415 y:99, x:318 y:246, x:442 y:295 _sm_move_head_and_base_end__3 = ConcurrencyContainer( outcomes=['failed'], conditions=[('failed', [('move head and base at the end', 'failed') ]), ('failed', [('Groupwait', 'end')])]) with _sm_move_head_and_base_end__3: # x:132 y:57 OperatableStateMachine.add('move head and base at the end', _sm_move_head_and_base_at_the_end_1, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:121 y:218 OperatableStateMachine.add('Groupwait', _sm_groupwait_0, transitions={'end': 'failed'}, autonomy={'end': Autonomy.Inherit}) # x:67 y:227 _sm_export_no_waypoint_4 = OperatableStateMachine( outcomes=['done'], output_keys=['waipoint', 'area_name']) with _sm_export_no_waypoint_4: # x:30 y:40 OperatableStateMachine.add('noWaypoint', SetKey(Value=None), transitions={'done': 'NoName'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'waipoint'}) # x:43 y:129 OperatableStateMachine.add('NoName', SetKey(Value=None), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'area_name'}) # x:30 y:458 _sm_export_waypoint_5 = OperatableStateMachine( outcomes=['done'], input_keys=['entity'], output_keys=['waipoint', 'area_name']) with _sm_export_waypoint_5: # x:58 y:107 OperatableStateMachine.add( 'Extract Wayppoint', CalculationState(calculation=lambda x: x.waypoint), transitions={'done': 'Extract Name'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'waipoint' }) # x:67 y:257 OperatableStateMachine.add( 'Extract Name', CalculationState(calculation=lambda x: x.name), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'area_name' }) # x:379 y:221 _sm_wait_to_compte_6 = OperatableStateMachine(outcomes=['finished']) with _sm_wait_to_compte_6: # x:77 y:195 OperatableStateMachine.add('one more wait', WaitState(wait_time=60), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:535 y:314 _sm_turn_around_7 = OperatableStateMachine(outcomes=['failed']) with _sm_turn_around_7: # x:47 y:45 OperatableStateMachine.add('set orientation', SetKey(Value=1.57), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:42 y:213 OperatableStateMachine.add('move head', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'waitwait'}, autonomy={'done': Autonomy.Off}) # x:53 y:294 OperatableStateMachine.add('waitwait', WaitState(wait_time=10), transitions={'done': 'action_turn_2'}, autonomy={'done': Autonomy.Off}) # x:33 y:122 OperatableStateMachine.add( 'action_turn', self.use_behavior( sara_flexbe_behaviors__action_turnSM, 'Try to reach/check person behind/move head and base/turn around/action_turn' ), transitions={ 'finished': 'move head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:31 y:434 OperatableStateMachine.add( 'action_turn_2', self.use_behavior( sara_flexbe_behaviors__action_turnSM, 'Try to reach/check person behind/move head and base/turn around/action_turn_2' ), transitions={ 'finished': 'head left right', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:35 y:595 OperatableStateMachine.add('head left right', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'wait turn head'}, autonomy={'done': Autonomy.Off}) # x:308 y:537 OperatableStateMachine.add('wait turn head', WaitState(wait_time=10), transitions={'done': 'head right left'}, autonomy={'done': Autonomy.Off}) # x:495 y:604 OperatableStateMachine.add('head right left', SaraSetHeadAngle(pitch=0, yaw=-1.57), transitions={'done': 'wait wait wait'}, autonomy={'done': Autonomy.Off}) # x:271 y:711 OperatableStateMachine.add('wait wait wait', WaitState(wait_time=10), transitions={'done': 'head left right'}, autonomy={'done': Autonomy.Off}) # x:845 y:395 _sm_find_human_8 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID']) with _sm_find_human_8: # x:223 y:137 OperatableStateMachine.add('find person', GetEntityByID(), transitions={ 'found': 'finished', 'not_found': 'find person' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:549 y:135, x:555 y:269, x:230 y:458 _sm_move_head_and_base_9 = ConcurrencyContainer( outcomes=['failed'], conditions=[('failed', [('turn around', 'failed')]), ('failed', [('wait to compte', 'finished')])]) with _sm_move_head_and_base_9: # x:268 y:77 OperatableStateMachine.add('turn around', _sm_turn_around_7, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:263 y:246 OperatableStateMachine.add('wait to compte', _sm_wait_to_compte_6, transitions={'finished': 'failed'}, autonomy={'finished': Autonomy.Inherit}) # x:493 y:206 _sm_container_10 = OperatableStateMachine(outcomes=['check']) with _sm_container_10: # x:230 y:160 OperatableStateMachine.add('wait long', WaitState(wait_time=40), transitions={'done': 'check'}, autonomy={'done': Autonomy.Off}) # x:30 y:458, x:706 y:447 _sm_navigate_to_the_point_11 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['pose']) with _sm_navigate_to_the_point_11: # x:174 y:122 OperatableStateMachine.add('set relative', SetKey(Value=False), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'relative'}) # x:347 y:191 OperatableStateMachine.add( 'Action_Move', self.use_behavior( sara_flexbe_behaviors__Action_MoveSM, 'Try to reach/Container/navigate to the point/Action_Move' ), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'pose'}) # x:728 y:335, x:792 y:103, x:738 y:249, x:724 y:448 _sm_check_person_behind_12 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['ID'], conditions=[('failed', [('move head and base', 'failed')]), ('finished', [('find human', 'finished')])]) with _sm_check_person_behind_12: # x:250 y:72 OperatableStateMachine.add('move head and base', _sm_move_head_and_base_9, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:261 y:238 OperatableStateMachine.add('find human', _sm_find_human_8, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:635 y:61, x:634 y:159, x:597 y:300, x:330 y:458, x:430 y:458, x:530 y:458 _sm_container_13 = ConcurrencyContainer( outcomes=['finished', 'failed', 'check'], input_keys=['waypoint'], conditions=[('check', [('Container', 'check')]), ('finished', [('navigate to the point', 'finished')]), ('failed', [('navigate to the point', 'failed')])]) with _sm_container_13: # x:315 y:51 OperatableStateMachine.add('navigate to the point', _sm_navigate_to_the_point_11, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'waypoint'}) # x:322 y:257 OperatableStateMachine.add('Container', _sm_container_10, transitions={'check': 'check'}, autonomy={'check': Autonomy.Inherit}) # x:98 y:451 _sm_get_area_containers_14 = OperatableStateMachine( outcomes=['done'], input_keys=['command'], output_keys=['containers']) with _sm_get_area_containers_14: # x:46 y:31 OperatableStateMachine.add( 'Set Empty Array', SetKey(Value=[]), transitions={'done': 'Set initial index'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'containers'}) # x:50 y:110 OperatableStateMachine.add('Set initial index', SetKey(Value=2), transitions={'done': 'Set Loop Max'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'iLoop'}) # x:49 y:192 OperatableStateMachine.add( 'Set Loop Max', CalculationState(calculation=lambda x: len(x)), transitions={'done': 'Check Loop End'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'command', 'output_value': 'iLoopMax' }) # x:514 y:278 OperatableStateMachine.add( 'Increment', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Print container array'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'iLoop', 'output_value': 'iLoop' }) # x:271 y:154 OperatableStateMachine.add( 'Add Container to Array', FlexibleCalculationState( calculation=lambda x: x[2] + [x[0][x[1]]], input_keys=['command', 'iLoop', 'containers']), transitions={'done': 'Increment'}, autonomy={'done': Autonomy.Off}, remapping={ 'command': 'command', 'iLoop': 'iLoop', 'containers': 'containers', 'output_value': 'containers' }) # x:42 y:287 OperatableStateMachine.add('Check Loop End', FlexibleCheckConditionState( predicate=lambda x: x[0] < x[1], input_keys=['iLoop', 'iLoopMax']), transitions={ 'true': 'Add Container to Array', 'false': 'done' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'iLoop': 'iLoop', 'iLoopMax': 'iLoopMax' }) # x:279 y:401 OperatableStateMachine.add('Print container array', LogKeyState( text="Containers: \n {}", severity=Logger.REPORT_HINT), transitions={'done': 'Check Loop End'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'containers'}) # x:626 y:228, x:607 y:71, x:230 y:458, x:330 y:458 _sm_operator_is_still_there_15 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['ID'], conditions=[('finished', [('find a human', 'finished')]), ('failed', [('Move head and base end ', 'failed')])]) with _sm_operator_is_still_there_15: # x:207 y:54 OperatableStateMachine.add('Move head and base end ', _sm_move_head_and_base_end__3, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:222 y:190 OperatableStateMachine.add('find a human', _sm_find_a_human_2, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:1620 y:109, x:1648 y:375 _sm_try_to_find_area_16 = OperatableStateMachine( outcomes=['found', 'not_found'], input_keys=['area_to_search', 'containers'], output_keys=['area_name', 'waypoint']) with _sm_try_to_find_area_16: # x:517 y:67 OperatableStateMachine.add( 'WonderlandUniqueEnity', self.use_behavior( sara_flexbe_behaviors__WonderlandUniqueEnitySM, 'Try to find area/WonderlandUniqueEnity'), transitions={ 'found': 'Export Waypoint', 'not_found': 'Export No Waypoint' }, autonomy={ 'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit }, remapping={ 'name': 'area_to_search', 'containers': 'containers', 'entity': 'entity' }) # x:966 y:71 OperatableStateMachine.add('Export Waypoint', _sm_export_waypoint_5, transitions={'done': 'say_going'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'entity': 'entity', 'waipoint': 'waypoint', 'area_name': 'area_name' }) # x:1247 y:295 OperatableStateMachine.add('Export No Waypoint', _sm_export_no_waypoint_4, transitions={'done': 'not_found'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'waipoint': 'waypoint', 'area_name': 'area_name' }) # x:1273 y:106 OperatableStateMachine.add( 'say_going', SaraSay(sentence=lambda x: "I'm going to the " + x, input_keys=[], emotion=0, block=True), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}) # x:323 y:632, x:638 y:631 _sm_try_to_reach_17 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['waypoint', 'relative', 'areaName', 'ID']) with _sm_try_to_reach_17: # x:252 y:161 OperatableStateMachine.add('Container', _sm_container_13, transitions={ 'finished': 'say_reached', 'failed': 'Say_not_reached', 'check': 'say check' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'check': Autonomy.Inherit }, remapping={'waypoint': 'waypoint'}) # x:581 y:163 OperatableStateMachine.add('check person behind', _sm_check_person_behind_12, transitions={ 'finished': 'say found', 'failed': 'say lost' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'ID': 'ID'}) # x:706 y:484 OperatableStateMachine.add( 'say lost', SaraSay(sentence="Oh no! I lost my operator!", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:445 y:69 OperatableStateMachine.add( 'say check', SaraSay(sentence="I check if my operator is still there", input_keys=[], emotion=1, block=True), transitions={'done': 'check person behind'}, autonomy={'done': Autonomy.Off}) # x:443 y:187 OperatableStateMachine.add( 'say found', SaraSay(sentence="Great. You are still there.", input_keys=[], emotion=1, block=True), transitions={'done': 'Container'}, autonomy={'done': Autonomy.Off}) # x:463 y:398 OperatableStateMachine.add( 'Say_not_reached', SaraSay(sentence=lambda x: "I have not reach the " + x + "!", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:61 y:366 OperatableStateMachine.add( 'say_reached', SaraSay(sentence=lambda x: "I have reach the " + x + "!", input_keys=[], emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:871 y:59 _sm_decompose_command_18 = OperatableStateMachine( outcomes=['done'], input_keys=['command'], output_keys=['containers', 'area']) with _sm_decompose_command_18: # x:163 y:34 OperatableStateMachine.add('Set State Command', Set_a_step(step=0), transitions={'done': 'Get area name'}, autonomy={'done': Autonomy.Off}) # x:387 y:58 OperatableStateMachine.add( 'Get area name', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Get area containers'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'command', 'output_value': 'area' }) # x:597 y:94 OperatableStateMachine.add('Get area containers', _sm_get_area_containers_14, transitions={'done': 'done'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'command': 'command', 'containers': 'containers' }) with _state_machine: # x:54 y:24 OperatableStateMachine.add( 'Get Person Id', GetRosParam(ParamName="behavior/FoundPerson/Id"), transitions={ 'done': 'GetPerson', 'failed': 'Cant Find Person' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'ID'}) # x:19 y:152 OperatableStateMachine.add( 'Decompose Command', _sm_decompose_command_18, transitions={'done': 'Try to find area'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'command': 'Action', 'containers': 'containers', 'area': 'area' }) # x:43 y:358 OperatableStateMachine.add('Try to reach', _sm_try_to_reach_17, transitions={ 'finished': 'operator is still there', 'failed': 'cause2' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'waypoint': 'waypoint', 'relative': 'relative', 'areaName': 'area_name', 'ID': 'ID' }) # x:360 y:109 OperatableStateMachine.add('Cant Find Person', SaraSay( sentence="I can't find a person.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:52 y:82 OperatableStateMachine.add('GetPerson', GetEntityByID(), transitions={ 'found': 'Decompose Command', 'not_found': 'Cant Find Person' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:36 y:219 OperatableStateMachine.add('Try to find area', _sm_try_to_find_area_16, transitions={ 'found': 'sayfollowme', 'not_found': 'Cant Find Person' }, autonomy={ 'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit }, remapping={ 'area_to_search': 'area', 'containers': 'containers', 'area_name': 'area_name', 'waypoint': 'waypoint' }) # x:27 y:438 OperatableStateMachine.add('operator is still there', _sm_operator_is_still_there_15, transitions={ 'finished': 'getentitybyID', 'failed': 'say lost operator' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'ID': 'ID'}) # x:347 y:370 OperatableStateMachine.add( 'say lost operator', SaraSay( sentence= "I have reach my goal but I lost the person I was guiding.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause3'}, autonomy={'done': Autonomy.Off}) # x:49 y:291 OperatableStateMachine.add('sayfollowme', SaraSay(sentence="Follow me please.", input_keys=[], emotion=1, block=True), transitions={'done': 'Try to reach'}, autonomy={'done': Autonomy.Off}) # x:808 y:451 OperatableStateMachine.add('head to middle', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:48 y:525 OperatableStateMachine.add('getentitybyID', GetEntityByID(), transitions={ 'found': 'get entity to point', 'not_found': 'say_reach_the_entity' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:249 y:562 OperatableStateMachine.add('get entity to point', WonderlandGetEntityVerbal(), transitions={ 'one': 'find the point', 'multiple': 'say_reach_the_entity', 'none': 'say_reach_the_entity', 'error': 'say_reach_the_entity' }, autonomy={ 'one': Autonomy.Off, 'multiple': Autonomy.Off, 'none': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'name': 'area_name', 'containers': 'containers', 'entities': 'entities', 'firstEntity': 'firstEntity' }) # x:708 y:529 OperatableStateMachine.add( 'Action_point_at', self.use_behavior(sara_flexbe_behaviors__Action_point_atSM, 'Action_point_at'), transitions={ 'finished': 'say_reach_the_entity', 'failed': 'say_reach_the_entity' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'targetPoint': 'targetPoint'}) # x:539 y:558 OperatableStateMachine.add( 'find the point', CalculationState(calculation=lambda x: x.position), transitions={'done': 'Action_point_at'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entities', 'output_value': 'targetPoint' }) # x:547 y:121 OperatableStateMachine.add( 'cause1', SetKey(Value="I didn't find any persone"), transitions={'done': 'setrosparamfail'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:507 y:220 OperatableStateMachine.add( 'cause2', SetKey(Value="I did not reach the area"), transitions={'done': 'setrosparamfail'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:559 y:337 OperatableStateMachine.add( 'cause3', SetKey(Value="I lost the person I was guiding."), transitions={'done': 'setrosparamfail'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:656 y:250 OperatableStateMachine.add( 'setrosparamfail', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:523 y:462 OperatableStateMachine.add( 'say_reach_the_entity', SaraSay(sentence=lambda x: "Here is the " + x, input_keys=[], emotion=1, block=True), transitions={'done': 'head to middle'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:272 y:55 _state_machine = OperatableStateMachine(outcomes=['failed'], input_keys=['ID']) _state_machine.userdata.ID = 1 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:470 y:563 _sm_delai_0 = OperatableStateMachine(outcomes=['finished']) with _sm_delai_0: # x:167 y:26 OperatableStateMachine.add('say lost', SaraSay(sentence="Sorry, I lost you.", input_keys=[], emotion=0, block=False), transitions={'done': 'delay'}, autonomy={'done': Autonomy.Off}) # x:167 y:190 OperatableStateMachine.add('say wait', SaraSay(sentence=" Please wait for me!", input_keys=[], emotion=0, block=False), transitions={'done': 'delay2'}, autonomy={'done': Autonomy.Off}) # x:161 y:362 OperatableStateMachine.add('say face 2', SaraSay(sentence="Make sure to let me see your face, please.", input_keys=[], emotion=0, block=False), transitions={'done': 'delay3'}, autonomy={'done': Autonomy.Off}) # x:173 y:278 OperatableStateMachine.add('delay2', WaitState(wait_time=3), transitions={'done': 'say face 2'}, autonomy={'done': Autonomy.Off}) # x:169 y:448 OperatableStateMachine.add('delay3', WaitState(wait_time=4), transitions={'done': 'say get closer'}, autonomy={'done': Autonomy.Off}) # x:176 y:108 OperatableStateMachine.add('delay', WaitState(wait_time=3), transitions={'done': 'say wait'}, autonomy={'done': Autonomy.Off}) # x:142 y:547 OperatableStateMachine.add('say get closer', SaraSay(sentence="I need you to get closer please.", input_keys=[], emotion=0, block=False), transitions={'done': 'delais4'}, autonomy={'done': Autonomy.Off}) # x:315 y:543 OperatableStateMachine.add('delais4', WaitState(wait_time=5), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:30 y:365 _sm_turn_head_1 = OperatableStateMachine(outcomes=['fail']) with _sm_turn_head_1: # x:54 y:55 OperatableStateMachine.add('turn r', SaraSetHeadAngle(pitch=-0.1, yaw=-1.5), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:749 y:183 OperatableStateMachine.add('wait1', WaitState(wait_time=5), transitions={'done': 'turn right'}, autonomy={'done': Autonomy.Off}) # x:727 y:309 OperatableStateMachine.add('turn right', SaraSetHeadAngle(pitch=-0.1, yaw=-1.5), transitions={'done': 'wait2'}, autonomy={'done': Autonomy.Off}) # x:582 y:311 OperatableStateMachine.add('wait2', WaitState(wait_time=5), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:246 y:54 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'turn left'}, autonomy={'done': Autonomy.Off}) # x:383 y:57 OperatableStateMachine.add('turn left', SaraSetHeadAngle(pitch=-0.1, yaw=1.5), transitions={'done': 'wait3'}, autonomy={'done': Autonomy.Off}) # x:728 y:69 OperatableStateMachine.add('center1', SaraSetHeadAngle(pitch=-0.1, yaw=0), transitions={'done': 'wait1'}, autonomy={'done': Autonomy.Off}) # x:576 y:63 OperatableStateMachine.add('wait3', WaitState(wait_time=5), transitions={'done': 'center1'}, autonomy={'done': Autonomy.Off}) # x:389 y:308 OperatableStateMachine.add('center2', SaraSetHeadAngle(pitch=-0.1, yaw=0), transitions={'done': 'wait4'}, autonomy={'done': Autonomy.Off}) # x:409 y:177 OperatableStateMachine.add('wait4', WaitState(wait_time=5), transitions={'done': 'turn left'}, autonomy={'done': Autonomy.Off}) # x:30 y:365 _sm_search_2 = OperatableStateMachine(outcomes=['found'], input_keys=['ID']) with _sm_search_2: # x:91 y:163 OperatableStateMachine.add('get en', GetEntityByID(), transitions={'found': 'found', 'not_found': 'get en'}, autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'ID': 'ID', 'Entity': 'Entity'}) # x:30 y:365, x:530 y:244, x:230 y:365, x:330 y:365, x:430 y:365 _sm_find_back_3 = ConcurrencyContainer(outcomes=['back', 'not_found'], input_keys=['ID'], conditions=[ ('back', [('search', 'found')]), ('back', [('Turn head', 'fail')]), ('not_found', [('Delai', 'finished')]) ]) with _sm_find_back_3: # x:145 y:108 OperatableStateMachine.add('search', _sm_search_2, transitions={'found': 'back'}, autonomy={'found': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:469 y:136 OperatableStateMachine.add('Turn head', _sm_turn_head_1, transitions={'fail': 'back'}, autonomy={'fail': Autonomy.Inherit}) # x:281 y:192 OperatableStateMachine.add('Delai', _sm_delai_0, transitions={'finished': 'not_found'}, autonomy={'finished': Autonomy.Inherit}) # x:30 y:458 _sm_look_at_4 = OperatableStateMachine(outcomes=['fail'], input_keys=['ID']) with _sm_look_at_4: # x:231 y:115 OperatableStateMachine.add('look', KeepLookingAt(), transitions={'failed': 'Find back'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:78 y:199 OperatableStateMachine.add('Found you', SaraSay(sentence="Here you are!", input_keys=[], emotion=1, block=False), transitions={'done': 'look'}, autonomy={'done': Autonomy.Off}) # x:248 y:264 OperatableStateMachine.add('Find back', _sm_find_back_3, transitions={'back': 'Found you', 'not_found': 'fail'}, autonomy={'back': Autonomy.Inherit, 'not_found': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:30 y:365 _sm_follow_5 = OperatableStateMachine(outcomes=['failed'], input_keys=['ID']) with _sm_follow_5: # x:65 y:167 OperatableStateMachine.add('follow', SaraFollow(distance=1.5, ReplanPeriod=3), transitions={'failed': 'follow'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:368 y:271, x:423 y:158, x:230 y:458 _sm_follow_6 = ConcurrencyContainer(outcomes=['not_found'], input_keys=['ID'], conditions=[ ('not_found', [('Look at', 'fail')]), ('not_found', [('Follow', 'failed')]) ]) with _sm_follow_6: # x:185 y:134 OperatableStateMachine.add('Follow', _sm_follow_5, transitions={'failed': 'not_found'}, autonomy={'failed': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:123 y:236 OperatableStateMachine.add('Look at', _sm_look_at_4, transitions={'fail': 'not_found'}, autonomy={'fail': Autonomy.Inherit}, remapping={'ID': 'ID'}) with _state_machine: # x:74 y:38 OperatableStateMachine.add('Follow', _sm_follow_6, transitions={'not_found': 'failed'}, autonomy={'not_found': Autonomy.Inherit}, remapping={'ID': 'ID'}) return _state_machine
def create(self): # x:831 y:173, x:520 y:222, x:335 y:32, x:701 y:385 _state_machine = OperatableStateMachine( outcomes=['success', 'unreachable', 'not found', 'dropped'], input_keys=['objectID', 'Entity']) _state_machine.userdata.objectID = 1585 _state_machine.userdata.PreGripPose = "PreGripPose" _state_machine.userdata.PostGripPose = "PostGripPose" _state_machine.userdata.Entity = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365, x:130 y:365, x:230 y:365 _sm_look_at_it_for_3s_0 = ConcurrencyContainer( outcomes=['done'], input_keys=['ID'], conditions=[('done', [('look', 'failed')]), ('done', [('wait', 'done')])]) with _sm_look_at_it_for_3s_0: # x:30 y:54 OperatableStateMachine.add('look', KeepLookingAt(), transitions={'failed': 'done'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:187 y:111 OperatableStateMachine.add('wait', WaitState(wait_time=3), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:30 y:324 _sm_getting_close_1 = OperatableStateMachine(outcomes=['done'], input_keys=['pose_app']) with _sm_getting_close_1: # x:36 y:40 OperatableStateMachine.add( 'gripperopen', SetGripperState(width=0.15, effort=1), transitions={ 'object': 'move_approach', 'no_object': 'move_approach' }, autonomy={ 'object': Autonomy.Off, 'no_object': Autonomy.Off }, remapping={'object_size': 'object_size'}) # x:30 y:129 OperatableStateMachine.add('move_approach', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'almost have it', 'failed': 'almost have it' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'pose_app'}) # x:39 y:204 OperatableStateMachine.add('almost have it', SaraSay(sentence="I am close", input_keys=[], emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:30 y:324, x:130 y:324 _sm_get_away_from_failure_2 = OperatableStateMachine( outcomes=['done', 'failed'], input_keys=['PostGripPose']) with _sm_get_away_from_failure_2: # x:30 y:40 OperatableStateMachine.add( 'open 2', SetGripperState(width=0.1, effort=1), transitions={ 'object': 'move back', 'no_object': 'move back' }, autonomy={ 'object': Autonomy.Off, 'no_object': Autonomy.Off }, remapping={'object_size': 'object_size'}) # x:235 y:50 OperatableStateMachine.add('move back', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'done', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'PostGripPose'}) # x:30 y:324, x:130 y:324 _sm_lift_object_3 = OperatableStateMachine( outcomes=['done', 'failed'], input_keys=['pose_lift', 'pose_ret']) with _sm_lift_object_3: # x:30 y:176 OperatableStateMachine.add('move_lift_object', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'genpose', 'failed': 'genpose' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'pose_lift'}) # x:228 y:176 OperatableStateMachine.add('move_Return', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'done', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'pose_ret'}) # x:38 y:45 OperatableStateMachine.add('genpose', GenPoseEuler(x=-0.2, y=0, z=0, roll=0, pitch=0, yaw=0), transitions={'done': 'move_back'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'backPose'}) # x:211 y:40 OperatableStateMachine.add('move_back', SaraMoveBase(reference="base_link"), transitions={ 'arrived': 'move_Return', 'failed': 'move_Return' }, autonomy={ 'arrived': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'pose': 'backPose'}) # x:499 y:227, x:263 y:214, x:271 y:492 _sm_pregrip_4 = OperatableStateMachine( outcomes=['fail', 'failed', 'done'], input_keys=['PreGripPose', 'posobjet'], output_keys=['pose_app', 'grippose', 'pose_lift', 'pose_ret']) with _sm_pregrip_4: # x:70 y:40 OperatableStateMachine.add('gen_gripPose', GenGripperPose(l=0.0, z=0, planar=True), transitions={ 'done': 'checkifposeaccess', 'fail': 'fail' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'pose_in': 'posobjet', 'pose_out': 'grippose' }) # x:56 y:256 OperatableStateMachine.add('move_PreGrip', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'gen_returnPose', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'PreGripPose'}) # x:30 y:398 OperatableStateMachine.add('gen_approachPose', GenGripperPose(l=0.15, z=0, planar=True), transitions={ 'done': 'gen_liftPose', 'fail': 'fail' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'pose_in': 'posobjet', 'pose_out': 'pose_app' }) # x:40 y:469 OperatableStateMachine.add('gen_liftPose', GenGripperPose(l=0, z=0.1, planar=True), transitions={ 'done': 'done', 'fail': 'fail' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'pose_in': 'posobjet', 'pose_out': 'pose_lift' }) # x:43 y:326 OperatableStateMachine.add('gen_returnPose', GenGripperPose(l=0.2, z=0.1, planar=True), transitions={ 'done': 'gen_approachPose', 'fail': 'fail' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'pose_in': 'posobjet', 'pose_out': 'pose_ret' }) # x:63 y:184 OperatableStateMachine.add('say can reach', SaraSay(sentence="I will grab it", input_keys=[], emotion=1, block=False), transitions={'done': 'move_PreGrip'}, autonomy={'done': Autonomy.Off}) # x:58 y:114 OperatableStateMachine.add('checkifposeaccess', MoveitMove(move=False, waitForExecution=True, group="RightArm"), transitions={ 'done': 'say can reach', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'grippose'}) # x:315 y:40, x:130 y:465 _sm_get_object_5 = OperatableStateMachine( outcomes=['not_found', 'finished'], input_keys=['objectID'], output_keys=['posobjet']) with _sm_get_object_5: # x:55 y:40 OperatableStateMachine.add('getobject', GetEntityByID(), transitions={ 'found': 'Say_See_It', 'not_found': 'not_found' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'objectID', 'Entity': 'Entity' }) # x:77 y:370 OperatableStateMachine.add( 'getpose', CalculationState(calculation=lambda x: x.position), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Entity', 'output_value': 'posobjet' }) # x:71 y:262 OperatableStateMachine.add('Look at it for 3s', _sm_look_at_it_for_3s_0, transitions={'done': 'getpose'}, autonomy={'done': Autonomy.Inherit}, remapping={'ID': 'objectID'}) # x:31 y:133 OperatableStateMachine.add( 'Say_See_It', SaraSay(sentence=lambda x: "I see the " + x.name, input_keys=["Entity"], emotion=0, block=True), transitions={'done': 'Look at it for 3s'}, autonomy={'done': Autonomy.Off}, remapping={'Entity': 'Entity'}) with _state_machine: # x:77 y:23 OperatableStateMachine.add('Get object', _sm_get_object_5, transitions={ 'not_found': 'not found', 'finished': 'transform point' }, autonomy={ 'not_found': Autonomy.Inherit, 'finished': Autonomy.Inherit }, remapping={ 'objectID': 'objectID', 'posobjet': 'posobjet' }) # x:258 y:385 OperatableStateMachine.add( 'gripclose', SetGripperState(width=0, effort=250), transitions={ 'object': 'say picked', 'no_object': 'get away from failure' }, autonomy={ 'object': Autonomy.Off, 'no_object': Autonomy.Off }, remapping={'object_size': 'object_size'}) # x:53 y:374 OperatableStateMachine.add('move_on_object', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'gripclose', 'failed': 'cant reach' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'grippose'}) # x:650 y:290 OperatableStateMachine.add('move_PostGrip', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'welcome', 'failed': 'unreachable' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'PostGripPose'}) # x:277 y:200 OperatableStateMachine.add('cant reach', SaraSay( sentence="Hum. I can't reach it.", input_keys=[], emotion=1, block=True), transitions={'done': 'unreachable'}, autonomy={'done': Autonomy.Off}) # x:265 y:291 OperatableStateMachine.add('say picked', SaraSay(sentence="I think I got it", input_keys=[], emotion=1, block=False), transitions={'done': 'Lift object'}, autonomy={'done': Autonomy.Off}) # x:837 y:291 OperatableStateMachine.add('welcome', SaraSay(sentence="you are welcome", input_keys=[], emotion=1, block=True), transitions={'done': 'success'}, autonomy={'done': Autonomy.Off}) # x:62 y:190 OperatableStateMachine.add('PreGrip', _sm_pregrip_4, transitions={ 'fail': 'cant reach', 'failed': 'cant reach', 'done': 'Getting close' }, autonomy={ 'fail': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'PreGripPose': 'PreGripPose', 'posobjet': 'posobjet', 'pose_app': 'pose_app', 'grippose': 'grippose', 'pose_lift': 'pose_lift', 'pose_ret': 'pose_ret' }) # x:67 y:114 OperatableStateMachine.add('transform point', TF_transformation(in_ref="map", out_ref="base_link"), transitions={ 'done': 'PreGrip', 'fail': 'Get object' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'in_pos': 'posobjet', 'out_pos': 'posobjet' }) # x:439 y:287 OperatableStateMachine.add('Lift object', _sm_lift_object_3, transitions={ 'done': 'move_PostGrip', 'failed': 'move_PostGrip' }, autonomy={ 'done': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'pose_lift': 'pose_lift', 'pose_ret': 'pose_ret' }) # x:441 y:391 OperatableStateMachine.add( 'get away from failure', _sm_get_away_from_failure_2, transitions={ 'done': 'dropped', 'failed': 'dropped' }, autonomy={ 'done': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'PostGripPose': 'PostGripPose'}) # x:47 y:279 OperatableStateMachine.add('Getting close', _sm_getting_close_1, transitions={'done': 'move_on_object'}, autonomy={'done': Autonomy.Inherit}, remapping={'pose_app': 'pose_app'}) return _state_machine
def create(self): # x:682 y:306, x:452 y:252 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['pos']) _state_machine.userdata.pos = {"x": 0.8, "y": -0.2, "z": 1} # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458, x:530 y:458, x:630 y:458, x:59 y:533, x:830 y:458 _sm_group_0 = ConcurrencyContainer( outcomes=['threshold', 'watchdog', 'fail'], conditions=[('threshold', [('read', 'threshold')]), ('watchdog', [('read', 'watchdog')]), ('fail', [('read', 'fail')]), ('threshold', [('read yaw', 'threshold')]), ('fail', [('read yaw', 'fail')]), ('watchdog', [('read yaw', 'watchdog')])]) with _sm_group_0: # x:86 y:125 OperatableStateMachine.add('read', ReadTorque( watchdog=1, Joint="right_elbow_pitch_joint", Threshold=0.7, min_time=0.4), transitions={ 'threshold': 'threshold', 'watchdog': 'watchdog', 'fail': 'fail' }, autonomy={ 'threshold': Autonomy.Off, 'watchdog': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'torque': 'torque'}) # x:252 y:135 OperatableStateMachine.add('read yaw', ReadTorque( watchdog=1, Joint="right_elbow_pitch_joint", Threshold=0.5, min_time=0.4), transitions={ 'threshold': 'threshold', 'watchdog': 'watchdog', 'fail': 'fail' }, autonomy={ 'threshold': Autonomy.Off, 'watchdog': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'torque': 'torque'}) # x:30 y:458 _sm_read_torque_1 = OperatableStateMachine(outcomes=['done']) with _sm_read_torque_1: # x:142 y:61 OperatableStateMachine.add('log', LogState(text="going down", severity=Logger.REPORT_HINT), transitions={'done': 'Group'}, autonomy={'done': Autonomy.Off}) # x:131 y:164 OperatableStateMachine.add('Group', _sm_group_0, transitions={ 'threshold': 'done', 'watchdog': 'log', 'fail': 'done' }, autonomy={ 'threshold': Autonomy.Inherit, 'watchdog': Autonomy.Inherit, 'fail': Autonomy.Inherit }) # x:30 y:458 _sm_go_down_2 = OperatableStateMachine(outcomes=['done'], input_keys=['GripPose']) with _sm_go_down_2: # x:92 y:127 OperatableStateMachine.add('place down', MoveitMoveCartesian( move=True, waitForExecution=True, group="RightArm", watchdog=15), transitions={ 'done': 'done', 'failed': 'done' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'targetPose': 'GripPose'}) # x:30 y:324, x:130 y:324 _sm_releasing_3 = OperatableStateMachine( outcomes=['object', 'no_object']) with _sm_releasing_3: # x:30 y:40 OperatableStateMachine.add('say touchdown', SaraSay(sentence="Touchdown!", input_keys=[], emotion=1, block=False), transitions={'done': 'open gripper'}, autonomy={'done': Autonomy.Off}) # x:139 y:176 OperatableStateMachine.add( 'open gripper', SetGripperState(width=0.14, effort=1), transitions={ 'object': 'object', 'no_object': 'no_object' }, autonomy={ 'object': Autonomy.Off, 'no_object': Autonomy.Off }, remapping={'object_size': 'object_size'}) # x:30 y:324, x:130 y:324 _sm_moveback_4 = OperatableStateMachine(outcomes=['arrived', 'failed']) with _sm_moveback_4: # x:30 y:40 OperatableStateMachine.add('genpose', GenPoseEuler(x=-0.3, y=-0.3, z=0, roll=0, pitch=0, yaw=0), transitions={'done': 'move back'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'backPose'}) # x:40 y:163 OperatableStateMachine.add('move back', SaraMoveBase(reference="base_link"), transitions={ 'arrived': 'arrived', 'failed': 'failed' }, autonomy={ 'arrived': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'pose': 'backPose'}) # x:536 y:72, x:231 y:292 _sm_prepare_grip_5 = OperatableStateMachine( outcomes=['failed', 'done'], input_keys=['pos'], output_keys=['approach_pose', 'grip_pose']) with _sm_prepare_grip_5: # x:50 y:40 OperatableStateMachine.add('Gen place_pos', GenGripperPose(l=0, z=-0.05, planar=True), transitions={ 'done': 'Gen approach_pos', 'fail': 'failed' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'pose_in': 'pos', 'pose_out': 'grip_pose' }) # x:30 y:176 OperatableStateMachine.add('MoveIt_isReachable', MoveitMove(move=False, waitForExecution=True, group="RightArm", watchdog=15), transitions={ 'done': 'log app', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'grip_pose'}) # x:37 y:108 OperatableStateMachine.add('Gen approach_pos', GenGripperPose(l=0.0, z=0.20, planar=True), transitions={ 'done': 'log place pos', 'fail': 'failed' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'pose_in': 'pos', 'pose_out': 'approach_pose' }) # x:41 y:269 OperatableStateMachine.add('log app', LogKeyState( text="{}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'approach_pose'}) # x:360 y:180 OperatableStateMachine.add( 'log place pos', LogKeyState(text="place pose is {}", severity=Logger.REPORT_HINT), transitions={'done': 'MoveIt_isReachable'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'grip_pose'}) # x:30 y:458, x:130 y:458, x:230 y:458 _sm_get_down_6 = ConcurrencyContainer( outcomes=['done'], input_keys=['GripPose'], conditions=[('done', [('Go down', 'done')]), ('done', [('read torque', 'done')])]) with _sm_get_down_6: # x:178 y:127 OperatableStateMachine.add('Go down', _sm_go_down_2, transitions={'done': 'done'}, autonomy={'done': Autonomy.Inherit}, remapping={'GripPose': 'GripPose'}) # x:405 y:150 OperatableStateMachine.add('read torque', _sm_read_torque_1, transitions={'done': 'done'}, autonomy={'done': Autonomy.Inherit}) # x:30 y:324, x:130 y:324 _sm_pretraitement_7 = OperatableStateMachine(outcomes=['fail', 'done'], input_keys=['pos'], output_keys=['pos']) with _sm_pretraitement_7: # x:30 y:40 OperatableStateMachine.add('TF_transformation', TF_transformation(in_ref="map", out_ref="base_link"), transitions={ 'done': 'LOG POSE', 'fail': 'fail' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'in_pos': 'pos', 'out_pos': 'pos' }) # x:33 y:107 OperatableStateMachine.add('LOG POSE', LogKeyState( text="{}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'pos'}) with _state_machine: # x:148 y:34 OperatableStateMachine.add('Pretraitement', _sm_pretraitement_7, transitions={ 'fail': 'failed', 'done': 'Pregrip' }, autonomy={ 'fail': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={'pos': 'pos'}) # x:634 y:410 OperatableStateMachine.add( 'close gripper', SetGripperState(width=0, effort=1), transitions={ 'object': 'finished', 'no_object': 'finished' }, autonomy={ 'object': Autonomy.Off, 'no_object': Autonomy.Off }, remapping={'object_size': 'object_size'}) # x:141 y:522 OperatableStateMachine.add('Get_down', _sm_get_down_6, transitions={'done': 'releasing'}, autonomy={'done': Autonomy.Inherit}, remapping={'GripPose': 'grip_pose'}) # x:159 y:352 OperatableStateMachine.add('look down', SaraSetHeadAngle(pitch=0.6, yaw=-0.3), transitions={'done': 'Move_approach'}, autonomy={'done': Autonomy.Off}) # x:156 y:238 OperatableStateMachine.add('Prepare grip', _sm_prepare_grip_5, transitions={ 'failed': 'failed', 'done': 'look down' }, autonomy={ 'failed': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'pos': 'pos', 'approach_pose': 'approach_pose', 'grip_pose': 'grip_pose' }) # x:139 y:444 OperatableStateMachine.add('Move_approach', MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15), transitions={ 'done': 'Get_down', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'approach_pose'}) # x:623 y:525 OperatableStateMachine.add('Moveback', _sm_moveback_4, transitions={ 'arrived': 'close gripper', 'failed': 'failed' }, autonomy={ 'arrived': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:298 y:520 OperatableStateMachine.add('releasing', _sm_releasing_3, transitions={ 'object': 'Pregrip_2', 'no_object': 'Pregrip_2' }, autonomy={ 'object': Autonomy.Inherit, 'no_object': Autonomy.Inherit }) # x:159 y:139 OperatableStateMachine.add('Pregrip', RunTrajectory(file="pre_grip_pose", duration=6), transitions={'done': 'Prepare grip'}, autonomy={'done': Autonomy.Off}) # x:440 y:537 OperatableStateMachine.add('Pregrip_2', RunTrajectory(file="pre_grip_pose", duration=0), transitions={'done': 'Moveback'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:1013 y:314, x:1017 y:162 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['question'], output_keys=['answer']) _state_machine.userdata.question = "" _state_machine.userdata.answer = "" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:79 y:95 OperatableStateMachine.add('say question', SaraSay(sentence=lambda x: x[0], input_keys=["question"], emotion=0, block=True), transitions={'done': 'get answer'}, autonomy={'done': Autonomy.Off}, remapping={'question': 'question'}) # x:92 y:304 OperatableStateMachine.add('get answer', GetSpeech(watchdog=10), transitions={ 'done': 'finished', 'nothing': 'retry ask', 'fail': 'retry ask' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'answer'}) # x:345 y:202 OperatableStateMachine.add('retry ask', ForLoop(repeat=2), transitions={ 'do': 'say not understand', 'end': 'say failed' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:232 y:90 OperatableStateMachine.add( 'say not understand', SaraSay(sentence="Sorry, I did not understand your answer.", input_keys=[], emotion=0, block=True), transitions={'done': 'say question'}, autonomy={'done': Autonomy.Off}) # x:604 y:151 OperatableStateMachine.add( 'say failed', SaraSay(sentence="Sorry, I can't understand your answer.", input_keys=[], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:998 y:146, x:723 y:729, x:1065 y:284 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = [] _state_machine.userdata.person_name = "operator" _state_machine.userdata.Empty = None _state_machine.userdata.className = "person" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:585 y:427, x:704 y:287, x:702 y:47 _sm_get_person_0 = OperatableStateMachine( outcomes=['true', 'done', 'pas_done'], input_keys=['Action', 'className'], output_keys=['entity']) with _sm_get_person_0: # x:30 y:40 OperatableStateMachine.add( 'cond', CheckConditionState(predicate=lambda x: x[1] != ''), transitions={ 'true': 'get ', 'false': 'Action_findPerson_2' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'Action'}) # x:9 y:537 OperatableStateMachine.add('name', GetSpeech(watchdog=5), transitions={ 'done': 'confirming_persons_name', 'nothing': 'name', 'fail': 'Action_findPerson' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'person_name'}) # x:295 y:359 OperatableStateMachine.add( 'confirming_persons_name', CheckConditionState(predicate=lambda x: "yes" in x), transitions={ 'true': 'true', 'false': 'Action_findPerson' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'person_name'}) # x:338 y:105 OperatableStateMachine.add( 'Action_findPerson_2', self.use_behavior(sara_flexbe_behaviors__Action_findPersonSM, 'get_person/Action_findPerson_2'), transitions={ 'done': 'done', 'pas_done': 'pas_done' }, autonomy={ 'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit }, remapping={ 'className': 'className', 'entity': 'entity' }) # x:21 y:223 OperatableStateMachine.add( 'Action_findPerson', self.use_behavior(sara_flexbe_behaviors__Action_findPersonSM, 'get_person/Action_findPerson'), transitions={ 'done': 'Is_Person', 'pas_done': 'Action_findPerson_2' }, autonomy={ 'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit }, remapping={ 'className': 'className', 'entity': 'entity' }) # x:47 y:127 OperatableStateMachine.add( 'get ', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Action_findPerson'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Action', 'output_value': 'person_name' }) # x:111 y:343 OperatableStateMachine.add( 'Is_Person', SaraSay(sentence=lambda x: "Hello, are you " + x + "?", input_keys=[], emotion=0, block=True), transitions={'done': 'name'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:43 y:40 OperatableStateMachine.add( 'Object', GetRosParam(ParamName="behavior/GripperContent"), transitions={ 'done': 'get_person', 'failed': 'no_object' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'gripperContent'}) # x:369 y:92 OperatableStateMachine.add( 'Action_Give', self.use_behavior(sara_flexbe_behaviors__Action_GiveSM, 'Action_Give'), transitions={ 'Given': 'empty hand', 'Person_not_found': 'Person_Lost', 'No_object_in_hand': 'cause1', 'fail': 'cause3' }, autonomy={ 'Given': Autonomy.Inherit, 'Person_not_found': Autonomy.Inherit, 'No_object_in_hand': Autonomy.Inherit, 'fail': Autonomy.Inherit }) # x:286 y:217 OperatableStateMachine.add( 'Nobody_here', SaraSay(sentence="I can't find a person. Goodbye.", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:398 y:360 OperatableStateMachine.add( 'getID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'Action_Give'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'ID' }) # x:116 y:178 OperatableStateMachine.add('get_person', _sm_get_person_0, transitions={ 'true': 'confirm giving', 'done': 'confirm giving', 'pas_done': 'Nobody_here' }, autonomy={ 'true': Autonomy.Inherit, 'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit }, remapping={ 'Action': 'Action', 'className': 'className', 'entity': 'entity' }) # x:53 y:480 OperatableStateMachine.add( 'no_object', SaraSay(sentence="There is nothing in my gripper.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:212 y:355 OperatableStateMachine.add( 'confirm giving', SaraSay(sentence="Let me give you this object.", input_keys=[], emotion=1, block=True), transitions={'done': 'getID'}, autonomy={'done': Autonomy.Off}) # x:806 y:92 OperatableStateMachine.add( 'empty hand', SetRosParam(ParamName="behavior/GripperContent"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Empty'}) # x:586 y:453 OperatableStateMachine.add( 'cause1', SetKey(Value="There was nothing in my gripper."), transitions={'done': 'setcausefailure'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:789 y:373 OperatableStateMachine.add('cause2', SetKey(Value="I lost the person."), transitions={'done': 'setcausefailure'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:886 y:230 OperatableStateMachine.add( 'cause3', SetKey(Value="I was unable to give the object."), transitions={'done': 'setcausefailure'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:930 y:463 OperatableStateMachine.add( 'setcausefailure', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:680 y:259 OperatableStateMachine.add( 'Person_Lost', SaraSay(sentence=lambda x: "I've lost " + x[1] + "!", input_keys=[], emotion=0, block=True), transitions={'done': 'cause2'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:1083 y:361, x:1016 y:460, x:1004 y:649 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["FindPerson","philippe"] _state_machine.userdata.rotation = -1.57 _state_machine.userdata.className = "person" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:458 _sm_look_at_0 = OperatableStateMachine(outcomes=['finished'], input_keys=['entity']) with _sm_look_at_0: # x:142 y:112 OperatableStateMachine.add('get ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'look'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity', 'output_value': 'ID'}) # x:106 y:262 OperatableStateMachine.add('look', KeepLookingAt(), transitions={'failed': 'look'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:297 y:737, x:531 y:481, x:378 y:120, x:739 y:299 _sm_ask_confirmation_1 = OperatableStateMachine(outcomes=['yes', 'no', 'error', 'noname'], input_keys=['name']) with _sm_ask_confirmation_1: # x:473 y:44 OperatableStateMachine.add('check if ask name', CheckConditionState(predicate=lambda x: x[1] != ""), transitions={'true': 'Repeat the question', 'false': 'wait 2'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'name'}) # x:35 y:303 OperatableStateMachine.add('Get Yes or No', GetSpeech(watchdog=5), transitions={'done': 'Repeat', 'nothing': 'Sara_Not_Understand', 'fail': 'Sara_Not_Understand'}, autonomy={'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'words': 'words'}) # x:34 y:486 OperatableStateMachine.add('Check Yes', CheckConditionState(predicate=lambda x: "yes" in x), transitions={'true': 'yes', 'false': 'Check No'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'words'}) # x:281 y:476 OperatableStateMachine.add('Check No', CheckConditionState(predicate=lambda x: "no" in x), transitions={'true': 'no', 'false': 'Sara_Not_Understand'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'words'}) # x:55 y:390 OperatableStateMachine.add('Repeat', LogKeyState(text="I heard: {}", severity=Logger.REPORT_HINT), transitions={'done': 'Check Yes'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'words'}) # x:529 y:253 OperatableStateMachine.add('say hi', SaraSay(sentence="Hi there", input_keys=[], emotion=1, block=True), transitions={'done': 'noname'}, autonomy={'done': Autonomy.Off}) # x:534 y:159 OperatableStateMachine.add('wait 2', WaitState(wait_time=2), transitions={'done': 'say hi'}, autonomy={'done': Autonomy.Off}) # x:37 y:95 OperatableStateMachine.add('Repeat the question', ForLoop(repeat=5), transitions={'do': 'Ask_Person', 'end': 'error'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:31 y:193 OperatableStateMachine.add('Ask_Person', SaraSay(sentence=lambda x: "Are you " + x + "?", input_keys=[], emotion=0, block=True), transitions={'done': 'Get Yes or No'}, autonomy={'done': Autonomy.Off}) # x:264 y:269 OperatableStateMachine.add('Sara_Not_Understand', SaraSay(sentence=lambda x: "I did not understand.", input_keys=[], emotion=0, block=True), transitions={'done': 'Repeat the question'}, autonomy={'done': Autonomy.Off}) # x:305 y:322, x:301 y:53 _sm_if_need_the_one_2 = OperatableStateMachine(outcomes=['done', 'no_param'], input_keys=['Action'], output_keys=['person', 'name']) with _sm_if_need_the_one_2: # x:30 y:40 OperatableStateMachine.add('cond', CheckConditionState(predicate=lambda x: x[1] != None and x[1] != ''), transitions={'true': 'ReadParameters', 'false': 'no_param'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'Action'}) # x:95 y:129 OperatableStateMachine.add('ReadParameters', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Say_Find_Objects'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Action', 'output_value': 'name'}) # x:122 y:297 OperatableStateMachine.add('set person', SetKey(Value="person"), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'person'}) # x:97 y:213 OperatableStateMachine.add('Say_Find_Objects', SaraSay(sentence=lambda x: "I'm now looking for " + x, input_keys=[], emotion=0, block=True), transitions={'done': 'set person'}, autonomy={'done': Autonomy.Off}) # x:499 y:233, x:499 y:138, x:508 y:57, x:467 y:328, x:430 y:458, x:530 y:458, x:630 y:458, x:470 y:369, x:840 y:558 _sm_confirm_and_look_at_3 = ConcurrencyContainer(outcomes=['yes', 'no', 'error', 'noname'], input_keys=['name', 'entity'], conditions=[ ('yes', [('Ask Confirmation', 'yes')]), ('no', [('Ask Confirmation', 'no')]), ('error', [('Ask Confirmation', 'error')]), ('noname', [('Ask Confirmation', 'noname')]), ('error', [('look at', 'finished')]) ]) with _sm_confirm_and_look_at_3: # x:176 y:40 OperatableStateMachine.add('Ask Confirmation', _sm_ask_confirmation_1, transitions={'yes': 'yes', 'no': 'no', 'error': 'error', 'noname': 'noname'}, autonomy={'yes': Autonomy.Inherit, 'no': Autonomy.Inherit, 'error': Autonomy.Inherit, 'noname': Autonomy.Inherit}, remapping={'name': 'name'}) # x:30 y:122 OperatableStateMachine.add('look at', _sm_look_at_0, transitions={'finished': 'error'}, autonomy={'finished': Autonomy.Inherit}, remapping={'entity': 'entity'}) with _state_machine: # x:62 y:38 OperatableStateMachine.add('Action_findPerson', self.use_behavior(Action_findPersonSM, 'Action_findPerson'), transitions={'done': 'If need the one', 'pas_done': 'reset Head'}, autonomy={'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit}, remapping={'className': 'className', 'entity': 'entity'}) # x:512 y:255 OperatableStateMachine.add('Do not find person', SaraSay(sentence="I did not find a person.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause2'}, autonomy={'done': Autonomy.Off}) # x:200 y:536 OperatableStateMachine.add('Retry', ForLoop(repeat=1), transitions={'do': 'Try again', 'end': 'reset Head'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:73 y:456 OperatableStateMachine.add('Try again', SaraSay(sentence="Oh, I will try again.", input_keys=[], emotion=1, block=True), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}) # x:63 y:310 OperatableStateMachine.add('action_turn', self.use_behavior(action_turnSM, 'action_turn'), transitions={'finished': 'Action_findPerson', 'failed': 'reset Head'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'rotation': 'rotation'}) # x:145 y:194 OperatableStateMachine.add('reset Head', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'Do not find person'}, autonomy={'done': Autonomy.Off}) # x:997 y:258 OperatableStateMachine.add('set param', SetRosParam(ParamName="/behavior/FoundPerson/Id"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'ID'}) # x:970 y:88 OperatableStateMachine.add('get ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'set param'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity', 'output_value': 'ID'}) # x:531 y:38 OperatableStateMachine.add('confirm and look at', _sm_confirm_and_look_at_3, transitions={'yes': 'Say_found', 'no': 'Retry', 'error': 'reset Head', 'noname': 'get ID'}, autonomy={'yes': Autonomy.Inherit, 'no': Autonomy.Inherit, 'error': Autonomy.Inherit, 'noname': Autonomy.Inherit}, remapping={'name': 'name', 'entity': 'entity'}) # x:662 y:325 OperatableStateMachine.add('cause2', SetKey(Value="I did not find any person."), transitions={'done': 'setrosparamfailure'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:820 y:377 OperatableStateMachine.add('setrosparamfailure', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:782 y:169 OperatableStateMachine.add('say found person', SaraSay(sentence="I found a person.", input_keys=[], emotion=1, block=True), transitions={'done': 'get ID'}, autonomy={'done': Autonomy.Off}) # x:417 y:167 OperatableStateMachine.add('say hello', SaraSay(sentence="Hello.", input_keys=[], emotion=1, block=True), transitions={'done': 'confirm and look at'}, autonomy={'done': Autonomy.Off}) # x:278 y:47 OperatableStateMachine.add('If need the one', _sm_if_need_the_one_2, transitions={'done': 'say hello', 'no_param': 'say found person'}, autonomy={'done': Autonomy.Inherit, 'no_param': Autonomy.Inherit}, remapping={'Action': 'Action', 'person': 'person', 'name': 'name'}) # x:724 y:101 OperatableStateMachine.add('Say_found', SaraSay(sentence=lambda x: "I have found " + x + "!", input_keys=[], emotion=0, block=True), transitions={'done': 'get ID'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:254 y:640, x:565 y:232, x:530 y:448 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Count", "bottle", "behavior/Count/CountedObject"] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:41 y:32 OperatableStateMachine.add('get name', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Say_Start'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Action', 'output_value': 'className'}) # x:22 y:213 OperatableStateMachine.add('Action_count', self.use_behavior(Action_countSM, 'Action_count'), transitions={'done': 'get paramname', 'failed': 'failed'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'className': 'className', 'Count': 'Count'}) # x:14 y:462 OperatableStateMachine.add('concat', FlexibleCalculationState(calculation=lambda x: "I counted "+str(x[0])+" "+str(x[1])+".", input_keys=["Count", "className"]), transitions={'done': 'Say_Count'}, autonomy={'done': Autonomy.Off}, remapping={'Count': 'Count', 'className': 'className', 'output_value': 'sentence'}) # x:28 y:388 OperatableStateMachine.add('store param', SetRosParamKey(), transitions={'done': 'concat'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Count', 'ParamName': 'ParamName'}) # x:45 y:290 OperatableStateMachine.add('get paramname', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'store param'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Action', 'output_value': 'ParamName'}) # x:17 y:633 OperatableStateMachine.add('set head back', SaraSetHeadAngle(pitch=-0.3, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:36 y:120 OperatableStateMachine.add('Say_Start', SaraSay(sentence=lambda x: "I'm starting to count the "+str(x)+"s.", input_keys=[], emotion=0, block=True), transitions={'done': 'Action_count'}, autonomy={'done': Autonomy.Off}) # x:27 y:546 OperatableStateMachine.add('Say_Count', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'set head back'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:1141 y:183, x:1069 y:298 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:119 OperatableStateMachine.add('coninue', ContinueButton(), transitions={'true': 'GetOrigin', 'false': 'GetOrigin'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}) # x:750 y:145 OperatableStateMachine.add('Farewell_2', self.use_behavior(FarewellSM, 'Farewell_2'), transitions={'finished': 'finish', 'failed': 'didnt succed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:410 y:182 OperatableStateMachine.add('Try again', SaraSay(sentence="Sorry, I will try with the next person", input_keys=[], emotion=2, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:403 y:47 OperatableStateMachine.add('say next', SaraSay(sentence="I will now get the net person.", input_keys=[], emotion=0, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:549 y:98 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'Action_Move'), transitions={'finished': 'Farewell_2', 'failed': 'Action_Move'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'poseOrigin'}) # x:1020 y:75 OperatableStateMachine.add('finish', SaraSay(sentence="I am done BYe bye", input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:906 y:243 OperatableStateMachine.add('didnt succed', SaraSay(sentence="I failed sorry", input_keys=[], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:173 y:106 OperatableStateMachine.add('Farewell', self.use_behavior(FarewellSM, 'Farewell'), transitions={'finished': 'say next', 'failed': 'Try again'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:39 y:31 OperatableStateMachine.add('GetOrigin', Get_Robot_Pose(), transitions={'done': 'Farewell'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'poseOrigin'}) return _state_machine
def create(self): # x:738 y:371, x:736 y:220 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.bin1Waypoint = "trash bin 2" _state_machine.userdata.bin2Waypoint = "trash bin" _state_machine.userdata.bin1Height = "1" _state_machine.userdata.bin2Height = "1" _state_machine.userdata.dropzone1Waypoint = "drop_zone1" _state_machine.userdata.dropzone2Waypoint = "drop_zone2" _state_machine.userdata.DoorName = "door_1_entry" _state_machine.userdata.exit_door = "door_2_exit" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:810 y:644, x:808 y:125 _sm_go_to_drop_the_bag_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['dropzoneWaypoint', 'exit_door']) with _sm_go_to_drop_the_bag_0: # x:84 y:36 OperatableStateMachine.add('say drop', SaraSay(sentence="I will go and drop this bag in the drop zone.", input_keys=[], emotion=0, block=False), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:66 y:598 OperatableStateMachine.add('open the gripper', SetGripperState(width=0.1, effort=1), transitions={'object': 'go to pose transport', 'no_object': 'go to pose transport'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:341 y:469 OperatableStateMachine.add('say cant go to drop zone', SaraSay(sentence="I am not able to go to the drop zone. I will put the bag here and go to the second bin.", input_keys=[], emotion=0, block=True), transitions={'done': 'run depose sac'}, autonomy={'done': Autonomy.Off}) # x:74 y:490 OperatableStateMachine.add('run depose sac', RunTrajectory(file="poubelle_depose", duration=10), transitions={'done': 'open the gripper'}, autonomy={'done': Autonomy.Off}) # x:69 y:726 OperatableStateMachine.add('go to pose transport', RunTrajectory(file="transport", duration=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:76 y:386 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'First bin/go to drop the bag/Action_Move'), transitions={'finished': 'run depose sac', 'failed': 'say cant go to drop zone'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'dropzoneWaypoint'}) # x:570 y:519, x:575 y:330 _sm_find_the_bin_1 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_find_the_bin_1: # x:89 y:62 OperatableStateMachine.add('place arm', RunTrajectory(file="poubelle_init", duration=0), transitions={'done': 'leve la tete'}, autonomy={'done': Autonomy.Off}) # x:270 y:378 OperatableStateMachine.add('get waypoint', Get_Reacheable_Waypoint(), transitions={'done': 'move'}, autonomy={'done': Autonomy.Off}, remapping={'pose_in': 'detectedBin', 'distance': 'distanceToBin', 'pose_out': 'poseToBin'}) # x:296 y:284 OperatableStateMachine.add('set distance to bin', SetKey(Value=0.5), transitions={'done': 'get waypoint'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distanceToBin'}) # x:278 y:201 OperatableStateMachine.add('pose form lidar to map', TF_transformation(in_ref="front_hokuyo_link", out_ref="map"), transitions={'done': 'set distance to bin', 'fail': 'failed'}, autonomy={'done': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'in_pos': 'detectedObstacle', 'out_pos': 'detectedBin'}) # x:80 y:514 OperatableStateMachine.add('redo ajustements de la position', ForLoop(repeat=2), transitions={'do': 'find closest obstacle point', 'end': 'finished'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:87 y:129 OperatableStateMachine.add('find closest obstacle point', GetClosestObstacle(topic="/scan", maximumDistance=2), transitions={'done': 'pose form lidar to map'}, autonomy={'done': Autonomy.Off}, remapping={'Angle': 'Angle', 'distance': 'distance', 'position': 'detectedObstacle'}) # x:288 y:77 OperatableStateMachine.add('leve la tete', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'find closest obstacle point'}, autonomy={'done': Autonomy.Off}) # x:287 y:464 OperatableStateMachine.add('move', SaraMoveBase(reference="map"), transitions={'arrived': 'redo ajustements de la position', 'failed': 'failed'}, autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'pose': 'poseToBin'}) # x:573 y:546, x:622 y:92 _sm_go_to_bin_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin1Waypoint']) with _sm_go_to_bin_2: # x:86 y:104 OperatableStateMachine.add('bras sur le cote', RunTrajectory(file="poubelle_transport", duration=0), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:254 y:199 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'First bin/go to bin/Action_Move'), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'bin1Waypoint'}) # x:600 y:617, x:666 y:442 _sm_find_the_bin_3 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_find_the_bin_3: # x:89 y:62 OperatableStateMachine.add('place arm', RunTrajectory(file="poubelle_init", duration=0), transitions={'done': 'leve la tete'}, autonomy={'done': Autonomy.Off}) # x:268 y:410 OperatableStateMachine.add('get waypoint', Get_Reacheable_Waypoint(), transitions={'done': 'move'}, autonomy={'done': Autonomy.Off}, remapping={'pose_in': 'detectedBin', 'distance': 'distanceToBin', 'pose_out': 'poseToBin'}) # x:293 y:326 OperatableStateMachine.add('set distance to bin', SetKey(Value=0.5), transitions={'done': 'get waypoint'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distanceToBin'}) # x:278 y:238 OperatableStateMachine.add('pose form lidar to map', TF_transformation(in_ref="front_hokuyo_link", out_ref="map"), transitions={'done': 'set distance to bin', 'fail': 'failed'}, autonomy={'done': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'in_pos': 'detectedObstacle', 'out_pos': 'detectedBin'}) # x:80 y:598 OperatableStateMachine.add('redo ajustements de la position', ForLoop(repeat=2), transitions={'do': 'find closest obstacle point', 'end': 'finished'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:89 y:168 OperatableStateMachine.add('find closest obstacle point', GetClosestObstacle(topic="/scan", maximumDistance=2), transitions={'done': 'pose form lidar to map'}, autonomy={'done': Autonomy.Off}, remapping={'Angle': 'Angle', 'distance': 'distance', 'position': 'detectedObstacle'}) # x:288 y:104 OperatableStateMachine.add('leve la tete', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'find closest obstacle point'}, autonomy={'done': Autonomy.Off}) # x:285 y:504 OperatableStateMachine.add('move', SaraMoveBase(reference="map"), transitions={'arrived': 'redo ajustements de la position', 'failed': 'failed'}, autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'pose': 'poseToBin'}) # x:30 y:365, x:130 y:365 _sm_go_to_bin_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin2Waypoint']) with _sm_go_to_bin_4: # x:59 y:100 OperatableStateMachine.add('bras sur le cote', RunTrajectory(file="poubelle_transport", duration=0), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:254 y:199 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'second bin/go to bin/Action_Move'), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'bin2Waypoint'}) # x:418 y:580, x:694 y:339 _sm_go_to_drop_the_bag_5 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['dropzoneWaypoint', 'exit_door']) with _sm_go_to_drop_the_bag_5: # x:65 y:215 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'second bin/go to drop the bag/Action_Move'), transitions={'finished': 'run depose sac', 'failed': 'say cant go to drop zone'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'dropzoneWaypoint'}) # x:385 y:324 OperatableStateMachine.add('say cant go to drop zone', SaraSay(sentence="I am not able to go to the drop zone. I will put the bag here.", input_keys=[], emotion=0, block=True), transitions={'done': 'run depose sac'}, autonomy={'done': Autonomy.Off}) # x:76 y:340 OperatableStateMachine.add('run depose sac', RunTrajectory(file="poubelle_depose", duration=10), transitions={'done': 'open the gripper'}, autonomy={'done': Autonomy.Off}) # x:74 y:505 OperatableStateMachine.add('go to pose transport', RunTrajectory(file="transport", duration=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:74 y:426 OperatableStateMachine.add('open the gripper', SetGripperState(width=0.1, effort=1), transitions={'object': 'go to pose transport', 'no_object': 'go to pose transport'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:704 y:599, x:717 y:53 _sm_first_bin_6 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin1Waypoint', 'bin1Height', 'dropzoneWaypoint', 'exit_door']) with _sm_first_bin_6: # x:57 y:26 OperatableStateMachine.add('Init_Sequence', self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'First bin/Init_Sequence'), transitions={'finished': 'go to bin', 'failed': 'go to bin'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:257 y:329 OperatableStateMachine.add('Action_TakeBag', self.use_behavior(sara_flexbe_behaviors__Action_TakeBagSM, 'First bin/Action_TakeBag'), transitions={'finished': 'go to drop the bag', 'failed': 'Missed It once'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:275 y:35 OperatableStateMachine.add('go to bin', _sm_go_to_bin_2, transitions={'finished': 'find the bin', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'bin1Waypoint': 'bin1Waypoint'}) # x:286 y:164 OperatableStateMachine.add('find the bin', _sm_find_the_bin_1, transitions={'finished': 'Try twice', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:292 y:234 OperatableStateMachine.add('Try twice', ForLoop(repeat=1), transitions={'do': 'Action_TakeBag', 'end': 'failed'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:102 y:262 OperatableStateMachine.add('Missed It once', SaraSay(sentence="I will try one more time.", input_keys=[], emotion=0, block=True), transitions={'done': 'Try twice'}, autonomy={'done': Autonomy.Off}) # x:267 y:432 OperatableStateMachine.add('go to drop the bag', _sm_go_to_drop_the_bag_0, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'dropzoneWaypoint': 'dropzoneWaypoint', 'exit_door': 'exit_door'}) # x:946 y:467, x:907 y:75 _sm_second_bin_7 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin2Waypoint', 'bin2Height', 'dropzoneWaypoint', 'exit_door']) with _sm_second_bin_7: # x:57 y:26 OperatableStateMachine.add('Init_Sequence', self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'second bin/Init_Sequence'), transitions={'finished': 'go to bin', 'failed': 'go to bin'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:267 y:432 OperatableStateMachine.add('go to drop the bag', _sm_go_to_drop_the_bag_5, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'dropzoneWaypoint': 'dropzoneWaypoint', 'exit_door': 'exit_door'}) # x:257 y:346 OperatableStateMachine.add('Action_TakeBag', self.use_behavior(sara_flexbe_behaviors__Action_TakeBagSM, 'second bin/Action_TakeBag'), transitions={'finished': 'go to drop the bag', 'failed': 'Missed It once'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:275 y:35 OperatableStateMachine.add('go to bin', _sm_go_to_bin_4, transitions={'finished': 'find the bin', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'bin2Waypoint': 'bin2Waypoint'}) # x:286 y:164 OperatableStateMachine.add('find the bin', _sm_find_the_bin_3, transitions={'finished': 'Try twice', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:298 y:254 OperatableStateMachine.add('Try twice', ForLoop(repeat=1), transitions={'do': 'Action_TakeBag', 'end': 'failed'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:102 y:262 OperatableStateMachine.add('Missed It once', SaraSay(sentence="I will try one more time.", input_keys=[], emotion=0, block=True), transitions={'done': 'Try twice'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:15 y:242 OperatableStateMachine.add('button', ContinueButton(), transitions={'true': 'Action_Pass_Door', 'false': 'Action_Pass_Door'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}) # x:485 y:201 OperatableStateMachine.add('try second bin', SaraSay(sentence="I failed to take out the garbage from the first bin but I will try the second bin.", input_keys=[], emotion=0, block=True), transitions={'done': 'second bin'}, autonomy={'done': Autonomy.Off}) # x:267 y:199 OperatableStateMachine.add('say take second bag', SaraSay(sentence="Good! I will now get rid of the second bag.", input_keys=[], emotion=0, block=True), transitions={'done': 'second bin'}, autonomy={'done': Autonomy.Off}) # x:267 y:376 OperatableStateMachine.add('second bin', _sm_second_bin_7, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'bin2Waypoint': 'bin2Waypoint', 'bin2Height': 'bin2Height', 'dropzoneWaypoint': 'dropzone2Waypoint', 'exit_door': 'exit_door'}) # x:94 y:153 OperatableStateMachine.add('IllManageSomehow', SaraSay(sentence="Hmmm. This is a hard door to pass...", input_keys=[], emotion=0, block=True), transitions={'done': 'First bin'}, autonomy={'done': Autonomy.Off}) # x:277 y:63 OperatableStateMachine.add('First bin', _sm_first_bin_6, transitions={'finished': 'say take second bag', 'failed': 'try second bin'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'bin1Waypoint': 'bin1Waypoint', 'bin1Height': 'bin1Height', 'dropzoneWaypoint': 'dropzone1Waypoint', 'exit_door': 'exit_door'}) # x:65 y:53 OperatableStateMachine.add('Action_Pass_Door', self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'Action_Pass_Door'), transitions={'Done': 'First bin', 'Fail': 'IllManageSomehow'}, autonomy={'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit}, remapping={'DoorName': 'DoorName'}) return _state_machine
def create(self): # x:358 y:506 _state_machine = OperatableStateMachine(outcomes=['finished']) _state_machine.userdata.relative = False _state_machine.userdata.EntryName = "door_1_entry" _state_machine.userdata.container = None _state_machine.userdata.TestName = "living room" _state_machine.userdata.ExitName = "door_2_exit" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:52 y:27 OperatableStateMachine.add('Bouton to start', ContinueButton(), transitions={'true': 'Action_Pass_Door', 'false': 'Bouton to start'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}) # x:41 y:430 OperatableStateMachine.add('Failed', SaraSound(sound="error.wav"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:328 y:370 OperatableStateMachine.add('Waiting', SaraSound(sound="to_be_continued.wav"), transitions={'done': 'Bouton continuer'}, autonomy={'done': Autonomy.Off}) # x:310 y:111 OperatableStateMachine.add('Move to test zone', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'Move to test zone'), transitions={'finished': 'say ready', 'failed': 'Failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'TestName'}) # x:329 y:197 OperatableStateMachine.add('say ready', SaraSay(sentence="I'm ready for my safety check. Press the continue button on my back when you are done.", input_keys=[], emotion=1, block=True), transitions={'done': 'Bouton continuer'}, autonomy={'done': Autonomy.Off}) # x:34 y:115 OperatableStateMachine.add('Action_Pass_Door', self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'Action_Pass_Door'), transitions={'Done': 'Move to test zone', 'Fail': 'Failed'}, autonomy={'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit}, remapping={'DoorName': 'EntryName'}) # x:332 y:282 OperatableStateMachine.add('Bouton continuer', ContinueButton(), transitions={'true': 'say bye', 'false': 'Waiting'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}) # x:513 y:432 OperatableStateMachine.add('Action_Pass_Door_2', self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'Action_Pass_Door_2'), transitions={'Done': 'finished', 'Fail': 'Failed'}, autonomy={'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit}, remapping={'DoorName': 'ExitName'}) # x:530 y:282 OperatableStateMachine.add('say bye', SaraSay(sentence="Thank you, See you later.", input_keys=[], emotion=1, block=True), transitions={'done': 'Action_Pass_Door_2'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:666 y:331, x:502 y:199, x:797 y:103 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Position', 'ID']) _state_machine.userdata.relative = False _state_machine.userdata.Position = 0 _state_machine.userdata.ID = 0 _state_machine.userdata.className = "person" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:379 y:221 _sm_wait_to_compte_0 = OperatableStateMachine(outcomes=['finished']) with _sm_wait_to_compte_0: # x:77 y:195 OperatableStateMachine.add('one more wait', WaitState(wait_time=60), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:535 y:314 _sm_turn_around_1 = OperatableStateMachine(outcomes=['failed']) with _sm_turn_around_1: # x:47 y:45 OperatableStateMachine.add('set orientation', SetKey(Value=1.57), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:42 y:213 OperatableStateMachine.add('move head', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'waitwait'}, autonomy={'done': Autonomy.Off}) # x:53 y:294 OperatableStateMachine.add('waitwait', WaitState(wait_time=10), transitions={'done': 'action_turn_2'}, autonomy={'done': Autonomy.Off}) # x:33 y:122 OperatableStateMachine.add( 'action_turn', self.use_behavior( action_turnSM, 'Try to reach/check person behind/move head and base/turn around/action_turn' ), transitions={ 'finished': 'move head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:31 y:434 OperatableStateMachine.add( 'action_turn_2', self.use_behavior( action_turnSM, 'Try to reach/check person behind/move head and base/turn around/action_turn_2' ), transitions={ 'finished': 'head left right', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:35 y:595 OperatableStateMachine.add('head left right', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'wait turn head'}, autonomy={'done': Autonomy.Off}) # x:308 y:537 OperatableStateMachine.add('wait turn head', WaitState(wait_time=10), transitions={'done': 'head right left'}, autonomy={'done': Autonomy.Off}) # x:495 y:604 OperatableStateMachine.add('head right left', SaraSetHeadAngle(pitch=0, yaw=-1.57), transitions={'done': 'wait wait wait'}, autonomy={'done': Autonomy.Off}) # x:262 y:652 OperatableStateMachine.add('wait wait wait', WaitState(wait_time=10), transitions={'done': 'head left right'}, autonomy={'done': Autonomy.Off}) # x:845 y:395 _sm_find_human_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID']) with _sm_find_human_2: # x:143 y:40 OperatableStateMachine.add('set name', SetKey(Value="person"), transitions={'done': 'list'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:361 y:53 OperatableStateMachine.add('list', list_entities_by_name( frontality_level=0.5, distance_max=3), transitions={ 'found': 'finished', 'none_found': 'list' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'name', 'entity_list': 'entity_list', 'number': 'number' }) # x:549 y:135, x:555 y:269, x:230 y:458 _sm_move_head_and_base_3 = ConcurrencyContainer( outcomes=['failed'], conditions=[('failed', [('turn around', 'failed')]), ('failed', [('wait to compte', 'finished')])]) with _sm_move_head_and_base_3: # x:268 y:77 OperatableStateMachine.add('turn around', _sm_turn_around_1, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:263 y:246 OperatableStateMachine.add('wait to compte', _sm_wait_to_compte_0, transitions={'finished': 'failed'}, autonomy={'finished': Autonomy.Inherit}) # x:493 y:206 _sm_container_4 = OperatableStateMachine(outcomes=['check']) with _sm_container_4: # x:230 y:160 OperatableStateMachine.add('wait long', WaitState(wait_time=40), transitions={'done': 'check'}, autonomy={'done': Autonomy.Off}) # x:30 y:458, x:706 y:447 _sm_navigate_to_the_point_5 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['pose']) with _sm_navigate_to_the_point_5: # x:174 y:122 OperatableStateMachine.add('set relative', SetKey(Value=False), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'relative'}) # x:347 y:191 OperatableStateMachine.add( 'Action_Move', self.use_behavior( Action_MoveSM, 'Try to reach/Container/navigate to the point/Action_Move' ), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'pose'}) # x:728 y:335, x:792 y:103, x:738 y:249, x:724 y:448 _sm_check_person_behind_6 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['ID'], conditions=[('failed', [('move head and base', 'failed')]), ('finished', [('find human', 'finished')])]) with _sm_check_person_behind_6: # x:250 y:72 OperatableStateMachine.add('move head and base', _sm_move_head_and_base_3, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:261 y:238 OperatableStateMachine.add('find human', _sm_find_human_2, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:635 y:61, x:634 y:159, x:597 y:300, x:330 y:458, x:430 y:458, x:530 y:458 _sm_container_7 = ConcurrencyContainer( outcomes=['finished', 'failed', 'check'], input_keys=['waypoint'], conditions=[('check', [('Container', 'check')]), ('finished', [('navigate to the point', 'finished')]), ('failed', [('navigate to the point', 'failed')])]) with _sm_container_7: # x:315 y:51 OperatableStateMachine.add('navigate to the point', _sm_navigate_to_the_point_5, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'waypoint'}) # x:322 y:257 OperatableStateMachine.add('Container', _sm_container_4, transitions={'check': 'check'}, autonomy={'check': Autonomy.Inherit}) # x:30 y:458 _sm_groupwait_8 = OperatableStateMachine(outcomes=['end']) with _sm_groupwait_8: # x:30 y:40 OperatableStateMachine.add('waitwait', WaitState(wait_time=20), transitions={'done': 'end'}, autonomy={'done': Autonomy.Off}) # x:534 y:319 _sm_move_head_and_base_at_the_end_9 = OperatableStateMachine( outcomes=['failed']) with _sm_move_head_and_base_at_the_end_9: # x:52 y:31 OperatableStateMachine.add('setkeyorientation', SetKey(Value=1.5), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:51 y:114 OperatableStateMachine.add( 'action_turn', self.use_behavior( action_turnSM, 'operator is still there/Move head and base end /move head and base at the end/action_turn' ), transitions={ 'finished': 'turn right head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:48 y:272 OperatableStateMachine.add('wait while head turn', WaitState(wait_time=4), transitions={'done': 'action_turn_2'}, autonomy={'done': Autonomy.Off}) # x:47 y:199 OperatableStateMachine.add( 'turn right head', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'wait while head turn'}, autonomy={'done': Autonomy.Off}) # x:45 y:348 OperatableStateMachine.add( 'action_turn_2', self.use_behavior( action_turnSM, 'operator is still there/Move head and base end /move head and base at the end/action_turn_2' ), transitions={ 'finished': 'left to rigth', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:43 y:592 OperatableStateMachine.add('left to rigth', SaraSetHeadAngle(pitch=0, yaw=-1.57), transitions={'done': 'waitwait1'}, autonomy={'done': Autonomy.Off}) # x:420 y:602 OperatableStateMachine.add('right to left', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'waitwait2'}, autonomy={'done': Autonomy.Off}) # x:266 y:541 OperatableStateMachine.add('waitwait1', WaitState(wait_time=8), transitions={'done': 'right to left'}, autonomy={'done': Autonomy.Off}) # x:236 y:674 OperatableStateMachine.add('waitwait2', WaitState(wait_time=8), transitions={'done': 'left to rigth'}, autonomy={'done': Autonomy.Off}) # x:231 y:538 _sm_find_a_human_10 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID']) with _sm_find_a_human_10: # x:168 y:102 OperatableStateMachine.add('set name', SetKey(Value="person"), transitions={'done': 'list'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:275 y:203 OperatableStateMachine.add('list', list_entities_by_name( frontality_level=0.5, distance_max=3), transitions={ 'found': 'finished', 'none_found': 'list' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'name', 'entity_list': 'entity_list', 'number': 'number' }) # x:415 y:99, x:318 y:246, x:442 y:295 _sm_move_head_and_base_end__11 = ConcurrencyContainer( outcomes=['failed'], conditions=[('failed', [('move head and base at the end', 'failed') ]), ('failed', [('Groupwait', 'end')])]) with _sm_move_head_and_base_end__11: # x:132 y:57 OperatableStateMachine.add('move head and base at the end', _sm_move_head_and_base_at_the_end_9, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:121 y:218 OperatableStateMachine.add('Groupwait', _sm_groupwait_8, transitions={'end': 'failed'}, autonomy={'end': Autonomy.Inherit}) # x:323 y:632, x:638 y:631 _sm_try_to_reach_12 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['waypoint', 'relative', 'ID']) with _sm_try_to_reach_12: # x:186 y:130 OperatableStateMachine.add('Container', _sm_container_7, transitions={ 'finished': 'Say_Reached', 'failed': 'say_not_reached', 'check': 'say check' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'check': Autonomy.Inherit }, remapping={'waypoint': 'waypoint'}) # x:581 y:163 OperatableStateMachine.add('check person behind', _sm_check_person_behind_6, transitions={ 'finished': 'say found', 'failed': 'say lost' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'ID': 'ID'}) # x:706 y:484 OperatableStateMachine.add( 'say lost', SaraSay(sentence="Oh no! I lost my operator!", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:445 y:69 OperatableStateMachine.add( 'say check', SaraSay(sentence="I check if my operator is still there", input_keys=[], emotion=1, block=True), transitions={'done': 'check person behind'}, autonomy={'done': Autonomy.Off}) # x:443 y:187 OperatableStateMachine.add( 'say found', SaraSay(sentence="Great. You are still there.", input_keys=[], emotion=1, block=True), transitions={'done': 'Container'}, autonomy={'done': Autonomy.Off}) # x:520 y:409 OperatableStateMachine.add( 'say_not_reached', SaraSay(sentence=lambda x: "I have not reach the destination", input_keys=[], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:139 y:339 OperatableStateMachine.add( 'Say_Reached', SaraSay(sentence=lambda x: "I have reach the destination", input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:626 y:228, x:607 y:71, x:230 y:458, x:330 y:458 _sm_operator_is_still_there_13 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['ID'], conditions=[('finished', [('find a human', 'finished')]), ('failed', [('Move head and base end ', 'failed')])]) with _sm_operator_is_still_there_13: # x:207 y:54 OperatableStateMachine.add('Move head and base end ', _sm_move_head_and_base_end__11, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:222 y:190 OperatableStateMachine.add('find a human', _sm_find_a_human_10, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'ID': 'ID'}) with _state_machine: # x:52 y:98 OperatableStateMachine.add('GetPerson', GetEntityByID(), transitions={ 'found': 'sayfollowme', 'not_found': 'Action_findPerson' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:29 y:416 OperatableStateMachine.add('operator is still there', _sm_operator_is_still_there_13, transitions={ 'finished': 'head to middle', 'failed': 'say lost operator' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'ID': 'ID'}) # x:290 y:324 OperatableStateMachine.add( 'say lost operator', SaraSay( sentence= "I have reach my goal but I lost the person I was guiding.", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:43 y:214 OperatableStateMachine.add('sayfollowme', SaraSay(sentence="Follow me please.", input_keys=[], emotion=1, block=True), transitions={'done': 'Try to reach'}, autonomy={'done': Autonomy.Off}) # x:413 y:441 OperatableStateMachine.add('head to middle', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:39 y:330 OperatableStateMachine.add('Try to reach', _sm_try_to_reach_12, transitions={ 'finished': 'operator is still there', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'waypoint': 'Position', 'relative': 'relative', 'ID': 'ID' }) # x:443 y:95 OperatableStateMachine.add('Cant Find Person', SaraSay( sentence="I can't find a person.", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:201 y:57 OperatableStateMachine.add('Action_findPerson', self.use_behavior( Action_findPersonSM, 'Action_findPerson'), transitions={ 'done': 'sayfollowme', 'pas_done': 'Cant Find Person' }, autonomy={ 'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit }, remapping={ 'className': 'className', 'entity': 'Entity' }) return _state_machine
def create(self): # x:902 y:757, x:882 y:161 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:1232 y:907, x:1267 y:613, x:1271 y:566 _sm_find_table_and_place_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'no_table']) with _sm_find_table_and_place_0: # x:877 y:673 OperatableStateMachine.add('run traj', RunTrajectory(file="receive_object", duration=0), transitions={'done': 'say release'}, autonomy={'done': Autonomy.Off}) # x:532 y:572 OperatableStateMachine.add('Action_find_2', self.use_behavior(Action_findSM, 'take objects and bring the order to customer/find table and place/Action_find_2'), transitions={'done': 'get table position', 'failed': 'say do not find table'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'className': 'tableKey', 'entity': 'tableEntity'}) # x:576 y:514 OperatableStateMachine.add('set tableKey', SetKey(Value="table"), transitions={'done': 'Action_find_2'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'tableKey'}) # x:538 y:638 OperatableStateMachine.add('get table position', CalculationState(calculation=lambda x: x.position), transitions={'done': 'Action_place'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'tableEntity', 'output_value': 'pose'}) # x:736 y:677 OperatableStateMachine.add('say do not find table', SaraSay(sentence="I can not find the table. Please be ready to grab the object.", input_keys=[], emotion=0, block=True), transitions={'done': 'run traj'}, autonomy={'done': Autonomy.Off}) # x:41 y:231 OperatableStateMachine.add('find a table and a free spot', GetPositionToPlaceOnTable(), transitions={'done': 'Action_place', 'not_found': 'look down_left'}, autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'}) # x:291 y:805 OperatableStateMachine.add('Action_place', self.use_behavior(Action_placeSM, 'take objects and bring the order to customer/find table and place/Action_place'), transitions={'finished': 'finished', 'failed': 'say do not find table'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pos': 'pose'}) # x:50 y:102 OperatableStateMachine.add('look down', SaraSetHeadAngle(pitch=0, yaw=0.7), transitions={'done': 'wait'}, autonomy={'done': Autonomy.Off}) # x:214 y:378 OperatableStateMachine.add('find a table and a free spot_2', GetPositionToPlaceOnTable(), transitions={'done': 'Action_place', 'not_found': 'look down_right'}, autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'}) # x:240 y:232 OperatableStateMachine.add('look down_left', SaraSetHeadAngle(pitch=0.7, yaw=0.6), transitions={'done': 'wait_2'}, autonomy={'done': Autonomy.Off}) # x:387 y:513 OperatableStateMachine.add('find a table and a free spot_2_2', GetPositionToPlaceOnTable(), transitions={'done': 'Action_place', 'not_found': 'set tableKey'}, autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'}) # x:402 y:378 OperatableStateMachine.add('look down_right', SaraSetHeadAngle(pitch=-0.7, yaw=0.6), transitions={'done': 'wait_3'}, autonomy={'done': Autonomy.Off}) # x:66 y:169 OperatableStateMachine.add('wait', WaitState(wait_time=3), transitions={'done': 'find a table and a free spot'}, autonomy={'done': Autonomy.Off}) # x:242 y:294 OperatableStateMachine.add('wait_2', WaitState(wait_time=3), transitions={'done': 'find a table and a free spot_2'}, autonomy={'done': Autonomy.Off}) # x:411 y:444 OperatableStateMachine.add('wait_3', WaitState(wait_time=3), transitions={'done': 'find a table and a free spot_2_2'}, autonomy={'done': Autonomy.Off}) # x:1000 y:670 OperatableStateMachine.add('say release', SaraSay(sentence="Here is your order. I will open my gripper in 3, 2, 1, now.", input_keys=[], emotion=0, block=True), transitions={'done': 'open gripper'}, autonomy={'done': Autonomy.Off}) # x:1122 y:666 OperatableStateMachine.add('open gripper', SetGripperState(width=0.1, effort=1), transitions={'object': 'say good day', 'no_object': 'say good day'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:1170 y:771 OperatableStateMachine.add('say good day', SaraSay(sentence="Thank you. Have a good day.", input_keys=[], emotion=6, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:62 y:29 OperatableStateMachine.add('set distance', SetKey(Value=0.3), transitions={'done': 'look down'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distanceFromEdge'}) # x:30 y:458, x:130 y:458 _sm_keep_looking_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['personID']) with _sm_keep_looking_1: # x:30 y:40 OperatableStateMachine.add('keep', KeepLookingAt(), transitions={'failed': 'keep'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'personID'}) # x:30 y:458, x:130 y:458 _sm_ask_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['question'], output_keys=['answer']) with _sm_ask_2: # x:30 y:40 OperatableStateMachine.add('Action_Ask', self.use_behavior(Action_AskSM, 'ask and save order/ask AND look person/ask/Action_Ask'), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'question': 'question', 'answer': 'answer'}) # x:30 y:458, x:130 y:458 _sm_confirm_order_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['orderList']) with _sm_confirm_order_3: # x:114 y:67 OperatableStateMachine.add('produce question', CalculationState(calculation=lambda x: "Did you order :" + str(x[0].data)), transitions={'done': 'Action_Ask'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'orderList', 'output_value': 'question'}) # x:423 y:171 OperatableStateMachine.add('Action_Ask', self.use_behavior(Action_AskSM, 'ask and save order/confirm order/Action_Ask'), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'question': 'question', 'answer': 'answer'}) # x:348 y:66, x:351 y:97, x:339 y:159, x:336 y:195, x:518 y:89, x:515 y:175 _sm_ask_and_look_person_4 = ConcurrencyContainer(outcomes=['finished', 'failed'], input_keys=['personID', 'question'], output_keys=['answer'], conditions=[ ('finished', [('ask', 'finished')]), ('failed', [('ask', 'failed')]), ('finished', [('keep looking', 'finished')]), ('failed', [('keep looking', 'failed')]) ]) with _sm_ask_and_look_person_4: # x:30 y:40 OperatableStateMachine.add('ask', _sm_ask_2, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'question': 'question', 'answer': 'answer'}) # x:30 y:138 OperatableStateMachine.add('keep looking', _sm_keep_looking_1, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'personID': 'personID'}) # x:30 y:458 _sm_look_around_2_5 = OperatableStateMachine(outcomes=['finished']) with _sm_look_around_2_5: # x:215 y:156 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:394 y:262 OperatableStateMachine.add('right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:206 y:372 OperatableStateMachine.add('center2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:38 y:596 OperatableStateMachine.add('w1', WaitState(wait_time=4), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:413 y:159 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'right'}, autonomy={'done': Autonomy.Off}) # x:415 y:370 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:39 y:371 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'left'}, autonomy={'done': Autonomy.Off}) # x:25 y:482 OperatableStateMachine.add('left', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:30 y:458 _sm_look_around_6 = OperatableStateMachine(outcomes=['finished']) with _sm_look_around_6: # x:215 y:156 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:394 y:262 OperatableStateMachine.add('right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:206 y:372 OperatableStateMachine.add('center2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:38 y:596 OperatableStateMachine.add('w1', WaitState(wait_time=4), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:413 y:159 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'right'}, autonomy={'done': Autonomy.Off}) # x:415 y:370 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:39 y:371 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'left'}, autonomy={'done': Autonomy.Off}) # x:25 y:482 OperatableStateMachine.add('left', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:30 y:458, x:130 y:458 _sm_turn_around_7 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_turn_around_7: # x:80 y:26 OperatableStateMachine.add('look around', _sm_look_around_6, transitions={'finished': 'pose with 180 rotation'}, autonomy={'finished': Autonomy.Inherit}) # x:66 y:286 OperatableStateMachine.add('rotation', SaraMoveBase(reference="base_link"), transitions={'arrived': 'look around_2', 'failed': 'failed'}, autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'pose': 'pose'}) # x:56 y:388 OperatableStateMachine.add('look around_2', _sm_look_around_2_5, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}) # x:63 y:133 OperatableStateMachine.add('pose with 180 rotation', GenPoseEuler(x=0, y=0, z=0, roll=0, pitch=0, yaw=3.14), transitions={'done': 'rotation'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'pose'}) # x:30 y:458 _sm_get_waving_people_8 = OperatableStateMachine(outcomes=['finished'], output_keys=['waving_person']) with _sm_get_waving_people_8: # x:47 y:45 OperatableStateMachine.add('set name', SetKey(Value="person"), transitions={'done': 'list people'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:40 y:236 OperatableStateMachine.add('filter waving', FilterKey(filter_function=lambda x: x[0].pose.right_arm_up or x[0].pose.left_arm_up, input_keys=["input_list"]), transitions={'not_empty': 'get first', 'empty': 'list people'}, autonomy={'not_empty': Autonomy.Off, 'empty': Autonomy.Off}, remapping={'input_list': 'entity_list', 'output_list': 'waving_peoples'}) # x:53 y:326 OperatableStateMachine.add('get first', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'waving_peoples', 'output_value': 'waving_person'}) # x:40 y:133 OperatableStateMachine.add('list people', list_entities_by_name(frontality_level=0.5, distance_max=5), transitions={'found': 'filter waving', 'none_found': 'list people'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'name', 'entity_list': 'entity_list', 'number': 'number'}) # x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458 _sm_look_for_waving_people_9 = ConcurrencyContainer(outcomes=['finished', 'failed'], output_keys=['waving_person'], conditions=[ ('failed', [('turn around', 'failed')]), ('failed', [('turn around', 'finished')]), ('finished', [('get waving people', 'finished')]) ]) with _sm_look_for_waving_people_9: # x:268 y:118 OperatableStateMachine.add('get waving people', _sm_get_waving_people_8, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'waving_person': 'waving_person'}) # x:60 y:126 OperatableStateMachine.add('turn around', _sm_turn_around_7, transitions={'finished': 'failed', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:441 y:583, x:93 y:568 _sm_repeate_if_first_commande_10 = OperatableStateMachine(outcomes=['finished', 'repeate'], input_keys=['commandNumber'], output_keys=['commandNumber']) with _sm_repeate_if_first_commande_10: # x:242 y:97 OperatableStateMachine.add('if first command', CheckConditionState(predicate=lambda x: x == 1), transitions={'true': 'set second command', 'false': 'finished'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'commandNumber'}) # x:73 y:329 OperatableStateMachine.add('set second command', SetKey(Value=2), transitions={'done': 'repeate'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'commandNumber'}) # x:1053 y:687, x:1068 y:149 _sm_take_objects_and_bring_the_order_to_customer_11 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['barPosition', 'orderList', 'robotPositionToCustomer']) with _sm_take_objects_and_bring_the_order_to_customer_11: # x:73 y:26 OperatableStateMachine.add('set indexkey', SetKey(Value=0), transitions={'done': 'one element by one element from the list'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'indexKey'}) # x:591 y:684 OperatableStateMachine.add('increment indexKey', CalculationState(calculation=lambda x: x+1), transitions={'done': 'check if end of the list'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'indexKey', 'output_value': 'indexKey'}) # x:796 y:679 OperatableStateMachine.add('check if end of the list', FlexibleCheckConditionState(predicate=lambda x: len(x[0]) <= x[1], input_keys=["orderList", "indexKey"]), transitions={'true': 'finished', 'false': 'Action_Move_2'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'orderList': 'orderList', 'indexKey': 'indexKey'}) # x:31 y:243 OperatableStateMachine.add('Action_find', self.use_behavior(Action_findSM, 'take objects and bring the order to customer/Action_find'), transitions={'done': 'get entity ID', 'failed': 'say cannot find'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'className': 'item', 'entity': 'entity'}) # x:76 y:183 OperatableStateMachine.add('say search and grip', SaraSay(sentence=lambda x: "I am now searching for the "+x[0]+".", input_keys=["item"], emotion=0, block=True), transitions={'done': 'Action_find'}, autonomy={'done': Autonomy.Off}, remapping={'item': 'item'}) # x:258 y:261 OperatableStateMachine.add('say cannot find', SaraSay(sentence="I cannot find the item.", input_keys=[], emotion=0, block=True), transitions={'done': 'place arm'}, autonomy={'done': Autonomy.Off}) # x:63 y:393 OperatableStateMachine.add('Action_pick', self.use_behavior(Action_pickSM, 'take objects and bring the order to customer/Action_pick'), transitions={'success': 'say go to the customer', 'unreachable': 'say cannot pick', 'not found': 'say cannot pick', 'dropped': 'say cannot pick'}, autonomy={'success': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'not found': Autonomy.Inherit, 'dropped': Autonomy.Inherit}, remapping={'objectID': 'entityID'}) # x:61 y:548 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'take objects and bring the order to customer/Action_Move'), transitions={'finished': 'find table and place', 'failed': 'say cant get back to customer'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'robotPositionToCustomer'}) # x:83 y:326 OperatableStateMachine.add('get entity ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'Action_pick'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity', 'output_value': 'entityID'}) # x:568 y:407 OperatableStateMachine.add('say put in the gripper', SaraSay(sentence=lambda x: "Please, put the "+x[0]+" in my gripper. I will close it in 3, 2, 1, now.", input_keys=["item"], emotion=0, block=True), transitions={'done': 'close gripper'}, autonomy={'done': Autonomy.Off}, remapping={'item': 'item'}) # x:243 y:387 OperatableStateMachine.add('say cannot pick', SaraSay(sentence="I can not pick the item.", input_keys=[], emotion=0, block=True), transitions={'done': 'place arm'}, autonomy={'done': Autonomy.Off}) # x:388 y:502 OperatableStateMachine.add('say thank you', SaraSay(sentence="Thank you.", input_keys=[], emotion=0, block=True), transitions={'done': 'place arm 2'}, autonomy={'done': Autonomy.Off}) # x:72 y:465 OperatableStateMachine.add('say go to the customer', SaraSay(sentence="I will serve it to the customer.", input_keys=[], emotion=0, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:522 y:505 OperatableStateMachine.add('close gripper', SetGripperState(width=0, effort=1), transitions={'object': 'say thank you', 'no_object': 'open gripper'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:357 y:569 OperatableStateMachine.add('say cant get back to customer', SaraSay(sentence="I am not able to go back to the customer.", input_keys=[], emotion=0, block=True), transitions={'done': 'increment indexKey'}, autonomy={'done': Autonomy.Off}) # x:541 y:317 OperatableStateMachine.add('open gripper', SetGripperState(width=0.10, effort=1), transitions={'object': 'say put in the gripper', 'no_object': 'say put in the gripper'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:63 y:110 OperatableStateMachine.add('one element by one element from the list', FlexibleCalculationState(calculation=lambda x: x[0][x[1]].data, input_keys=["orderList", "indexKey"]), transitions={'done': 'say search and grip'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList', 'indexKey': 'indexKey', 'output_value': 'item'}) # x:376 y:324 OperatableStateMachine.add('place arm', RunTrajectory(file="receive_object", duration=0), transitions={'done': 'open gripper'}, autonomy={'done': Autonomy.Off}) # x:638 y:184 OperatableStateMachine.add('Action_Move_2', self.use_behavior(Action_MoveSM, 'take objects and bring the order to customer/Action_Move_2'), transitions={'finished': 'increment indexKey', 'failed': 'say cannot go back to bar'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'barPosition'}) # x:817 y:178 OperatableStateMachine.add('say cannot go back to bar', SaraSay(sentence="I am not able to go back to the barman. I will try a new command.", input_keys=[], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:85 y:740 OperatableStateMachine.add('find table and place', _sm_find_table_and_place_0, transitions={'finished': 'retour repos', 'failed': 'retour repos', 'no_table': 'retour repos'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'no_table': Autonomy.Inherit}) # x:248 y:505 OperatableStateMachine.add('place arm 2', RunTrajectory(file="transport", duration=0), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:362 y:679 OperatableStateMachine.add('retour repos', RunTrajectory(file="repos", duration=0), transitions={'done': 'increment indexKey'}, autonomy={'done': Autonomy.Off}) # x:242 y:352, x:1086 y:243 _sm_get_the_order_12 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['orderList']) with _sm_get_the_order_12: # x:68 y:39 OperatableStateMachine.add('length 1', CheckConditionState(predicate=lambda x: len(x) == 1), transitions={'true': 'say order', 'false': 'length 2'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'orderList'}) # x:236 y:41 OperatableStateMachine.add('length 2', CheckConditionState(predicate=lambda x: len(x) ==2), transitions={'true': 'say order_2', 'false': 'say order_3'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'orderList'}) # x:228 y:111 OperatableStateMachine.add('say order_2', SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+" and one "+x[0][1].data+", please.", input_keys=["orderList"], emotion=0, block=True), transitions={'done': 'wait 5'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList'}) # x:374 y:109 OperatableStateMachine.add('say order_3', SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+", one "+x[0][1].data+" and one "+x[0][2].data+", please.", input_keys=["orderList"], emotion=0, block=True), transitions={'done': 'wait 5'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList'}) # x:67 y:117 OperatableStateMachine.add('say order', SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+", please.", input_keys=["orderList"], emotion=0, block=True), transitions={'done': 'wait 5'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList'}) # x:224 y:256 OperatableStateMachine.add('wait 5', WaitState(wait_time=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:216 y:528, x:572 y:238 _sm_go_to_the_barman_13 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['barPosition', 'barmanID']) with _sm_go_to_the_barman_13: # x:30 y:102 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'go to the barman/Action_Move'), transitions={'finished': 'if barman id is 0', 'failed': 'say cannot'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'barPosition'}) # x:365 y:99 OperatableStateMachine.add('say cannot', SaraSay(sentence="I can not reach my destination. I will take another order. If you have one, please raise your hand.", input_keys=[], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:30 y:320 OperatableStateMachine.add('Action_findPersonByID', self.use_behavior(Action_findPersonByIDSM, 'go to the barman/Action_findPersonByID', default_keys=['className']), transitions={'found': 'finished', 'not_found': 'finished'}, autonomy={'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit}, remapping={'personID': 'barmanID', 'personEntity': 'personEntity'}) # x:115 y:230 OperatableStateMachine.add('if barman id is 0', CheckConditionState(predicate=lambda x: x == 0), transitions={'true': 'finished', 'false': 'Action_findPersonByID'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'barmanID'}) # x:898 y:561, x:871 y:123 _sm_ask_and_save_order_14 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['customerID'], output_keys=['orderList']) with _sm_ask_and_save_order_14: # x:70 y:112 OperatableStateMachine.add('say ready', SaraSay(sentence="Hello.", input_keys=[], emotion=0, block=True), transitions={'done': 'set question'}, autonomy={'done': Autonomy.Off}) # x:69 y:187 OperatableStateMachine.add('set question', SetKey(Value="What do you want to order?"), transitions={'done': 'ask AND look person'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'question'}) # x:411 y:138 OperatableStateMachine.add('say cannot', SaraSay(sentence="I am not able to understand your order. I will take another order.", input_keys=[], emotion=0, block=True), transitions={'done': 'set question'}, autonomy={'done': Autonomy.Off}) # x:59 y:272 OperatableStateMachine.add('ask AND look person', _sm_ask_and_look_person_4, transitions={'finished': 'nlu restaurant', 'failed': 'say cannot'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'personID': 'customerID', 'question': 'question', 'answer': 'answer'}) # x:411 y:453 OperatableStateMachine.add('nlu restaurant', SaraNLUrestaurant(), transitions={'understood': 'confirm order', 'fail': 'say cannot'}, autonomy={'understood': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'sentence': 'answer', 'orderList': 'orderList'}) # x:657 y:417 OperatableStateMachine.add('confirm order', _sm_confirm_order_3, transitions={'finished': 'say bring', 'failed': 'set question'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'orderList': 'orderList'}) # x:852 y:402 OperatableStateMachine.add('say bring', SaraSay(sentence=lambda x: "understood. I will bring you the "+str(x[0][0].data), input_keys=["orderList"], emotion=0, block=False), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList'}) # x:677 y:318, x:735 y:160 _sm_move_to_table_and_save_position_15 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['customerPosition'], output_keys=['robotPositionToCustomer']) with _sm_move_to_table_and_save_position_15: # x:69 y:24 OperatableStateMachine.add('set distance to person', SetKey(Value=0.7), transitions={'done': 'compute robot pose to customer'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distanceToPerson'}) # x:344 y:115 OperatableStateMachine.add('say cannot', SaraSay(sentence="I can not reach my destination. I will take another order. If you have one, please raise your hand.", input_keys=[], emotion=0, block=True), transitions={'done': 'save pose to not get flexbe problems'}, autonomy={'done': Autonomy.Off}) # x:497 y:129 OperatableStateMachine.add('save pose to not get flexbe problems', Get_Robot_Pose(), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'pose'}) # x:47 y:98 OperatableStateMachine.add('compute robot pose to customer', Get_Reacheable_Waypoint(), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'pose_in': 'customerPosition', 'distance': 'distanceToPerson', 'pose_out': 'robotPositionToCustomer'}) # x:78 y:199 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'move to table and save position/Action_Move'), transitions={'finished': 'finished', 'failed': 'say cannot'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'robotPositionToCustomer'}) # x:112 y:525, x:363 y:46 _sm_detect_people_waving_16 = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['customerPosition', 'customerID']) with _sm_detect_people_waving_16: # x:84 y:25 OperatableStateMachine.add('say looking', SaraSay(sentence="I am now looking for customers. If you want to order something, please raise your hand.", input_keys=[], emotion=0, block=False), transitions={'done': 'Look for waving people'}, autonomy={'done': Autonomy.Off}) # x:73 y:375 OperatableStateMachine.add('reachable position_2', Get_Reacheable_Waypoint(), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'pose_in': 'customerPosition', 'distance': 'distance', 'pose_out': 'customerPosition'}) # x:71 y:291 OperatableStateMachine.add('set distance to customer_2', SetKey(Value=0.5), transitions={'done': 'reachable position_2'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distance'}) # x:94 y:216 OperatableStateMachine.add('get attributes', GetAttribute(attributes=["ID", "position"]), transitions={'done': 'set distance to customer_2'}, autonomy={'done': Autonomy.Off}, remapping={'object': 'waving_person', 'ID': 'customerID', 'position': 'customerPosition'}) # x:63 y:120 OperatableStateMachine.add('Look for waving people', _sm_look_for_waving_people_9, transitions={'finished': 'get attributes', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'waving_person': 'waving_person'}) # x:874 y:499, x:747 y:100 _sm_save_bar_position_and_initiation_17 = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['barPosition', 'commandNumber', 'barmanID']) with _sm_save_bar_position_and_initiation_17: # x:66 y:96 OperatableStateMachine.add('set question barman', SetKey(Value="Are you the bartender?"), transitions={'done': 'Action_FindPersonByQuestion'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'questionBarman'}) # x:488 y:85 OperatableStateMachine.add('say failed to find the barman', SaraSay(sentence="I am not able to find the barman. I will get the order and come back here to get the items.", input_keys=[], emotion=0, block=True), transitions={'done': 'barman ID to 0'}, autonomy={'done': Autonomy.Off}) # x:83 y:406 OperatableStateMachine.add('save barman ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'get current pose'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'personFound', 'output_value': 'barmanID'}) # x:512 y:183 OperatableStateMachine.add('barman ID to 0', SetKey(Value=0), transitions={'done': 'get current pose'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'barmanID'}) # x:488 y:391 OperatableStateMachine.add('get current pose', Get_Robot_Pose(), transitions={'done': 'set first command'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'barPosition'}) # x:55 y:214 OperatableStateMachine.add('Action_FindPersonByQuestion', self.use_behavior(Action_FindPersonByQuestionSM, 'save bar position and initiation/Action_FindPersonByQuestion'), transitions={'found': 'save barman ID', 'failed': 'say failed to find the barman', 'not_found': 'say failed to find the barman'}, autonomy={'found': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'not_found': Autonomy.Inherit}, remapping={'question': 'questionBarman', 'entityFound': 'personFound'}) # x:489 y:478 OperatableStateMachine.add('set first command', SetKey(Value=1), transitions={'done': 'say instructions'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'commandNumber'}) # x:689 y:477 OperatableStateMachine.add('say instructions', SaraSay(sentence="Good! I wil now start to serve people.", input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:30 y:40 OperatableStateMachine.add('save bar position and initiation', _sm_save_bar_position_and_initiation_17, transitions={'finished': 'detect people waving', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'barPosition': 'barPosition', 'commandNumber': 'commandNumber', 'barmanID': 'barmanID'}) # x:131 y:141 OperatableStateMachine.add('detect people waving', _sm_detect_people_waving_16, transitions={'finished': 'move to table and save position', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'customerPosition': 'customerPosition', 'customerID': 'customerID'}) # x:233 y:250 OperatableStateMachine.add('move to table and save position', _sm_move_to_table_and_save_position_15, transitions={'finished': 'ask and save order', 'failed': 'detect people waving'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'customerPosition': 'customerPosition', 'robotPositionToCustomer': 'robotPositionToCustomer'}) # x:286 y:337 OperatableStateMachine.add('ask and save order', _sm_ask_and_save_order_14, transitions={'finished': 'go to the barman', 'failed': 'detect people waving'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'customerID': 'customerID', 'orderList': 'orderList'}) # x:264 y:434 OperatableStateMachine.add('go to the barman', _sm_go_to_the_barman_13, transitions={'finished': 'get the order', 'failed': 'detect people waving'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'barPosition': 'barPosition', 'barmanID': 'barmanID'}) # x:278 y:526 OperatableStateMachine.add('get the order', _sm_get_the_order_12, transitions={'finished': 'take objects and bring the order to customer', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'orderList': 'orderList'}) # x:260 y:626 OperatableStateMachine.add('take objects and bring the order to customer', _sm_take_objects_and_bring_the_order_to_customer_11, transitions={'finished': 'repeate if first commande', 'failed': 'repeate if first commande'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'barPosition': 'barPosition', 'orderList': 'orderList', 'robotPositionToCustomer': 'robotPositionToCustomer'}) # x:118 y:735 OperatableStateMachine.add('repeate if first commande', _sm_repeate_if_first_commande_10, transitions={'finished': 'say finish', 'repeate': 'detect people waving'}, autonomy={'finished': Autonomy.Inherit, 'repeate': Autonomy.Inherit}, remapping={'commandNumber': 'commandNumber'}) # x:431 y:743 OperatableStateMachine.add('say finish', SaraSay(sentence="I reach the end of my scenario. I will go back to the bar.", input_keys=[], emotion=0, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:616 y:735 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'Action_Move'), transitions={'finished': 'finished', 'failed': 'finished'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'barPosition'}) return _state_machine
def create(self): # x:283 y:267 _state_machine = OperatableStateMachine(outcomes=['Shutdown']) _state_machine.userdata.Command = "no nothing" _state_machine.userdata.End = False # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:887 y:420 _sm_sara_brain_0 = OperatableStateMachine( outcomes=['error'], input_keys=['HighFIFO', 'LowFIFO', 'MedFIFO', 'DoNow', 'End']) with _sm_sara_brain_0: # x:270 y:346 OperatableStateMachine.add( 'sara_command_manager', self.use_behavior( sara_command_managerSM, 'Sara parallel Runtime/Sara brain/sara_command_manager'), transitions={ 'finished': 'if stop', 'failed': 'set end' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'DoNow': 'DoNow' }) # x:725 y:388 OperatableStateMachine.add( 'set end', CalculationState(calculation=lambda x: True), transitions={'done': 'error'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'End', 'output_value': 'End' }) # x:516 y:448 OperatableStateMachine.add( 'if stop', CheckConditionState(predicate=lambda x: x[0][0] == "Stop"), transitions={ 'true': 'set end', 'false': 'sara_command_manager' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'DoNow'}) # x:841 y:231 _sm_sara_action_executor_1 = OperatableStateMachine( outcomes=['shutdown'], input_keys=['HighFIFO', 'MedFIFO', 'LowFIFO', 'DoNow', 'End']) with _sm_sara_action_executor_1: # x:128 y:134 OperatableStateMachine.add( 'log', LogState(text="Start action executor", severity=Logger.REPORT_HINT), transitions={'done': 'Sara action executor'}, autonomy={'done': Autonomy.Off}) # x:636 y:111 OperatableStateMachine.add( 'Critical failure', LogState(text="Critical fail in action executer!", severity=Logger.REPORT_HINT), transitions={'done': 'shutdown'}, autonomy={'done': Autonomy.Off}) # x:366 y:149 OperatableStateMachine.add( 'Sara action executor', self.use_behavior( SaraactionexecutorSM, 'Sara parallel Runtime/Sara action executor/Sara action executor' ), transitions={ 'CriticalFail': 'Critical failure', 'Shutdown': 'shutdown' }, autonomy={ 'CriticalFail': Autonomy.Inherit, 'Shutdown': Autonomy.Inherit }, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'End': 'End' }) # x:30 y:365 _sm_sara_init_2 = OperatableStateMachine(outcomes=['done']) with _sm_sara_init_2: # x:30 y:40 OperatableStateMachine.add('set head', SaraSetHeadAngle(angle=0.1), transitions={'done': 'set face'}, autonomy={'done': Autonomy.Off}) # x:47 y:119 OperatableStateMachine.add('set arm', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'hello', 'failed': 'hello' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'target'}) # x:242 y:288 OperatableStateMachine.add( 'hello', SaraSay( sentence= "Good morning. I am Sara the robot. Please give me orders.", emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:239 y:21 OperatableStateMachine.add('set face', SetExpression(emotion=0, brightness=200), transitions={'done': 'www'}, autonomy={'done': Autonomy.Off}) # x:339 y:116 OperatableStateMachine.add('on face', SetExpression(emotion=1, brightness=-1), transitions={'done': 'setTarget'}, autonomy={'done': Autonomy.Off}) # x:484 y:41 OperatableStateMachine.add('www', WaitState(wait_time=2), transitions={'done': 'on face'}, autonomy={'done': Autonomy.Off}) # x:197 y:113 OperatableStateMachine.add('setTarget', SetKey(Value="IdlePose"), transitions={'done': 'set arm'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:55 y:366 _sm_create_fifos_3 = OperatableStateMachine( outcomes=['done'], output_keys=['HighFIFO', 'MedFIFO', 'LowFIFO', 'DoNow']) with _sm_create_fifos_3: # x:33 y:40 OperatableStateMachine.add('Create HighFIFO', FIFO_New(), transitions={'done': 'Create MedFIFO'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'HighFIFO'}) # x:30 y:114 OperatableStateMachine.add('Create MedFIFO', FIFO_New(), transitions={'done': 'Create LowFIFO'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'MedFIFO'}) # x:30 y:189 OperatableStateMachine.add('Create LowFIFO', FIFO_New(), transitions={'done': 'Create DoNow'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'LowFIFO'}) # x:34 y:260 OperatableStateMachine.add('Create DoNow', FIFO_New(), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'DoNow'}) # x:30 y:322 _sm_sara_shutdown_4 = OperatableStateMachine(outcomes=['finished']) with _sm_sara_shutdown_4: # x:57 y:86 OperatableStateMachine.add('log', LogState(text="shutdown", severity=Logger.REPORT_HINT), transitions={'done': 'say'}, autonomy={'done': Autonomy.Off}) # x:122 y:228 OperatableStateMachine.add( 'say', SaraSay(sentence="I'm goint to shutdown for safety reasons", emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:336 y:314, x:315 y:433, x:276 y:271 _sm_sara_parallel_runtime_5 = ConcurrencyContainer( outcomes=['Shutdown'], input_keys=['HighFIFO', 'MedFIFO', 'LowFIFO', 'DoNow', 'End'], conditions=[('Shutdown', [('Sara brain', 'error')]), ('Shutdown', [('Sara action executor', 'shutdown')])]) with _sm_sara_parallel_runtime_5: # x:52 y:416 OperatableStateMachine.add('Sara action executor', _sm_sara_action_executor_1, transitions={'shutdown': 'Shutdown'}, autonomy={'shutdown': Autonomy.Inherit}, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'DoNow': 'DoNow', 'End': 'End' }) # x:57 y:333 OperatableStateMachine.add('Sara brain', _sm_sara_brain_0, transitions={'error': 'Shutdown'}, autonomy={'error': Autonomy.Inherit}, remapping={ 'HighFIFO': 'HighFIFO', 'LowFIFO': 'LowFIFO', 'MedFIFO': 'MedFIFO', 'DoNow': 'DoNow', 'End': 'End' }) with _state_machine: # x:43 y:60 OperatableStateMachine.add('log', LogState(text="Start Sara", severity=Logger.REPORT_HINT), transitions={'done': 'low head'}, autonomy={'done': Autonomy.Off}) # x:306 y:365 OperatableStateMachine.add( 'Sara parallel Runtime', _sm_sara_parallel_runtime_5, transitions={'Shutdown': 'Sara shutdown'}, autonomy={'Shutdown': Autonomy.Inherit}, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'DoNow': 'DoNow', 'End': 'End' }) # x:315 y:462 OperatableStateMachine.add('Sara shutdown', _sm_sara_shutdown_4, transitions={'finished': 'low head'}, autonomy={'finished': Autonomy.Inherit}) # x:328 y:284 OperatableStateMachine.add( 'Create_FIFOs', _sm_create_fifos_3, transitions={'done': 'Sara parallel Runtime'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'DoNow': 'DoNow' }) # x:150 y:207 OperatableStateMachine.add('listen', GetSpeech(watchdog=10), transitions={ 'done': 'check hello', 'nothing': 'listen', 'fail': 'Shutdown' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) # x:345 y:195 OperatableStateMachine.add('Sara init', _sm_sara_init_2, transitions={'done': 'Create_FIFOs'}, autonomy={'done': Autonomy.Inherit}) # x:355 y:96 OperatableStateMachine.add( 'check hello', RegexTester( regex= ".*((wake up)|(sarah?)|(shut up)|(hello)(robot)|(hi)|(morning)|(greet)).*" ), transitions={ 'true': 'Sara init', 'false': 'listen' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'words', 'result': 'result' }) # x:15 y:220 OperatableStateMachine.add('close face', SetExpression(emotion=0, brightness=20), transitions={'done': 'listen'}, autonomy={'done': Autonomy.Off}) # x:102 y:115 OperatableStateMachine.add('low head', SaraSetHeadAngle(angle=0.8), transitions={'done': 'close face'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:825 y:465, x:801 y:48 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:458, x:130 y:458 _sm_torque_control_0 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_torque_control_0: # x:126 y:51 OperatableStateMachine.add('w2', WaitState(wait_time=5), transitions={'done': 'check torque'}, autonomy={'done': Autonomy.Off}) # x:120 y:195 OperatableStateMachine.add('check torque', ReadTorque( watchdog=20, Joint="right_shoulder_pitch_joint", Threshold=2.5, min_time=0.5), transitions={ 'threshold': 'finished', 'watchdog': 'check torque', 'fail': 'check torque' }, autonomy={ 'threshold': Autonomy.Off, 'watchdog': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'torque': 'torque'}) # x:30 y:458, x:130 y:458 _sm_trajectory_down_1 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_trajectory_down_1: # x:68 y:158 OperatableStateMachine.add('run down', RunTrajectory(file="poubelle_app", duration=16), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458, x:530 y:458 _sm_container_2 = ConcurrencyContainer( outcomes=['finished', 'failed'], conditions=[('failed', [('test', 'done')]), ('finished', [('tor', 'threshold')]), ('failed', [('tor', 'watchdog')]), ('failed', [('tor', 'fail')])]) with _sm_container_2: # x:67 y:108 OperatableStateMachine.add('test', RunTrajectory(file="poubelle_valide", duration=7), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:327 y:130 OperatableStateMachine.add('tor', ReadTorque( watchdog=7, Joint="right_elbow_yaw_joint", Threshold=1.6, min_time=1), transitions={ 'threshold': 'finished', 'watchdog': 'failed', 'fail': 'failed' }, autonomy={ 'threshold': Autonomy.Off, 'watchdog': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'torque': 'torque'}) # x:30 y:365, x:130 y:365 _sm_trajectory_to_transport_pose_3 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_trajectory_to_transport_pose_3: # x:30 y:40 OperatableStateMachine.add('trajectory to transport pose', RunTrajectory(file="poubelle_transport", duration=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:479 y:236, x:469 y:51, x:462 y:111, x:471 y:160, x:430 y:365, x:530 y:365 _sm_trajectory_down_with_torque_limit_4 = ConcurrencyContainer( outcomes=['finished', 'failed'], conditions=[('finished', [('torque control', 'finished')]), ('finished', [('trajectory down', 'finished')]), ('failed', [('trajectory down', 'failed')]), ('failed', [('torque control', 'failed')])]) with _sm_trajectory_down_with_torque_limit_4: # x:109 y:63 OperatableStateMachine.add('trajectory down', _sm_trajectory_down_1, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:102 y:211 OperatableStateMachine.add('torque control', _sm_torque_control_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:30 y:458, x:130 y:458 _sm_has_bag_in_gripper_5 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_has_bag_in_gripper_5: # x:191 y:132 OperatableStateMachine.add('Container', _sm_container_2, transitions={ 'finished': 'ok', 'failed': 'bad' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:18 y:266 OperatableStateMachine.add('ok', SaraSay(sentence="Yay! I got it!", input_keys=[], emotion=6, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:238 y:279 OperatableStateMachine.add('bad', SaraSay(sentence="Woops! I missed!", input_keys=[], emotion=3, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:30 y:458, x:130 y:458 _sm_trajectory_up_6 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_trajectory_up_6: # x:71 y:122 OperatableStateMachine.add('trajectory up', RunTrajectory(file="poubelle_eloigne", duration=10), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:30 y:458, x:130 y:458 _sm_close_gripper_7 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_close_gripper_7: # x:79 y:177 OperatableStateMachine.add( 'close gripper', SetGripperState(width=0, effort=1), transitions={ 'object': 'finished', 'no_object': 'retry close' }, autonomy={ 'object': Autonomy.Off, 'no_object': Autonomy.Off }, remapping={'object_size': 'object_size'}) # x:272 y:237 OperatableStateMachine.add('retry close', ForLoop(repeat=1), transitions={ 'do': 'close gripper', 'end': 'failed' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) with _state_machine: # x:89 y:35 OperatableStateMachine.add('head down', SaraSetHeadAngle(pitch=0.8, yaw=0), transitions={'done': 'open gripper'}, autonomy={'done': Autonomy.Off}) # x:484 y:164 OperatableStateMachine.add('close gripper', _sm_close_gripper_7, transitions={ 'finished': 'trajectory up', 'failed': 'trajectory up' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:473 y:300 OperatableStateMachine.add('trajectory up', _sm_trajectory_up_6, transitions={ 'finished': 'wait a little bit', 'failed': 'wait a little bit' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:232 y:434 OperatableStateMachine.add('has bag in gripper', _sm_has_bag_in_gripper_5, transitions={ 'finished': 'trajectory to transport pose', 'failed': 'open gripper' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:467 y:33 OperatableStateMachine.add('trajectory down with torque limit', _sm_trajectory_down_with_torque_limit_4, transitions={ 'finished': 'close gripper', 'failed': 'head down' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:283 y:83 OperatableStateMachine.add( 'open gripper', SetGripperState(width=0.12, effort=0), transitions={ 'object': 'trajectory down with torque limit', 'no_object': 'trajectory down with torque limit' }, autonomy={ 'object': Autonomy.Off, 'no_object': Autonomy.Off }, remapping={'object_size': 'object_size'}) # x:557 y:454 OperatableStateMachine.add('trajectory to transport pose', _sm_trajectory_to_transport_pose_3, transitions={ 'finished': 'finished', 'failed': 'finished' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:356 y:357 OperatableStateMachine.add( 'wait a little bit', WaitState(wait_time=1), transitions={'done': 'has bag in gripper'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:702 y:576, x:764 y:158, x:766 y:33 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Place", "table"] _state_machine.userdata.Empty = None _state_machine.userdata.IdlePos = "IdlePose" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:44 y:28 OperatableStateMachine.add( 'gripper contain', GetRosParam(ParamName="behavior/GripperContent"), transitions={ 'done': 'if contain something', 'failed': 'cause1' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'content'}) # x:222 y:497 OperatableStateMachine.add('Action_place', self.use_behavior( Action_placeSM, 'Action_place'), transitions={ 'finished': 'idlearm', 'failed': 'cause3' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pos': 'MapPosition'}) # x:39 y:367 OperatableStateMachine.add( 'genPoseArm', GenPoseEuler(x=0.75, y=-0.25, z=0.85, roll=0, pitch=0, yaw=0), transitions={'done': 'referential from robot to map'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'position'}) # x:8 y:433 OperatableStateMachine.add('referential from robot to map', TF_transformation(in_ref="base_link", out_ref="map"), transitions={ 'done': 'log pose', 'fail': 'log tf error' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'in_pos': 'position', 'out_pos': 'MapPosition' }) # x:25 y:98 OperatableStateMachine.add( 'if contain something', CheckConditionState(predicate=lambda x: x != ''), transitions={ 'true': 'cond', 'false': 'say nothing in gripper' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'content'}) # x:209 y:98 OperatableStateMachine.add( 'say nothing in gripper', SaraSay(sentence="It seems I have nothing in my gripper", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:28 y:236 OperatableStateMachine.add( 'construction phrase', FlexibleCalculationState( calculation=lambda x: "I will place this " + str(x[ 0]) + " on the " + str(x[1][1]), input_keys=["content", "Action"]), transitions={'done': 'Say_Place_object'}, autonomy={'done': Autonomy.Off}, remapping={ 'content': 'content', 'Action': 'Action', 'output_value': 'sentence' }) # x:33 y:167 OperatableStateMachine.add( 'cond', CheckConditionState(predicate=lambda x: x[1] != ''), transitions={ 'true': 'construction phrase', 'false': 'cause2' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'Action'}) # x:257 y:413 OperatableStateMachine.add('log tf error', LogState(text="tf error", severity=Logger.REPORT_HINT), transitions={'done': 'cause3'}, autonomy={'done': Autonomy.Off}) # x:42 y:502 OperatableStateMachine.add( 'log pose', LogKeyState(text="the placement pose will be: {}", severity=Logger.REPORT_HINT), transitions={'done': 'Action_place'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'MapPosition'}) # x:493 y:535 OperatableStateMachine.add( 'empty hand', SetRosParam(ParamName="behavior/GripperContent"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Empty'}) # x:448 y:54 OperatableStateMachine.add( 'cause1', SetKey(Value="I didn't have any object in my gripper"), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:422 y:149 OperatableStateMachine.add( 'cause2', SetKey(Value="I didn't know where to place the object."), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:575 y:158 OperatableStateMachine.add( 'setrosparamcause', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:449 y:325 OperatableStateMachine.add( 'cause3', SetKey( Value="I was unable to calculate how to place the object." ), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:342 y:583 OperatableStateMachine.add('idlearm', MoveitMove(move=True, waitForExecution=False, group="RightArm", watchdog=15), transitions={ 'done': 'empty hand', 'failed': 'empty hand' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'IdlePos'}) # x:706 y:460 OperatableStateMachine.add( 'Say_Place_It_This_Place', SaraSay(sentence=lambda x: "I will place this " + x + " right there.", input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:35 y:301 OperatableStateMachine.add('Say_Place_object', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'genPoseArm'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:102 y:572, x:889 y:107 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.Pose1 = "PostGripPose" _state_machine.userdata.Pose2 = "IdlePose" _state_machine.userdata.actionList = [["Find", "bottle"], ["move", "kitchen"]] _state_machine.userdata.titre = "test" _state_machine.userdata.relative = False _state_machine.userdata.pitch = -0.8 _state_machine.userdata.Action1 = ["move", "counter"] _state_machine.userdata.Action2 = ["move", "table"] _state_machine.userdata.pose = "Dining room" _state_machine.userdata.say1 = "say one" _state_machine.userdata.say2 = "say two" _state_machine.userdata.index = -1 _state_machine.userdata.name = "person" _state_machine.userdata.name2 = "apple" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:28 y:104 OperatableStateMachine.add('set_2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'list'}, autonomy={'done': Autonomy.Off}) # x:190 y:78 OperatableStateMachine.add('calc', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'gen'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity_list', 'output_value': 'entity'}) # x:351 y:116 OperatableStateMachine.add('gen', GenPointedPoints(step=0.3, qty=5), transitions={'done': 'loop', 'not_pointing': 'say 1', 'failed': 'say2'}, autonomy={'done': Autonomy.Off, 'not_pointing': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'entity': 'entity', 'positionList': 'positionList'}) # x:218 y:246 OperatableStateMachine.add('loop', ForLoop(repeat=5), transitions={'do': 'flex calc', 'end': 'say 5'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:493 y:35 OperatableStateMachine.add('say 1', SaraSay(sentence="Point at something", input_keys=[], emotion=0, block=True), transitions={'done': 'list'}, autonomy={'done': Autonomy.Off}) # x:533 y:125 OperatableStateMachine.add('say2', SaraSay(sentence="Failed to generate points", input_keys=[], emotion=0, block=True), transitions={'done': 'list'}, autonomy={'done': Autonomy.Off}) # x:377 y:255 OperatableStateMachine.add('flex calc', FlexibleCalculationState(calculation=lambda x: x[0][x[1]], input_keys=["positionList","index"]), transitions={'done': 'look'}, autonomy={'done': Autonomy.Off}, remapping={'positionList': 'positionList', 'index': 'index', 'output_value': 'point'}) # x:554 y:251 OperatableStateMachine.add('look', LookAtPos(), transitions={'failed': 'say 3', 'done': 'list entities'}, autonomy={'failed': Autonomy.Off, 'done': Autonomy.Off}, remapping={'pos': 'point'}) # x:403 y:354 OperatableStateMachine.add('say 3', SaraSay(sentence="I can not look at this point", input_keys=[], emotion=0, block=True), transitions={'done': 'loop'}, autonomy={'done': Autonomy.Off}) # x:116 y:405 OperatableStateMachine.add('say 5', SaraSay(sentence="I didn't see anything", input_keys=[], emotion=0, block=True), transitions={'done': 'set'}, autonomy={'done': Autonomy.Off}) # x:150 y:493 OperatableStateMachine.add('set', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:30 y:40 OperatableStateMachine.add('list', list_entities_by_name(frontality_level=0.5, distance_max=10), transitions={'found': 'calc', 'none_found': 'list'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'name', 'entity_list': 'entity_list', 'number': 'number'}) # x:698 y:310 OperatableStateMachine.add('list entities', list_entities_near_point(radius=2.0), transitions={'found': 'say 4', 'none_found': 'wait'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'name2', 'position': 'point', 'entity_list': 'found_entity_list', 'number': 'number'}) # x:714 y:407 OperatableStateMachine.add('say 4', SaraSay(sentence="I see something.", input_keys=[], emotion=0, block=True), transitions={'done': 'set'}, autonomy={'done': Autonomy.Off}) # x:569 y:346 OperatableStateMachine.add('wait', WaitState(wait_time=3), transitions={'done': 'loop'}, autonomy={'done': Autonomy.Off}) return _state_machine