def create(self): # x:942 y:70, x:835 y:318 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.JV = [ 5.034934007384111, 4.749860722186541, 2.0863536874817012, 3.548160146665105, -4.830528404237258, -3.8607153900995290 ] _state_machine.userdata.JN = [ "m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3", "m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6" ] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:142 y:64 OperatableStateMachine.add( 'Get_Values', GetJointValuesState( joints=[ "m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3", "m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6" ], topic_name="/m1n6s200_driver/joint_states"), transitions={'retrieved': 'Log_Values'}, autonomy={'retrieved': Autonomy.Off}, remapping={'joint_values': 'joint_values'}) # x:394 y:102 OperatableStateMachine.add('Log_Values', LogKeyState( text='%d', severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_values'}) return _state_machine
def create(self): # x:1534 y:100, x:62 y:356 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], output_keys=['ARMidMAIN', 'BothArmsFull', 'BothArmsEMPTY']) _state_machine.userdata.arm_idR = 'right_arm' _state_machine.userdata.arm_idL = 'left_arm' _state_machine.userdata.ARMidMAIN = '' _state_machine.userdata.BothArmsFull = '' _state_machine.userdata.Ja_value = 'Ja' _state_machine.userdata.Nee_value = 'Nee' _state_machine.userdata.BothArmsEMPTY = '' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:70 y:32 OperatableStateMachine.add('CheckGripperRight', CheckGripperattached(), transitions={ 'True': 'CheckGripperLeft', 'False': 'CheckGripperLeft_2', 'invalid_arm_id': 'failed' }, autonomy={ 'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off }, remapping={'arm_id': 'arm_idR'}) # x:69 y:94 OperatableStateMachine.add('CheckGripperLeft', CheckGripperattached(), transitions={ 'True': 'SetBothArmFull', 'False': 'ResetBothArmFull', 'invalid_arm_id': 'failed' }, autonomy={ 'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off }, remapping={'arm_id': 'arm_idL'}) # x:702 y:22 OperatableStateMachine.add('SetArmRight', ReplaceState(), transitions={'done': 'LogRighterarm'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'arm_idR', 'result': 'ARMidMAIN' }) # x:702 y:85 OperatableStateMachine.add('SetArmLeft', ReplaceState(), transitions={'done': 'LogLeftArm'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'arm_idL', 'result': 'ARMidMAIN' }) # x:528 y:142 OperatableStateMachine.add('SetBothArmFull', ReplaceState(), transitions={'done': 'BothArmsFullLOG'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'Ja_value', 'result': 'BothArmsFull' }) # x:888 y:19 OperatableStateMachine.add( 'LogRighterarm', LogState(text="The Right arm has nothing attached to it", severity=Logger.REPORT_HINT), transitions={'done': 'Arm_id is:'}, autonomy={'done': Autonomy.Off}) # x:887 y:77 OperatableStateMachine.add( 'LogLeftArm', LogState(text="The Left arm has nothing attached to it", severity=Logger.REPORT_HINT), transitions={'done': 'Arm_id is:'}, autonomy={'done': Autonomy.Off}) # x:530 y:84 OperatableStateMachine.add('ResetBothArmFull', ReplaceState(), transitions={'done': 'SetArmLeft'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'Nee_value', 'result': 'BothArmsFull' }) # x:530 y:26 OperatableStateMachine.add('ResetBothArmFull_2', ReplaceState(), transitions={'done': 'SetArmRight'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'Nee_value', 'result': 'BothArmsFull' }) # x:1149 y:41 OperatableStateMachine.add('Arm_id is:', LogKeyState( text="The chosen arm_id is:{}", severity=Logger.REPORT_HINT), transitions={'done': 'BothArmsFullLOG'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'ARMidMAIN'}) # x:1365 y:90 OperatableStateMachine.add('BothArmsFullLOG', LogKeyState( text='Zijn beide armen vol: {}', severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'BothArmsFull'}) # x:300 y:28 OperatableStateMachine.add('CheckGripperLeft_2', CheckGripperattached(), transitions={ 'True': 'ResetBothArmFull_2', 'False': 'SetBothArmEMPTY', 'invalid_arm_id': 'failed' }, autonomy={ 'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off }, remapping={'arm_id': 'arm_idL'}) # x:319 y:412 OperatableStateMachine.add( 'SetBothArmEMPTY', ReplaceState(), transitions={'done': 'ResetBothArmFull_2'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'Ja_value', 'result': 'BothArmsEMPTY' }) return _state_machine
def create(self): # x:736 y:384, x:79 y:422 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['targetPoint']) _state_machine.userdata.targetPoint = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:86 y:32 OperatableStateMachine.add( 'gripper', SetGripperState(width=0, effort=1), transitions={ 'object': 'direction', 'no_object': 'direction' }, autonomy={ 'object': Autonomy.Off, 'no_object': Autonomy.Off }, remapping={'object_size': 'object_size'}) # x:503 y:276 OperatableStateMachine.add('move', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'pose'}) # x:114 y:312 OperatableStateMachine.add( 'invert', CalculationState(calculation=lambda x: -x), transitions={'done': 'point'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'pitch', 'output_value': 'pitch' }) # x:269 y:267 OperatableStateMachine.add('point', point_at_gen_pose(offsetx=0.16, offsety=-0.2, offsetz=1.2, l=0.75), transitions={'pose': 'move'}, autonomy={'pose': Autonomy.Off}, remapping={ 'yaw': 'yaw', 'pitch': 'pitch', 'pose': 'pose' }) # x:99 y:199 OperatableStateMachine.add('print pitch', LogKeyState( text="pitch = {}", severity=Logger.REPORT_HINT), transitions={'done': 'invert'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'pitch'}) # x:235 y:103 OperatableStateMachine.add( 'direction', Get_direction_to_point( frame_origin="base_link", frame_reference="right_upper_arm_upper_link"), transitions={ 'done': 'print pitch', 'fail': 'failed' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'targetPoint': 'targetPoint', 'yaw': 'yaw', 'pitch': 'pitch' }) return _state_machine
def create(self): # x:1066 y:202, x:512 y:391 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.action_topic = "/move_group" _state_machine.userdata.move_group_arm = "arm" _state_machine.userdata.config_name = "Home" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:41 y:57 OperatableStateMachine.add( 'SetupMoveItProxy', SetupProxyMoveItClientState( robot_description="/robot_description", robot_description_semantic=None, move_group_capabilities="/move_group", action_type_and_topics=None, enter_wait_duration=0.5), transitions={ 'connected': 'GetHome', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'connected': Autonomy.Off, 'topics_unavailable': Autonomy.High, 'param_error': Autonomy.High }, remapping={ 'robot_name': 'robot_name', 'move_groups': 'move_groups' }) # x:469 y:185 OperatableStateMachine.add('PrintConfig', LogKeyState( text=" Config {}", severity=Logger.REPORT_HINT), transitions={'done': 'MoveHome'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_names'}) # x:698 y:110 OperatableStateMachine.add('MoveHome', UserDataToMoveActionState( joint_tolerance=0.05, constraint_weight=1.0, allowed_planning_time=2.0, wait_duration=2.0, timeout=30.0, action_topic=None), transitions={ 'reached': 'MoveVertical', 'param_error': 'failed', 'planning_failed': 'failed', 'control_failed': 'failed', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Full, 'param_error': Autonomy.Full, 'planning_failed': Autonomy.Full, 'control_failed': Autonomy.Full, 'failed': Autonomy.Off }, remapping={ 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_names': 'joint_names', 'joint_values': 'joint_values', 'status_text': 'status_text' }) # x:453 y:563 OperatableStateMachine.add( 'MoveVertical', JointValuesToMoveActionState(joint_names=[ 'm1n6s200_joint_1', 'm1n6s200_joint_2', 'm1n6s200_joint_3', 'm1n6s200_joint_4', 'm1n6s200_joint_5', 'm1n6s200_joint_6' ], joint_values=[ 0.0, 3.14, 3.14, 0.0, 0.0, 0.0 ], move_group="arm", action_topic="/move_group", joint_tolerance=0.05, constraint_weight=1.0, allowed_planning_time=2.0, wait_duration=2.0, timeout=10.0), transitions={ 'reached': 'GetHome', 'param_error': 'failed', 'planning_failed': 'failed', 'control_failed': 'failed', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Low, 'param_error': Autonomy.High, 'planning_failed': Autonomy.High, 'control_failed': Autonomy.High, 'failed': Autonomy.High }, remapping={ 'move_group': 'move_group', 'action_topic': 'action_topic', 'status_text': 'status_text', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:87 y:259 OperatableStateMachine.add('GetHome', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'PrintConfig', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.High, 'param_error': Autonomy.High }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group_arm', 'config_name': 'config_name', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) return _state_machine
def create(self): # x:712 y:612, x:820 y:97 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.name = "person" _state_machine.userdata.distance = 1.05 _state_machine.userdata.taxi = "taxi" _state_machine.userdata.umbrella = "umbrella" # 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_get_closer_0 = ConcurrencyContainer( outcomes=['finished'], input_keys=['ID'], conditions=[('finished', [('follow', 'failed')]), ('finished', [('wait 3', 'done')])]) with _sm_get_closer_0: # x:46 y:131 OperatableStateMachine.add('follow', SaraFollow(distance=1.15, ReplanPeriod=0.5), transitions={'failed': 'finished'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:223 y:134 OperatableStateMachine.add('wait 3', WaitState(wait_time=4), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:571 y:592 _sm_scan_1 = OperatableStateMachine(outcomes=['finished']) with _sm_scan_1: # x:42 y:66 OperatableStateMachine.add( 'Looking', SaraSay( sentence= "I am trying to find who wants to leave. Please raise your hand.", input_keys=[], emotion=0, block=False), transitions={'done': 'center'}, autonomy={'done': Autonomy.Off}) # x:394 y:262 OperatableStateMachine.add('right', SaraSetHeadAngle(pitch=0.1, yaw=-0.3), 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:34 y:589 OperatableStateMachine.add('w1', WaitState(wait_time=3), transitions={'done': 'center3'}, autonomy={'done': Autonomy.Off}) # x:413 y:159 OperatableStateMachine.add('w2', WaitState(wait_time=3), transitions={'done': 'right'}, autonomy={'done': Autonomy.Off}) # x:415 y:370 OperatableStateMachine.add('w3', WaitState(wait_time=3), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:39 y:371 OperatableStateMachine.add('w4', WaitState(wait_time=3), transitions={'done': 'left'}, autonomy={'done': Autonomy.Off}) # x:16 y:475 OperatableStateMachine.add('left', SaraSetHeadAngle(pitch=0.1, yaw=0.3), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:215 y:156 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:213 y:579 OperatableStateMachine.add('center3', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'wait'}, autonomy={'done': Autonomy.Off}) # x:393 y:576 OperatableStateMachine.add('wait', WaitState(wait_time=4), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:32 y:494, x:627 y:411 _sm_filtregender_2 = OperatableStateMachine( outcomes=['none_found', 'found person'], input_keys=['name'], output_keys=['pronoun', 'person']) with _sm_filtregender_2: # x:109 y:51 OperatableStateMachine.add('List', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'FiltreWave', 'none_found': 'none_found' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'name', 'entity_list': 'entity_list', 'number': 'number' }) # x:180 y:301 OperatableStateMachine.add( 'FiltreExitingwomen', Filter(filter=lambda x: x.face.gender == "female"), transitions={'done': 'no female?'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_list': 'persons', 'output_list': 'female' }) # x:174 y:386 OperatableStateMachine.add( 'no female?', CheckConditionState(predicate=lambda x: len(x) > 0), transitions={ 'true': 'set pronoun female', 'false': 'set pronoun male' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'female'}) # x:374 y:312 OperatableStateMachine.add( 'set pronoun female', SetKey(Value="miss"), transitions={'done': 'get first female'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'pronoun'}) # x:181 y:476 OperatableStateMachine.add('set pronoun male', SetKey(Value=""), transitions={'done': 'get first male'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'pronoun'}) # x:599 y:290 OperatableStateMachine.add( 'get first female', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'found person'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'female', 'output_value': 'person' }) # x:381 y:408 OperatableStateMachine.add( 'get first male', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'found person'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'persons', 'output_value': 'person' }) # x:151 y:135 OperatableStateMachine.add( 'FiltreWave', Filter(filter=lambda x: x.pose.right_arm_up or x.pose. left_arm_up), transitions={'done': 'if more than 0'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_list': 'entity_list', 'output_list': 'persons' }) # x:157 y:217 OperatableStateMachine.add( 'if more than 0', CheckConditionState(predicate=lambda x: len(x) > 0), transitions={ 'true': 'FiltreExitingwomen', 'false': 'none_found' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'persons'}) # x:772 y:208, x:360 y:516 _sm_confirm_3 = OperatableStateMachine( outcomes=['false', 'done'], input_keys=['Person', 'pronoun']) with _sm_confirm_3: # x:66 y:83 OperatableStateMachine.add( 'Confirm', SaraSay(sentence=lambda x: "would you like to leave, " + x[0] + "?", input_keys=["pronoun"], emotion=0, block=True), transitions={'done': 'GetSpeech'}, autonomy={'done': Autonomy.Off}, remapping={'pronoun': 'pronoun'}) # x:82 y:504 OperatableStateMachine.add('GetID', SetRosParam(ParamName="OpeID"), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'ID'}) # x:67 y:298 OperatableStateMachine.add('if yes', RegexTester(regex=".*((yes)|(I do)).*"), transitions={ 'true': 'get id', 'false': 'test no' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'text', 'result': 'result' }) # x:84 y:409 OperatableStateMachine.add('get id', GetAttribute(attributes=["ID"]), transitions={'done': 'GetID'}, autonomy={'done': Autonomy.Off}, remapping={ 'object': 'Person', 'ID': 'ID' }) # x:87 y:188 OperatableStateMachine.add('GetSpeech', GetSpeech(watchdog=10), transitions={ 'done': 'if yes', 'nothing': 'sayno', 'fail': 'if yes' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'text'}) # x:259 y:193 OperatableStateMachine.add( 'sayno', SaraSay( sentence="Sorry, did you say that you wanted to leave?", input_keys=[], emotion=0, block=True), transitions={'done': 'getno'}, autonomy={'done': Autonomy.Off}) # x:439 y:179 OperatableStateMachine.add('getno', GetSpeech(watchdog=10), transitions={ 'done': 'getNO', 'nothing': 'false', 'fail': 'false' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'text'}) # x:603 y:337 OperatableStateMachine.add('getNO', RegexTester(regex=".*((yes)|(I do)).*"), transitions={ 'true': 'get id', 'false': 'false' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'text', 'result': 'result' }) # x:246 y:298 OperatableStateMachine.add('test no', RegexTester(regex=".*((no)|(not)).*"), transitions={ 'true': 'false', 'false': 'sayno' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'text', 'result': 'result' }) # x:228 y:419 _sm_find_raising_arm_4 = OperatableStateMachine( outcomes=['finished'], output_keys=['umbrella']) with _sm_find_raising_arm_4: # x:33 y:48 OperatableStateMachine.add('setperson', SetKey(Value="person"), transitions={'done': 'gettaxiperson'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'person'}) # x:183 y:120 OperatableStateMachine.add('gettaxiperson', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'taxi', 'none_found': 'gettaxiperson' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'person', 'entity_list': 'entity_list', 'number': 'number' }) # x:408 y:270 OperatableStateMachine.add( 'if', CheckConditionState(predicate=lambda x: len(x) > 0), transitions={ 'true': 'get first', 'false': 'gettaxiperson' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'output_list'}) # x:344 y:184 OperatableStateMachine.add( 'taxi', Filter(filter=lambda x: x.pose.right_arm_up or x.pose. left_arm_up), transitions={'done': 'if'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_list': 'entity_list', 'output_list': 'output_list' }) # x:405 y:359 OperatableStateMachine.add( 'get first', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'output_list', 'output_value': 'umbrella' }) # x:30 y:365 _sm_rotation_5 = OperatableStateMachine(outcomes=['end']) with _sm_rotation_5: # x:51 y:38 OperatableStateMachine.add('Set 180 degres', SetKey(Value=3.1416), transitions={'done': 'Look Center'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:613 y:470 OperatableStateMachine.add( 'action_turn', self.use_behavior( sara_flexbe_behaviors__action_turnSM, 'GetTaxi/Find umbrella/gettaxihuman/Rotation/action_turn'), transitions={ 'finished': 'Look Right', 'failed': 'Look Right' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:421 y:54 OperatableStateMachine.add('Look Right', SaraSetHeadAngle(pitch=0, yaw=-1.5), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:265 y:56 OperatableStateMachine.add('w1', WaitState(wait_time=4), transitions={'done': 'Look Right'}, autonomy={'done': Autonomy.Off}) # x:630 y:56 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'center'}, autonomy={'done': Autonomy.Off}) # x:250 y:177 OperatableStateMachine.add('Look Center', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:618 y:304 OperatableStateMachine.add('Look Left 2', SaraSetHeadAngle(pitch=0, yaw=1.5), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:612 y:138 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:635 y:214 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'Look Left 2'}, autonomy={'done': Autonomy.Off}) # x:636 y:394 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}) # x:30 y:365 _sm_find_entity_6 = OperatableStateMachine(outcomes=['found'], input_keys=['className'], output_keys=['entity']) with _sm_find_entity_6: # x:181 y:178 OperatableStateMachine.add('find_entity', list_entities_by_name( frontality_level=0.5, distance_max=4), transitions={ 'found': 'Get Entity', 'none_found': 'find_entity' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'className', 'entity_list': 'entity_list', 'number': 'number' }) # x:454 y:178 OperatableStateMachine.add( 'Get Entity', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'entity' }) # x:194 y:40 OperatableStateMachine.add('WaitState', WaitState(wait_time=1), transitions={'done': 'find_entity'}, autonomy={'done': Autonomy.Off}) # x:34 y:496, x:130 y:365, x:475 y:291, x:330 y:365, x:430 y:365 _sm_gettaxihuman_7 = ConcurrencyContainer( outcomes=['found', 'not_found'], input_keys=['umbrella'], output_keys=['umbrella'], conditions=[('not_found', [('Rotation', 'end')]), ('found', [('Find Entity', 'found')]), ('found', [('FInd raising arm', 'finished')])]) with _sm_gettaxihuman_7: # x:127 y:67 OperatableStateMachine.add('Find Entity', _sm_find_entity_6, transitions={'found': 'found'}, autonomy={'found': Autonomy.Inherit}, remapping={ 'className': 'umbrella', 'entity': 'umbrella' }) # x:129 y:180 OperatableStateMachine.add('Rotation', _sm_rotation_5, transitions={'end': 'not_found'}, autonomy={'end': Autonomy.Inherit}) # x:377 y:127 OperatableStateMachine.add('FInd raising arm', _sm_find_raising_arm_4, transitions={'finished': 'found'}, autonomy={'finished': Autonomy.Inherit}, remapping={'umbrella': 'umbrella'}) # x:73 y:441, x:586 y:51 _sm_find_umbrella_8 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['umbrella'], output_keys=['umbrella']) with _sm_find_umbrella_8: # x:67 y:42 OperatableStateMachine.add('Look Center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'gettaxihuman'}, autonomy={'done': Autonomy.Off}) # x:278 y:138 OperatableStateMachine.add('Look Center Not Found', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:58 y:326 OperatableStateMachine.add('Log Entity', LogKeyState( text="Found entity: {}", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'umbrella'}) # x:63 y:126 OperatableStateMachine.add('gettaxihuman', _sm_gettaxihuman_7, transitions={ 'found': 'WaitState', 'not_found': 'Look Center Not Found' }, autonomy={ 'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit }, remapping={'umbrella': 'umbrella'}) # x:67 y:222 OperatableStateMachine.add('WaitState', WaitState(wait_time=1), transitions={'done': 'Log Entity'}, autonomy={'done': Autonomy.Off}) # x:83 y:284 _sm_lift_head_9 = OperatableStateMachine(outcomes=['finished']) with _sm_lift_head_9: # x:53 y:42 OperatableStateMachine.add('lift head', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'wait 2'}, autonomy={'done': Autonomy.Off}) # x:57 y:152 OperatableStateMachine.add('wait 2', WaitState(wait_time=3), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:30 y:365 _sm_get_closer_10 = OperatableStateMachine(outcomes=['finished'], input_keys=['person']) with _sm_get_closer_10: # x:151 y:63 OperatableStateMachine.add('get ID', GetAttribute(attributes=["ID"]), transitions={'done': 'Get closer'}, autonomy={'done': Autonomy.Off}, remapping={ 'object': 'person', 'ID': 'ID' }) # x:135 y:194 OperatableStateMachine.add('Get closer', _sm_get_closer_0, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:607 y:98, x:558 y:330 _sm_get_gender_11 = OperatableStateMachine( outcomes=['none_found', 'done'], input_keys=['name', 'distance'], output_keys=['person', 'pronoun']) with _sm_get_gender_11: # x:30 y:40 OperatableStateMachine.add('set head', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'Scan'}, autonomy={'done': Autonomy.Off}) # x:441 y:109 OperatableStateMachine.add('FiltreGender', _sm_filtregender_2, transitions={ 'none_found': 'none_found', 'found person': 'done' }, autonomy={ 'none_found': Autonomy.Inherit, 'found person': Autonomy.Inherit }, remapping={ 'name': 'name', 'pronoun': 'pronoun', 'person': 'person' }) # x:211 y:93 OperatableStateMachine.add( 'Scan', _sm_scan_1, transitions={'finished': 'FiltreGender'}, autonomy={'finished': Autonomy.Inherit}) # x:30 y:373, x:130 y:373 _sm_nevermind_12 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['poseOrigin']) with _sm_nevermind_12: # x:30 y:40 OperatableStateMachine.add('say ok', SaraSay(sentence="I will try again", input_keys=[], emotion=0, block=False), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:26 y:192 OperatableStateMachine.add( 'Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'nevermind/Action_Move'), transitions={ 'finished': 'done', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'poseOrigin'}) # x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458 _sm_confirm_13 = ConcurrencyContainer( outcomes=['false', 'done'], input_keys=['person', 'pronoun'], conditions=[('false', [('Confirm', 'false')]), ('done', [('Confirm', 'done')]), ('false', [('LookAtClosest', 'failed')])]) with _sm_confirm_13: # x:95 y:163 OperatableStateMachine.add('Confirm', _sm_confirm_3, transitions={ 'false': 'false', 'done': 'done' }, autonomy={ 'false': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'Person': 'person', 'pronoun': 'pronoun' }) # x:309 y:162 OperatableStateMachine.add( 'LookAtClosest', self.use_behavior(sara_flexbe_behaviors__LookAtClosestSM, 'confirm/LookAtClosest'), transitions={'failed': 'false'}, autonomy={'failed': Autonomy.Inherit}) # x:65 y:581, x:688 y:449 _sm_gettaxi_14 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['distance', 'taxi', 'umbrella']) with _sm_gettaxi_14: # x:103 y:28 OperatableStateMachine.add( 'Action_Move_to taxi', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'GetTaxi/Action_Move_to taxi'), transitions={ 'finished': 'Lift head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'taxi'}) # x:220 y:546 OperatableStateMachine.add( 'Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'GetTaxi/Action_Move'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'pose_out'}) # x:80 y:475 OperatableStateMachine.add('NotTooClose', Get_Reacheable_Waypoint(), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={ 'pose_in': 'position', 'distance': 'distance', 'pose_out': 'pose_out' }) # x:81 y:400 OperatableStateMachine.add('GEtUmbrellaPosition', GetAttribute(attributes=["position"]), transitions={'done': 'NotTooClose'}, autonomy={'done': Autonomy.Off}, remapping={ 'object': 'umbrella', 'position': 'position' }) # x:108 y:131 OperatableStateMachine.add( 'Lift head', _sm_lift_head_9, transitions={'finished': 'Find umbrella'}, autonomy={'finished': Autonomy.Inherit}) # x:150 y:315 OperatableStateMachine.add( 'log', LogKeyState(text="found umbrella: {}", severity=Logger.REPORT_HINT), transitions={'done': 'GEtUmbrellaPosition'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'umbrella'}) # x:267 y:312 OperatableStateMachine.add( 'say see taxy', SaraSay( sentence= "I see the taxi. Over there. The person with the umbrella.", input_keys=[], emotion=0, block=True), transitions={'done': 'log'}, autonomy={'done': Autonomy.Off}) # x:111 y:204 OperatableStateMachine.add('Find umbrella', _sm_find_umbrella_8, transitions={ 'finished': 'say see taxy', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'umbrella': 'umbrella'}) with _state_machine: # x:46 y:16 OperatableStateMachine.add('GetOrigin', Get_Robot_Pose(), transitions={'done': 'Init_Sequence'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'poseOrigin'}) # x:63 y:574 OperatableStateMachine.add('GetTaxi', _sm_gettaxi_14, transitions={ 'finished': 'say succeed', 'failed': 'say fail' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'distance': 'distance', 'taxi': 'taxi', 'umbrella': 'umbrella' }) # x:103 y:369 OperatableStateMachine.add('confirm', _sm_confirm_13, transitions={ 'false': 'nevermind', 'done': 'say taxi' }, autonomy={ 'false': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'person': 'person', 'pronoun': 'pronoun' }) # x:289 y:142 OperatableStateMachine.add( 'say nobody', SaraSay(sentence="I don't see any raised hand.", input_keys=[], emotion=0, block=True), transitions={'done': 'Get Gender'}, autonomy={'done': Autonomy.Off}) # x:571 y:104 OperatableStateMachine.add( 'say fail', SaraSay(sentence="I failed this scenario. Sorry.", input_keys=[], emotion=3, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:245 y:595 OperatableStateMachine.add('say succeed', SaraSay( sentence="here is the taxy driver.", input_keys=[], emotion=5, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:103 y:483 OperatableStateMachine.add( 'say taxi', SaraSay(sentence="Ok, follow me to the taxi then.", input_keys=[], emotion=0, block=True), transitions={'done': 'GetTaxi'}, autonomy={'done': Autonomy.Off}) # x:49 y:101 OperatableStateMachine.add( 'Init_Sequence', self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'Init_Sequence'), transitions={ 'finished': 'Get Gender', 'failed': 'say fail' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:17 y:288 OperatableStateMachine.add('nevermind', _sm_nevermind_12, transitions={ 'done': 'Get Gender', 'failed': 'say fail' }, autonomy={ 'done': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'poseOrigin': 'poseOrigin'}) # x:30 y:192 OperatableStateMachine.add('Get Gender', _sm_get_gender_11, transitions={ 'none_found': 'say nobody', 'done': 'get closer' }, autonomy={ 'none_found': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'name': 'name', 'distance': 'distance', 'person': 'person', 'pronoun': 'pronoun' }) # x:259 y:310 OperatableStateMachine.add('get closer', _sm_get_closer_10, transitions={'finished': 'confirm'}, autonomy={'finished': Autonomy.Inherit}, remapping={'person': 'person'}) return _state_machine
def create(self): # x:808 y:575, x:78 y:552, x:477 y:566 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = [ "Ask", "How are you today?", "Answer" ] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:675 y:219, x:771 y:50 _sm_confirm_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['answer']) with _sm_confirm_0: # x:70 y:60 OperatableStateMachine.add( 'Heard', 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:501 y:98 OperatableStateMachine.add( 'check yes', CheckConditionState( predicate=lambda x: "yes" in x and not "no" in x), transitions={ 'true': 'finished', 'false': 'failed' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'words'}) # x:259 y:92 OperatableStateMachine.add('get speech', GetSpeech(watchdog=5), transitions={ 'done': 'check yes', 'nothing': 'failed', 'fail': 'finished' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) with _state_machine: # x:43 y:162 OperatableStateMachine.add( 'SetPerson', SetKey(Value="person"), transitions={'done': 'Action_findPerson'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'personKey'}) # x:194 y:99 OperatableStateMachine.add( 'trouveLaQuestion', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'AskTheQuestion'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Action', 'output_value': 'question' }) # x:545 y:161 OperatableStateMachine.add('GetTheResponse', GetSpeech(watchdog=7), transitions={ 'done': 'Confirm', 'nothing': 'looping', 'fail': 'looping' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'response'}) # x:372 y:262 OperatableStateMachine.add( 'NotUnderstand', SaraSay(sentence="Soory, I did not understand.", input_keys=[], emotion=1, block=True), transitions={'done': 'AskTheQuestion'}, autonomy={'done': Autonomy.Off}) # x:23 y:285 OperatableStateMachine.add('Action_findPerson', self.use_behavior( Action_findPersonSM, 'Action_findPerson'), transitions={ 'done': 'fisrtSentence', 'pas_done': 'NoPerson' }, autonomy={ 'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit }, remapping={ 'className': 'personKey', 'entity': 'entity' }) # x:224 y:301 OperatableStateMachine.add( 'NoPerson', SaraSay(sentence="I did not find any person. ", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:169 y:194 OperatableStateMachine.add( 'fisrtSentence', SaraSay(sentence="Hello, I have a question for you.", input_keys=[], emotion=1, block=True), transitions={'done': 'trouveLaQuestion'}, autonomy={'done': Autonomy.Off}) # x:539 y:332 OperatableStateMachine.add('looping', ForLoop(repeat=2), transitions={ 'do': 'NotUnderstand', 'end': 'saraSorry' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:540 y:473 OperatableStateMachine.add( 'saraSorry', SaraSay(sentence="Sorry, I can't understand your response.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause2'}, autonomy={'done': Autonomy.Off}) # x:776 y:209 OperatableStateMachine.add( 'getRosparmName', CalculationState(calculation=lambda x: x[2]), transitions={'done': 'SetRosParamKey'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Action', 'output_value': 'RosParamName' }) # x:778 y:301 OperatableStateMachine.add('SetRosParamKey', SetRosParamKey(), transitions={'done': 'log'}, autonomy={'done': Autonomy.Off}, remapping={ 'Value': 'response', 'ParamName': 'RosParamName' }) # x:787 y:482 OperatableStateMachine.add( 'thank you', SaraSay(sentence="Thank you for your answer.", input_keys=[], emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:789 y:390 OperatableStateMachine.add('log', LogKeyState( text="{}", severity=Logger.REPORT_HINT), transitions={'done': 'thank you'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'response'}) # x:221 y:384 OperatableStateMachine.add( 'cause1', SetKey(Value="I didn't find any person"), transitions={'done': 'set cause failure'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:326 y:503 OperatableStateMachine.add( 'cause2', SetKey(Value="I did not understand the answer"), transitions={'done': 'set cause failure'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:159 y:468 OperatableStateMachine.add( 'set cause failure', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:769 y:88 OperatableStateMachine.add('Confirm', _sm_confirm_0, transitions={ 'finished': 'getRosparmName', 'failed': 'AskTheQuestion' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'answer': 'response'}) # x:367 y:117 OperatableStateMachine.add('AskTheQuestion', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'GetTheResponse'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:72 y:443, x:514 y:143 _state_machine = OperatableStateMachine(outcomes=['done', 'pas_done'], input_keys=['className'], output_keys=['entity']) _state_machine.userdata.className = "person" _state_machine.userdata.entity = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:706 y:647 _sm_rotation_0 = OperatableStateMachine(outcomes=['end']) with _sm_rotation_0: # x:51 y:38 OperatableStateMachine.add('Set 180 degres', SetKey(Value=3.1416), transitions={'done': 'Look Center'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:613 y:470 OperatableStateMachine.add('action_turn', self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'Container/Rotation/action_turn'), transitions={'finished': 'Look Right', 'failed': 'Look Right'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'rotation': 'rotation'}) # x:421 y:54 OperatableStateMachine.add('Look Right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:265 y:56 OperatableStateMachine.add('w1', WaitState(wait_time=4), transitions={'done': 'Look Right'}, autonomy={'done': Autonomy.Off}) # x:630 y:56 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'center'}, autonomy={'done': Autonomy.Off}) # x:250 y:177 OperatableStateMachine.add('Look Center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:618 y:304 OperatableStateMachine.add('Look Left 2', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:612 y:138 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:635 y:214 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'Look Left 2'}, autonomy={'done': Autonomy.Off}) # x:636 y:394 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}) # x:683 y:188 _sm_find_entity_1 = OperatableStateMachine(outcomes=['found'], input_keys=['className'], output_keys=['entity']) with _sm_find_entity_1: # x:181 y:178 OperatableStateMachine.add('find_entity', list_entities_by_name(frontality_level=0.5, distance_max=4), transitions={'found': 'Get Entity', 'none_found': 'find_entity'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'className', 'entity_list': 'entity_list', 'number': 'number'}) # x:454 y:178 OperatableStateMachine.add('Get Entity', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity_list', 'output_value': 'entity'}) # x:194 y:40 OperatableStateMachine.add('WaitState', WaitState(wait_time=1), transitions={'done': 'find_entity'}, autonomy={'done': Autonomy.Off}) # x:372 y:27, x:370 y:220, x:368 y:100, x:330 y:458 _sm_container_2 = ConcurrencyContainer(outcomes=['found', 'not_found'], input_keys=['className'], output_keys=['entity'], conditions=[ ('not_found', [('Rotation', 'end')]), ('found', [('Find Entity', 'found')]) ]) with _sm_container_2: # x:131 y:44 OperatableStateMachine.add('Find Entity', _sm_find_entity_1, transitions={'found': 'found'}, autonomy={'found': Autonomy.Inherit}, remapping={'className': 'className', 'entity': 'entity'}) # x:129 y:197 OperatableStateMachine.add('Rotation', _sm_rotation_0, transitions={'end': 'not_found'}, autonomy={'end': Autonomy.Inherit}) with _state_machine: # x:67 y:42 OperatableStateMachine.add('Look Center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'Container'}, autonomy={'done': Autonomy.Off}) # x:278 y:138 OperatableStateMachine.add('Look Center Not Found', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'pas_done'}, autonomy={'done': Autonomy.Off}) # x:58 y:326 OperatableStateMachine.add('Log Entity', LogKeyState(text="Found entity: {}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'entity'}) # x:63 y:126 OperatableStateMachine.add('Container', _sm_container_2, transitions={'found': 'WaitState', 'not_found': 'Look Center Not Found'}, autonomy={'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit}, remapping={'className': 'className', 'entity': 'entity'}) # x:67 y:222 OperatableStateMachine.add('WaitState', WaitState(wait_time=1), transitions={'done': 'Log Entity'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:317 y:570, x:757 y:138, x:589 y:443 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail']) # 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_turntofindanotherpersononce_0 = OperatableStateMachine( outcomes=['end', 'finished', 'failed']) with _sm_turntofindanotherpersononce_0: # x:32 y:135 OperatableStateMachine.add( 'sayNeverMind', SaraSay(sentence="OK. Never mind", input_keys=[], emotion=1, block=True), transitions={'done': 'loopFindOtherPerson'}, autonomy={'done': Autonomy.Off}) # x:156 y:151 OperatableStateMachine.add('loopFindOtherPerson', ForLoop(repeat=1), transitions={ 'do': 'SetRotation', 'end': 'end' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index3'}) # x:30 y:40 OperatableStateMachine.add( 'action_turn', self.use_behavior(action_turnSM, 'turnToFindAnotherPersonOnce/action_turn'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:194 y:86 OperatableStateMachine.add('SetRotation', SetKey(Value=3.1416), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:196 y:458, x:495 y:340 _sm_listenandstorethequestion_1 = OperatableStateMachine( outcomes=['done', 'end'], output_keys=['operatorQuestion']) with _sm_listenandstorethequestion_1: # x:30 y:51 OperatableStateMachine.add( 'sayAskQuestion', SaraSay(sentence="Ask me your question please.", input_keys=[], emotion=1, block=True), transitions={'done': 'getQuestion'}, autonomy={'done': Autonomy.Off}) # x:50 y:194 OperatableStateMachine.add('getQuestion', GetSpeech(watchdog=7), transitions={ 'done': 'done', 'nothing': 'looping2', 'fail': 'looping2' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'operatorQuestion'}) # x:250 y:262 OperatableStateMachine.add('looping2', ForLoop(repeat=1), transitions={ 'do': 'sayRepeat2', 'end': 'end' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index2'}) # x:273 y:94 OperatableStateMachine.add( 'sayRepeat2', SaraSay(sentence="I didn't understand. Can you repeat please?", input_keys=[], emotion=1, block=True), transitions={'done': 'getQuestion'}, autonomy={'done': Autonomy.Off}) # x:137 y:546, x:475 y:361, x:494 y:244 _sm_ifquestionforsara_2 = OperatableStateMachine( outcomes=['true', 'false', 'end']) with _sm_ifquestionforsara_2: # x:30 y:40 OperatableStateMachine.add( 'AskIfQuestion', SaraSay(sentence="Do you have a question for me?", input_keys=[], emotion=1, block=True), transitions={'done': 'getResponse'}, autonomy={'done': Autonomy.Off}) # x:23 y:108 OperatableStateMachine.add('getResponse', GetSpeech(watchdog=7), transitions={ 'done': 'ifYes', 'nothing': 'looping', 'fail': 'looping' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'response'}) # x:261 y:216 OperatableStateMachine.add('looping', ForLoop(repeat=1), transitions={ 'do': 'sayRepeat', 'end': 'end' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:254 y:63 OperatableStateMachine.add( 'sayRepeat', SaraSay(sentence= "I didn't understand. Do you have a question for me?", input_keys=[], emotion=1, block=True), transitions={'done': 'getResponse'}, autonomy={'done': Autonomy.Off}) # x:73 y:394 OperatableStateMachine.add( 'ifYes', CheckConditionState(predicate=lambda x: "yes" in x or "sure" in x or "of course" in x), transitions={ 'true': 'true', 'false': 'false' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'response'}) with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'SetPersonClass', SetKey(Value="person"), transitions={'done': 'Action_findPerson'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'personClass'}) # x:20 y:117 OperatableStateMachine.add('Action_findPerson', self.use_behavior( Action_findPersonSM, 'Action_findPerson'), transitions={ 'done': 'IfQuestionForSARA', 'pas_done': 'noPerson' }, autonomy={ 'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit }, remapping={ 'className': 'personClass', 'entity': 'entity' }) # x:374 y:60 OperatableStateMachine.add( 'noPerson', SaraSay(sentence="I can't find any person here.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:332 y:298 OperatableStateMachine.add( 'NotUnderstandEnd', SaraSay(sentence="Sorry. I can't understand your answer.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause2'}, autonomy={'done': Autonomy.Off}) # x:39 y:384 OperatableStateMachine.add('SendToNLU', SaraNLUspr(), transitions={ 'understood': 'log answer', 'not_understood': 'CannotAnswer', 'fail': 'CannotAnswer' }, autonomy={ 'understood': Autonomy.Off, 'not_understood': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'sentence': 'operatorQuestion', 'answer': 'answer' }) # x:334 y:408 OperatableStateMachine.add( 'CannotAnswer', SaraSay(sentence="Sorry. I can't answer your question.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause3'}, autonomy={'done': Autonomy.Off}) # x:23 y:208 OperatableStateMachine.add('IfQuestionForSARA', _sm_ifquestionforsara_2, transitions={ 'true': 'ListenAndStoreTheQuestion', 'false': 'turnToFindAnotherPersonOnce', 'end': 'NotUnderstandEnd' }, autonomy={ 'true': Autonomy.Inherit, 'false': Autonomy.Inherit, 'end': Autonomy.Inherit }) # x:16 y:298 OperatableStateMachine.add( 'ListenAndStoreTheQuestion', _sm_listenandstorethequestion_1, transitions={ 'done': 'SendToNLU', 'end': 'NotUnderstandEnd' }, autonomy={ 'done': Autonomy.Inherit, 'end': Autonomy.Inherit }, remapping={'operatorQuestion': 'operatorQuestion'}) # x:299 y:169 OperatableStateMachine.add('turnToFindAnotherPersonOnce', _sm_turntofindanotherpersononce_0, transitions={ 'end': 'cause1', 'finished': 'Action_findPerson', 'failed': 'cause1' }, autonomy={ 'end': Autonomy.Inherit, 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:541 y:89 OperatableStateMachine.add( 'cause1', SetKey(Value="I did not find any person"), transitions={'done': 'SetRosParam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:523 y:253 OperatableStateMachine.add( 'cause2', SetKey(Value="I did not understand the question."), transitions={'done': 'SetRosParam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:515 y:350 OperatableStateMachine.add( 'cause3', SetKey(Value="I was unable to answer the question."), transitions={'done': 'SetRosParam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:630 y:196 OperatableStateMachine.add( 'SetRosParam', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:40 y:469 OperatableStateMachine.add('log answer', LogKeyState( text="{}", severity=Logger.REPORT_HINT), transitions={'done': 'Say_Answer'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'answer'}) # x:42 y:550 OperatableStateMachine.add('Say_Answer', 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:1070 y:60, x:1070 y:209 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.joint_config = [-1.0, 0.5] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:110 y:168 OperatableStateMachine.add( 'flexbe_WaitState', WaitState(wait_time=1.0), transitions={'done': 'flexbe_GetJointValuesState'}, autonomy={'done': Autonomy.Off}) # x:516 y:175 OperatableStateMachine.add( 'flexbe_LogKeyState', LogKeyState(text='joint_values: {}', severity=Logger.REPORT_HINT), transitions={'done': 'hsr_MoveitToJointsState'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_values'}) # x:753 y:82 OperatableStateMachine.add( 'hsr_MoveitToJointsState', MoveitToJointsState( move_group='whole_body', joint_names=['arm_flex_joint', 'arm_lift_joint'], action_topic='/move_group'), transitions={ 'reached': 'finished', 'planning_failed': 'failed', 'control_failed': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off }, remapping={'joint_config': 'joint_config'}) # x:291 y:55 OperatableStateMachine.add( 'flexbe_GetJointValuesState', GetJointValuesState(joints=[ 'arm_flex_joint', 'arm_lift_joint', 'arm_roll_joint', 'base_l_drive_wheel_joint', 'base_r_drive_wheel_joint', 'base_roll_joint', 'hand_l_spring_proximal_joint', 'hand_motor_joint', 'hand_r_spring_proximal_joint', 'head_pan_joint', 'head_tilt_joint', 'wrist_flex_joint', 'wrist_roll_joint' ]), transitions={'retrieved': 'flexbe_LogKeyState'}, autonomy={'retrieved': Autonomy.Off}, remapping={'joint_values': 'joint_values'}) 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:1500 y:655, x:784 y:398 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.action_topic = "/move_group" _state_machine.userdata.trajectory_action_topic = "/m1n6s200_driver/arm_controller/follow_joint_trajectory" _state_machine.userdata.move_group_arm = "arm" _state_machine.userdata.config_vertical = "Vertical" _state_machine.userdata.config_home = "Home" _state_machine.userdata.config_retract = "Retract" _state_machine.userdata.components = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'MoveIt', SetupProxyMoveItClientState( robot_description="/robot_description", robot_description_semantic=None, move_group_capabilities="/move_group", action_type_and_topics=None, enter_wait_duration=0.0), transitions={ 'connected': 'Connected', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'connected': Autonomy.High, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'move_groups': 'move_groups' }) # x:146 y:145 OperatableStateMachine.add('Connected', LogState(text="Connected", severity=Logger.REPORT_HINT), transitions={'done': 'RobotName'}, autonomy={'done': Autonomy.Off}) # x:645 y:678 OperatableStateMachine.add('Vertical', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'GoalTolerances', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Low, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group_arm', 'config_name': 'config_vertical', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:823 y:29 OperatableStateMachine.add( 'Plan', JointValuesToMoveItPlanState(timeout=5.0, enter_wait_duration=0.5, action_topic=None), transitions={ 'planned': 'Execute', 'failed': 'failed', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Full, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'action_topic': 'action_topic', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values', 'joint_trajectory': 'joint_trajectory' }) # x:1215 y:121 OperatableStateMachine.add( 'Execute', TrajectoryToFollowJointTrajectoryActionState( goal_time_tolerance=3.0, max_delay=-1.0, wait_duration=2.0, timeout=5.0, action_topic=None), transitions={ 'reached': 'Decision', 'goal_failed': 'GoalFailedLog', 'path_failed': 'PathTolLog', 'invalid_request': 'failed', 'param_error': 'failed', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Full, 'goal_failed': Autonomy.Off, 'path_failed': Autonomy.Off, 'invalid_request': Autonomy.Full, 'param_error': Autonomy.Full, 'failed': Autonomy.Full }, remapping={ 'trajectory_action_topic': 'trajectory_action_topic', 'joint_trajectory': 'joint_trajectory', 'joint_goal_tolerances': 'joint_goal_tolerances', 'joint_path_tolerances': 'joint_path_tolerances', 'status_text': 'status_text', 'goal_names': 'goal_names', 'goal_values': 'goal_values' }) # x:616 y:559 OperatableStateMachine.add('GoalTolerances', SetJointTrajectoryTolerancesState( position_constraints=[0.05], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'LogValues', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_goal_tolerances' }) # x:544 y:189 OperatableStateMachine.add('PathTolerances', SetJointTrajectoryTolerancesState( position_constraints=[0.08], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'DumpGoalTolerance', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_path_tolerances' }) # x:389 y:34 OperatableStateMachine.add( 'DumpGoalTolerance', LogKeyState(text="Goal tolerance {}", severity=Logger.REPORT_HINT), transitions={'done': 'DumpPathTolerance'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_goal_tolerances'}) # x:560 y:50 OperatableStateMachine.add( 'DumpPathTolerance', LogKeyState(text="Path tolerance {}", severity=Logger.REPORT_HINT), transitions={'done': 'Plan'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_path_tolerances'}) # x:889 y:579 OperatableStateMachine.add('Home', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'Plan', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group_arm', 'config_name': 'config_home', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:1265 y:670 OperatableStateMachine.add('Decision', OperatorDecisionState(outcomes=[ "home", "vertical", "PS", "stop" ], hint=None, suggestion=None), transitions={ 'home': 'Home', 'vertical': 'Vertical', 'PS': 'ApplyPS', 'stop': 'finished' }, autonomy={ 'home': Autonomy.Off, 'vertical': Autonomy.Off, 'PS': Autonomy.Low, 'stop': Autonomy.Full }) # x:48 y:261 OperatableStateMachine.add( 'ClearOM', ClearOctomapState(timeout=5.0, wait_duration=5, action_topic="/clear_octomap"), transitions={ 'done': 'QueryPlanners', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.High }, remapping={'action_topic': 'action_topic'}) # x:47 y:402 OperatableStateMachine.add( 'QueryPlanners', QueryPlannersState(timeout=5.0, wait_duration=0.001, action_topic="/query_planner_interface"), transitions={ 'done': 'LogPlanners', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'action_topic': 'action_topic', 'planner_interfaces': 'planner_interfaces' }) # x:108 y:527 OperatableStateMachine.add( 'LogPlanners', LogKeyState(text="Available Planners {}", severity=Logger.REPORT_HINT), transitions={'done': 'GetPlanningScene'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'planner_interfaces'}) # x:87 y:641 OperatableStateMachine.add('GetPlanningScene', GetPlanningSceneState( components=1023, timeout=5.0, wait_duration=5, action_topic="/get_planning_scene"), transitions={ 'done': 'LogPS', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={ 'action_topic': 'action_topic', 'scene': 'scene' }) # x:1411 y:534 OperatableStateMachine.add( 'ApplyPS', ApplyPlanningSceneState(timeout=5.0, wait_duration=5, action_topic="/apply_planning_scene"), transitions={ 'done': 'Decision', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'action_topic': 'action_topic'}) # x:363 y:684 OperatableStateMachine.add('LogPS', LogKeyState( text="Planning Scence {}", severity=Logger.REPORT_HINT), transitions={'done': 'Vertical'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'scene'}) # x:1157 y:336 OperatableStateMachine.add('PathTolLog', LogState(text="Path tolerance failure", severity=Logger.REPORT_HINT), transitions={'done': 'Decision'}, autonomy={'done': Autonomy.High}) # x:1254 y:288 OperatableStateMachine.add( 'GoalFailedLog', LogState(text="Failed to reach goal tolerance", severity=Logger.REPORT_HINT), transitions={'done': 'Decision'}, autonomy={'done': Autonomy.High}) # x:279 y:190 OperatableStateMachine.add('RobotName', LogKeyState( text="Robot Name ({})", severity=Logger.REPORT_HINT), transitions={'done': 'ClearOM'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'robot_name'}) # x:632 y:279 OperatableStateMachine.add('LogValues', LogKeyState( text="Joint Values {}", severity=Logger.REPORT_HINT), transitions={'done': 'PathTolerances'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_values'}) return _state_machine
def create(self): wait_for_confirm = 25 wait_for_utter = 15 intents = ['YesNo', 'Complete', 'ConfirmGripperFinished'] answer = 'part_type' detail_levels = 'low' confirm_key = 'answer' wait_before_exit = 2 session_id = None # x:143 y:908, x:539 y:365, x:546 y:667, x:755 y:635, x:860 y:350, x:668 y:414 _state_machine = OperatableStateMachine( outcomes=[ 'complete', 'incomplete', 'fail', 'unknwon', 'aborted', 'succeded' ], input_keys=[ 'screw_id', 'screw_found', 'last_insert_finished_successful' ], output_keys=['list_available_screw_poses', 'confirmed']) _state_machine.userdata.gripper_ended_question = 'Is the gripper complete?' _state_machine.userdata.answer = '' _state_machine.userdata.retry = 2 _state_machine.userdata.list_available_screw_poses = [] _state_machine.userdata.next_trajectory = -1 _state_machine.userdata.screw_id = 0 _state_machine.userdata.screw_found = 0 _state_machine.userdata.last_insert_finished_successful = True _state_machine.userdata.confirmed = '' _state_machine.userdata.utter_abort_screw_state = 'Error in the update of the screw state, aborting' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:373, x:385 y:382 _sm_continueretry_0 = OperatableStateMachine( outcomes=['true', 'false'], input_keys=['retry'], output_keys=['retry']) with _sm_continueretry_0: # x:158 y:82 OperatableStateMachine.add( 'decrease_retry', CalculationState(calculation=lambda x: x - 1), transitions={'done': 'continue_asking'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'retry', 'output_value': 'retry' }) # x:156 y:221 OperatableStateMachine.add( 'continue_asking', CheckConditionState(predicate=lambda x: x > 0), transitions={ 'true': 'true', 'false': 'false' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'retry'}) with _state_machine: # x:25 y:35 OperatableStateMachine.add( 'log_insert_success', LogKeyState(text="GEC-ENTER: last insertion was succesful? {}", severity=Logger.REPORT_HINT), transitions={'done': 'log_screw_id'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'last_insert_finished_successful'}) # x:1069 y:524 OperatableStateMachine.add('ContinueRetry', _sm_continueretry_0, transitions={ 'true': 'log_retry_value', 'false': 'wait_unknown' }, autonomy={ 'true': Autonomy.Inherit, 'false': Autonomy.Inherit }, remapping={'retry': 'retry'}) # x:104 y:685 OperatableStateMachine.add( 'GetConfirm', LisaGetPayloadKeyState(payload_key=confirm_key), transitions={ 'done': 'wait_complete', 'error': 'fail' }, autonomy={ 'done': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'payload': 'payload', 'payload_value': 'confirmed' }) # x:1304 y:483 OperatableStateMachine.add( 'NotRecognized', LisaRecognitionResultToStringState( context_id=session_id, detail_levels=detail_levels), transitions={'done': 'UtterRecognizedText'}, autonomy={'done': Autonomy.Off}, remapping={ 'payload': 'payload', 'original_sentence': 'original_sentence', 'error_reason': 'error_reason', 'intent_recognized': 'intent_recognized', 'text_to_utter': 'text_to_utter' }) # x:52 y:376 OperatableStateMachine.add('Recognized', LisaRecognitionResultToStringState( context_id=session_id, detail_levels=detail_levels), transitions={'done': 'UtterRecognized'}, autonomy={'done': Autonomy.Off}, remapping={ 'payload': 'payload', 'original_sentence': 'original_sentence', 'error_reason': 'error_reason', 'intent_recognized': 'intent_recognized', 'text_to_utter': 'text_to_utter' }) # x:49 y:134 OperatableStateMachine.add('Update Screw State', SetScrewServiceState(), transitions={ 'succeeded': 'Update Success', 'aborted': 'Utter Error Update Screw State' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'screw_id': 'screw_id', 'finished_successful': 'last_insert_finished_successful', 'screw_found': 'screw_found', 'result': 'result' }) # x:380 y:87 OperatableStateMachine.add( 'Update Success', LogKeyState(text="GEC: Update Screw State returned: {}", severity=Logger.REPORT_HINT), transitions={'done': 'Valid Screw In Current Gripper'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'result'}) # x:355 y:155 OperatableStateMachine.add('Utter Error Update Screw State', LisaUtterState(context_id=session_id, wait_time=15, suspend_time=0), transitions={ 'done': 'fail', 'preempt': 'fail', 'timeouted': 'fail', 'error': 'fail' }, autonomy={ 'done': Autonomy.Off, 'preempt': Autonomy.Off, 'timeouted': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'text_to_utter': 'utter_abort_screw_state', 'error_reason': 'error_reason' }) # x:74 y:542 OperatableStateMachine.add('UtterRecognized', LisaUtterState(context_id=None, wait_time=wait_for_utter, suspend_time=0), transitions={ 'done': 'GetConfirm', 'preempt': 'GetConfirm', 'timeouted': 'GetConfirm', 'error': 'fail' }, autonomy={ 'done': Autonomy.Off, 'preempt': Autonomy.Off, 'timeouted': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'text_to_utter': 'text_to_utter', 'error_reason': 'error_reason' }) # x:1104 y:747 OperatableStateMachine.add('UtterRecognizedText', LisaUtterState(context_id=session_id, wait_time=wait_for_utter, suspend_time=0), transitions={ 'done': 'ContinueRetry', 'preempt': 'wait_unknown', 'timeouted': 'wait_unknown', 'error': 'fail' }, autonomy={ 'done': Autonomy.Off, 'preempt': Autonomy.Off, 'timeouted': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'text_to_utter': 'text_to_utter', 'error_reason': 'error_reason' }) # x:545 y:26 OperatableStateMachine.add( 'Valid Screw In Current Gripper', PartAvailablePoseService(check_only_current_gripper=True), transitions={ 'succeeded': 'succeded', 'aborted': 'aborted' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'last_trajectory_id': 'screw_id', 'list_available_parts': 'list_available_screw_poses' }) # x:768 y:137 OperatableStateMachine.add( 'log_exit_incomplete', LogKeyState(text="GEC-EXIT-INCOMPLETE {}", severity=Logger.REPORT_HINT), transitions={'done': 'wait_before_incomplete'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'list_available_screw_poses'}) # x:1126 y:350 OperatableStateMachine.add( 'log_retry_value', LogKeyState(text="GEC: retry level is {}", severity=Logger.REPORT_HINT), transitions={'done': 'AskGripperEneded'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'retry'}) # x:308 y:28 OperatableStateMachine.add( 'log_screw_found', LogKeyState(text="GEC-ENTER: I found the latest screw? {}", severity=Logger.REPORT_HINT), transitions={'done': 'Update Screw State'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'screw_found'}) # x:193 y:36 OperatableStateMachine.add( 'log_screw_id', LogKeyState(text="GEC-ENTER: latest screw id was: {}", severity=Logger.REPORT_HINT), transitions={'done': 'log_screw_found'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'screw_id'}) # x:589 y:148 OperatableStateMachine.add('wait_before_incomplete', WaitState(wait_time=wait_before_exit), transitions={'done': 'incomplete'}, autonomy={'done': Autonomy.Off}) # x:109 y:774 OperatableStateMachine.add('wait_complete', WaitState(wait_time=wait_before_exit), transitions={'done': 'complete'}, autonomy={'done': Autonomy.Off}) # x:857 y:592 OperatableStateMachine.add('wait_unknown', WaitState(wait_time=wait_before_exit), transitions={'done': 'unknwon'}, autonomy={'done': Autonomy.Off}) # x:1271 y:115 OperatableStateMachine.add( 'AskGripperEneded', LisaUtterAndWaitForIntentState(context_id=session_id, intents=intents, wait_time=wait_for_confirm), transitions={ 'intent_recognized': 'Recognized', 'intent_not_recognized': 'NotRecognized', 'preempt': 'wait_unknown', 'timeouted': 'wait_unknown', 'error': 'fail' }, autonomy={ 'intent_recognized': Autonomy.Off, 'intent_not_recognized': Autonomy.Off, 'preempt': Autonomy.Off, 'timeouted': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'text_to_utter': 'gripper_ended_question', 'payload': 'payload', 'original_sentence': 'original_sentence', 'error_reason': 'error_reason', 'intent_recognized': 'intent_recognized' }) return _state_machine
def create(self): # x:1490 y:191, x:764 y:433 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.action_topic = "/move_group" _state_machine.userdata.trajectory_action_topic = "/m1n6s200_driver/arm_controller/follow_joint_trajectory" _state_machine.userdata.config_vertical = "Vertical" _state_machine.userdata.config_home = "Home" _state_machine.userdata.config_retract = "Retract" _state_machine.userdata.components = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'MoveIt', SetupProxyMoveItClientState( robot_description="/robot_description", robot_description_semantic=None, move_group_capabilities=[[ "/move_group", ["arm", "gripper"] ]], action_type_and_topics=None, enter_wait_duration=0.0), transitions={ 'connected': 'Connected', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'connected': Autonomy.High, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'move_groups': 'move_groups' }) # x:52 y:151 OperatableStateMachine.add('Connected', LogState(text="Connected", severity=Logger.REPORT_HINT), transitions={'done': 'SetMG'}, autonomy={'done': Autonomy.Off}) # x:690 y:610 OperatableStateMachine.add('DumpJVUser', LogKeyState( text="Joint Values {}", severity=Logger.REPORT_HINT), transitions={'done': 'GetJVList'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'user_joint_values'}) # x:243 y:287 OperatableStateMachine.add('GetNames', GetJointNamesFromMoveGroupState(), transitions={ 'retrieved': 'UserNames', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'selected_move_group', 'move_group': 'move_group', 'joint_names': 'user_joint_names' }) # x:51 y:257 OperatableStateMachine.add( 'SetMG', UserDataState(data="arm"), transitions={'done': 'GetNames'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'selected_move_group'}) # x:438 y:420 OperatableStateMachine.add( 'GetJVUser', GetCurrentJointValuesUserState( timeout=5.0, joint_states_topic='/m1n6s200_driver/joint_states'), transitions={ 'retrieved': 'DumpJVUser', 'timeout': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'timeout': Autonomy.High }, remapping={ 'joint_names': 'no_ee_names', 'joint_values': 'user_joint_values' }) # x:853 y:612 OperatableStateMachine.add( 'GetJVList', GetCurrentJointValuesListState( joint_names=[ 'm1n6s200_joint_1', 'm1n6s200_joint_2', 'm1n6s200_joint_3', 'm1n6s200_joint_4' ], timeout=None, joint_states_topic='/m1n6s200_driver/joint_states'), transitions={ 'retrieved': 'ListNames', 'timeout': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'timeout': Autonomy.High }, remapping={ 'joint_names': 'list_joint_names', 'joint_values': 'list_joint_values' }) # x:572 y:156 OperatableStateMachine.add('ListNames', LogKeyState( text='List Names {}', severity=Logger.REPORT_HINT), transitions={'done': 'ListValues'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'list_joint_names'}) # x:647 y:42 OperatableStateMachine.add('ListValues', LogKeyState( text='List Values {}', severity=Logger.REPORT_HINT), transitions={'done': 'SetConfigName'}, autonomy={'done': Autonomy.Full}, remapping={'data': 'list_joint_values'}) # x:188 y:388 OperatableStateMachine.add('UserNames', LogKeyState( text='User names {}', severity=Logger.REPORT_HINT), transitions={'done': 'JustArmJoints'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'user_joint_names'}) # x:222 y:484 OperatableStateMachine.add( 'JustArmJoints', UserDataState(data=[ 'm1n6s200_joint_1', 'm1n6s200_joint_2', 'm1n6s200_joint_3', 'm1n6s200_joint_4', 'm1n6s200_joint_5', 'm1n6s200_joint_6' ]), transitions={'done': 'GetJVUser'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'no_ee_names'}) # x:1000 y:34 OperatableStateMachine.add('GetRetract', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'JVTrajectory', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group', 'config_name': 'config_name', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:802 y:44 OperatableStateMachine.add('SetConfigName', UserDataState(data="Retract"), transitions={'done': 'GetRetract'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'config_name'}) # x:1035 y:139 OperatableStateMachine.add( 'JVTrajectory', JointValuesToTrajectoryState(duration=10.0), transitions={ 'done': 'SetGoal', 'param_error': 'failed' }, autonomy={ 'done': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_values': 'joint_values', 'joint_trajectory': 'joint_trajectory' }) # x:1131 y:464 OperatableStateMachine.add( 'FJTA', TrajectoryToFollowJointTrajectoryActionState( goal_time_tolerance=3.0, max_delay=-1.0, wait_duration=2.0, timeout=1.0, action_topic=None), transitions={ 'reached': 'JV2MoveAction', 'goal_failed': 'GoalTolFailure', 'path_failed': 'PathTolFailed', 'invalid_request': 'failed', 'param_error': 'failed', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Full, 'goal_failed': Autonomy.Off, 'path_failed': Autonomy.Off, 'invalid_request': Autonomy.Full, 'param_error': Autonomy.Full, 'failed': Autonomy.Full }, remapping={ 'trajectory_action_topic': 'trajectory_action_topic', 'joint_trajectory': 'joint_trajectory', 'joint_goal_tolerances': 'joint_goal_tolerances', 'joint_path_tolerances': 'joint_path_tolerances', 'status_text': 'status_text', 'goal_names': 'goal_names', 'goal_values': 'goal_values' }) # x:1051 y:230 OperatableStateMachine.add('SetGoal', SetJointTrajectoryTolerancesState( position_constraints=[0.02], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'SetPath', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_goal_tolerances' }) # x:1062 y:349 OperatableStateMachine.add('SetPath', SetJointTrajectoryTolerancesState( position_constraints=[0.0], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'FJTA', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_path_tolerances' }) # x:1386 y:539 OperatableStateMachine.add( 'JV2MoveAction', JointValuesToMoveActionState( joint_names=['m1n6s200_joint_1', 'm1n6s200_joint_2'], joint_values=[0.0, 1.57], move_group='arm', action_topic='/move_group', joint_tolerance=0.02, constraint_weight=1.0, allowed_planning_time=4.0, wait_duration=0.2, timeout=30.0), transitions={ 'reached': 'SetConfigName_2', 'param_error': 'failed', 'planning_failed': 'failed', 'control_failed': 'CtrlFailed', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Full, 'param_error': Autonomy.Full, 'planning_failed': Autonomy.Full, 'control_failed': Autonomy.Full, 'failed': Autonomy.Off }, remapping={ 'move_group': 'move_group', 'action_topic': 'action_topic', 'status_text': 'status_text', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:1324 y:272 OperatableStateMachine.add('CtrlFailed', LogState(text="Control failed", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Full}) # x:1648 y:554 OperatableStateMachine.add('GetHome', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'SayJointNames', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group', 'config_name': 'config_name', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:1489 y:749 OperatableStateMachine.add('SetConfigName_2', UserDataState(data="Home"), transitions={'done': 'ConfigNamePrint'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'config_name'}) # x:1644 y:267 OperatableStateMachine.add('MoveGroup', MoveGroupState(timeout=30.0, wait_duration=0.02, move_group=None), transitions={ 'reached': 'finished', 'control_failed': 'CtrlFailed', 'planning_failed': 'failed', 'param_error': 'failed', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Full, 'control_failed': Autonomy.Full, 'planning_failed': Autonomy.High, 'param_error': Autonomy.Full, 'failed': Autonomy.Full }, remapping={ 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values', 'status_text': 'status_text', 'planned_trajectory': 'planned_trajectory', 'executed_trajectory': 'executed_trajectory' }) # x:1689 y:666 OperatableStateMachine.add('ConfigNamePrint', LogKeyState( text="Config {}", severity=Logger.REPORT_HINT), transitions={'done': 'GetHome'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'config_name'}) # x:1701 y:450 OperatableStateMachine.add('SayJointNames', LogKeyState( text="Joint names {}", severity=Logger.REPORT_HINT), transitions={'done': 'SayJointValues'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_names'}) # x:1550 y:367 OperatableStateMachine.add('SayJointValues', LogKeyState( text="Values {}", severity=Logger.REPORT_HINT), transitions={'done': 'MoveGroup'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_values'}) # x:1113 y:682 OperatableStateMachine.add('PathTolFailed', LogState(text="Path tolerance failure", severity=Logger.REPORT_HINT), transitions={'done': 'JV2MoveAction'}, autonomy={'done': Autonomy.High}) # x:1240 y:661 OperatableStateMachine.add('GoalTolFailure', LogState(text="Goal tolerance failure", severity=Logger.REPORT_HINT), transitions={'done': 'JV2MoveAction'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:1453 y:629, x:807 y:466 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.action_topic = "/move_group" _state_machine.userdata.trajectory_action_topic = "/m1n6s200_driver/arm_controller/follow_joint_trajectory" _state_machine.userdata.move_group_arm = "arm" _state_machine.userdata.config_vertical = "Vertical" _state_machine.userdata.config_home = "Home" _state_machine.userdata.config_retract = "Retract" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'MoveIt', SetupProxyMoveItClientState( robot_description="/robot_description", robot_description_semantic=None, move_group_capabilities="/move_group", action_type_and_topics=[[ "MoveGroupAction", ["/move_group"] ]], enter_wait_duration=0.0), transitions={ 'connected': 'Connected', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'connected': Autonomy.High, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'move_groups': 'move_groups' }) # x:132 y:177 OperatableStateMachine.add('Connected', LogState(text="Connected", severity=Logger.REPORT_HINT), transitions={'done': 'Vertical'}, autonomy={'done': Autonomy.Off}) # x:61 y:638 OperatableStateMachine.add('Vertical', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'GoalTolerances', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Low, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group_arm', 'config_name': 'config_vertical', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:823 y:29 OperatableStateMachine.add( 'Plan', JointValuesToMoveItPlanState(timeout=5.0, enter_wait_duration=0.5, action_topic=None), transitions={ 'planned': 'Execute', 'failed': 'failed', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Full, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'action_topic': 'action_topic', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values', 'joint_trajectory': 'joint_trajectory' }) # x:1215 y:121 OperatableStateMachine.add( 'Execute', TrajectoryToFollowJointTrajectoryActionState( goal_time_tolerance=3.0, max_delay=-1.0, wait_duration=2.0, timeout=1.0, action_topic=None), transitions={ 'reached': 'Decision', 'goal_failed': 'GoalFail', 'path_failed': 'PathFail', 'invalid_request': 'failed', 'param_error': 'failed', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Full, 'goal_failed': Autonomy.Low, 'path_failed': Autonomy.Low, 'invalid_request': Autonomy.Full, 'param_error': Autonomy.Full, 'failed': Autonomy.Full }, remapping={ 'trajectory_action_topic': 'trajectory_action_topic', 'joint_trajectory': 'joint_trajectory', 'joint_goal_tolerances': 'joint_goal_tolerances', 'joint_path_tolerances': 'joint_path_tolerances', 'status_text': 'status_text', 'goal_names': 'goal_names', 'goal_values': 'goal_values' }) # x:342 y:441 OperatableStateMachine.add('GoalTolerances', SetJointTrajectoryTolerancesState( position_constraints=[0.05], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'PathTolerances', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_goal_tolerances' }) # x:251 y:334 OperatableStateMachine.add('PathTolerances', SetJointTrajectoryTolerancesState( position_constraints=[0.08], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'DumpGoalTolerance', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_path_tolerances' }) # x:389 y:34 OperatableStateMachine.add( 'DumpGoalTolerance', LogKeyState(text="Goal tolerance {}", severity=Logger.REPORT_HINT), transitions={'done': 'DumpPathTolerance'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_goal_tolerances'}) # x:560 y:50 OperatableStateMachine.add( 'DumpPathTolerance', LogKeyState(text="Path tolerance {}", severity=Logger.REPORT_HINT), transitions={'done': 'Plan'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_path_tolerances'}) # x:889 y:579 OperatableStateMachine.add('Home', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'Plan', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group_arm', 'config_name': 'config_home', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:1265 y:670 OperatableStateMachine.add( 'Decision', OperatorDecisionState(outcomes=["home", "vertical", "stop"], hint=None, suggestion=None), transitions={ 'home': 'Home', 'vertical': 'Vertical', 'stop': 'finished' }, autonomy={ 'home': Autonomy.Off, 'vertical': Autonomy.Off, 'stop': Autonomy.Full }) # x:1238 y:387 OperatableStateMachine.add('PathFail', LogKeyState( text="Path failure {}", severity=Logger.REPORT_HINT), transitions={'done': 'Decision'}, autonomy={'done': Autonomy.High}, remapping={'data': 'status_text'}) # x:1136 y:380 OperatableStateMachine.add('GoalFail', LogKeyState( text="Goal failure {}", severity=Logger.REPORT_HINT), transitions={'done': 'Decision'}, autonomy={'done': Autonomy.High}, remapping={'data': 'status_text'}) 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:332 y:411, x:368 y:23 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # 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_drive_in_square_and_move_arm_demo_0 = ConcurrencyContainer( outcomes=['finished', 'failed'], conditions=[('finished', [ ('Drive_In_Squre_Stop_If_Obstacle', 'finished') ]), ('failed', [('Drive_In_Squre_Stop_If_Obstacle', 'failed')])]) with _sm_drive_in_square_and_move_arm_demo_0: # x:64 y:131 OperatableStateMachine.add( 'Drive_In_Squre_Stop_If_Obstacle', self.use_behavior( Drive_In_Squre_Stop_If_ObstacleSM, 'Drive in square and move arm demo/Drive_In_Squre_Stop_If_Obstacle' ), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) with _state_machine: # x:72 y:47 OperatableStateMachine.add( 'get new command', GetRobcoCommand(), transitions={ 'failed': 'failed', 'done': 'log key' }, autonomy={ 'failed': Autonomy.Off, 'done': Autonomy.Off }, remapping={'robco_command_OUT': 'robco_command_OUT'}) # x:579 y:56 OperatableStateMachine.add('Tell a joke Bulgarian', self.use_behavior( TellajokeBulgarianSM, 'Tell a joke Bulgarian'), transitions={ 'finished': 'get new command', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:227 y:122 OperatableStateMachine.add( 'which demo?', DecisionState(outcomes=['joke', 'demo', 'exit'], conditions=lambda x: 'joke' if x == 'tell_a_joke' else ('demo' if x == 'full_demo' else 'exit')), transitions={ 'joke': 'Tell a joke Bulgarian', 'demo': 'Drive in square and move arm demo', 'exit': 'finished' }, autonomy={ 'joke': Autonomy.Low, 'demo': Autonomy.Low, 'exit': Autonomy.Off }, remapping={'input_value': 'robco_command_OUT'}) # x:129 y:242 OperatableStateMachine.add('log key', LogKeyState( text='robco_command_OUT value: {}', severity=Logger.REPORT_INFO), transitions={'done': 'which demo?'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'robco_command_OUT'}) # x:548 y:180 OperatableStateMachine.add('Drive in square and move arm demo', _sm_drive_in_square_and_move_arm_demo_0, transitions={ 'finished': 'get new command', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) 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( 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_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( 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: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( 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(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(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): conveyor_speed = 100 move_group = "robot1" joint_names = ["robot1_elbow_joint", "robot1_shoulder_lift_joint", "robot1_shoulder_pan_joint", "robot1_wrist_1_joint", "robot1_wrist_2_joint", "robot1_wrist_3_joint"] # x:1738 y:801, x:63 y:817 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.conveyor_speed = conveyor_speed # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:455, x:130 y:455, x:230 y:455, x:330 y:455 _sm_grasp_0 = OperatableStateMachine(outcomes=['failed', 'planning_failed', 'control_failed', 'reached'], input_keys=['pose']) with _sm_grasp_0: # x:33 y:40 OperatableStateMachine.add('compute grasp', ComputeGraspState(group=move_group, offset=0.15, joint_names=joint_names, tool_link="vacuum_gripper1_suction_cup", rotation=3.14), transitions={'continue': 'move to grasp', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'pose': 'pose', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:30 y:146 OperatableStateMachine.add('move to grasp', flexbe_manipulation_states__MoveitToJointsDynState(move_group=move_group, action_topic='/move_group'), transitions={'reached': 'reached', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off}, remapping={'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:29 y:455, x:130 y:455, x:230 y:455, x:330 y:455 _sm_pregrasp_position_1 = OperatableStateMachine(outcomes=['failed', 'reached', 'planning_failed', 'control_failed'], input_keys=['pose']) with _sm_pregrasp_position_1: # x:30 y:40 OperatableStateMachine.add('compute pregrasp position', ComputeGraspState(group=move_group, offset=0.2, joint_names=joint_names, tool_link="vacuum_gripper1_suction_cup", rotation=3.14), transitions={'continue': 'move to pregrasp', 'failed': 'log'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'pose': 'pose', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:162 y:126 OperatableStateMachine.add('move to pregrasp', flexbe_manipulation_states__MoveitToJointsDynState(move_group=move_group, action_topic='/move_group'), transitions={'reached': 'reached', 'planning_failed': 'log_2', 'control_failed': 'control_failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off}, remapping={'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:17 y:201 OperatableStateMachine.add('log', LogState(text="compute pregrasp failed", severity=Logger.REPORT_HINT), transitions={'done': 'compute pregrasp position'}, autonomy={'done': Autonomy.Off}) # x:220 y:214 OperatableStateMachine.add('log_2', LogState(text="planning pregrasp failed", severity=Logger.REPORT_HINT), transitions={'done': 'planning_failed'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:32 y:28 OperatableStateMachine.add('go home', omtp_factory_flexbe_states__SrdfStateToMoveit(config_name="R1Home", move_group="robot1", action_topic='/move_group', robot_name=""), transitions={'reached': 'start conveyor', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:417 y:26 OperatableStateMachine.add('add objects', ControlFeederState(activation=True), transitions={'succeeded': 'wait', 'failed': 'failed'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}) # x:209 y:79 OperatableStateMachine.add('start conveyor', SetConveyorPowerState(stop=False), transitions={'succeeded': 'add objects', 'failed': 'failed'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'speed': 'conveyor_speed'}) # x:1005 y:70 OperatableStateMachine.add('subscribe to beam', SubscriberState(topic="/break_beam_sensor", blocking=True, clear=False), transitions={'received': 'decision', 'unavailable': 'failed'}, autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off}, remapping={'message': 'message'}) # x:1203 y:22 OperatableStateMachine.add('decision', DecisionState(outcomes=["True","False"], conditions=lambda a: a.object_detected==True), transitions={'True': 'stop conveyor', 'False': 'subscribe to beam'}, autonomy={'True': Autonomy.Off, 'False': Autonomy.Off}, remapping={'input_value': 'message'}) # x:1354 y:60 OperatableStateMachine.add('stop conveyor', SetConveyorPowerState(stop=True), transitions={'succeeded': 'get camera state', 'failed': 'failed'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'speed': 'conveyor_speed'}) # x:1612 y:25 OperatableStateMachine.add('get camera state', DetectPartCameraState(ref_frame="world", camera_topic="/omtp/logical_camera", camera_frame="logical_camera_frame"), transitions={'continue': 'log', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'pose': 'pose'}) # x:1707 y:111 OperatableStateMachine.add('log', LogKeyState(text="data:{}", severity=Logger.REPORT_HINT), transitions={'done': 'pregrasp position'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'pose'}) # x:1660 y:195 OperatableStateMachine.add('pregrasp position', _sm_pregrasp_position_1, transitions={'failed': 'failed', 'reached': 'activate gripper', 'planning_failed': 'failed', 'control_failed': 'failed'}, autonomy={'failed': Autonomy.Inherit, 'reached': Autonomy.Inherit, 'planning_failed': Autonomy.Inherit, 'control_failed': Autonomy.Inherit}, remapping={'pose': 'pose'}) # x:1634 y:297 OperatableStateMachine.add('activate gripper', VacuumGripperControlState(enable=True, service_name="/gripper1/control"), transitions={'continue': 'grasp', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1609 y:674 OperatableStateMachine.add('go to bin', omtp_factory_flexbe_states__SrdfStateToMoveit(config_name="R1Place", move_group="robot1", action_topic='/move_group', robot_name=""), transitions={'reached': 'deactivate gripper', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1648 y:402 OperatableStateMachine.add('grasp', _sm_grasp_0, transitions={'failed': 'failed', 'planning_failed': 'failed', 'control_failed': 'failed', 'reached': 'go to home'}, autonomy={'failed': Autonomy.Inherit, 'planning_failed': Autonomy.Inherit, 'control_failed': Autonomy.Inherit, 'reached': Autonomy.Inherit}, remapping={'pose': 'pose'}) # x:1523 y:762 OperatableStateMachine.add('deactivate gripper', VacuumGripperControlState(enable=False, service_name="/gripper1/control"), transitions={'continue': 'go to bin', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1617 y:504 OperatableStateMachine.add('go to home', omtp_factory_flexbe_states__SrdfStateToMoveit(config_name="R1Home", move_group="robot1", action_topic='/move_group', robot_name=""), transitions={'reached': 'wait2', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:622 y:70 OperatableStateMachine.add('wait', WaitState(wait_time=3), transitions={'done': 'stop adding objects'}, autonomy={'done': Autonomy.Off}) # x:773 y:25 OperatableStateMachine.add('stop adding objects', ControlFeederState(activation=False), transitions={'succeeded': 'subscribe to beam', 'failed': 'failed'}, autonomy={'succeeded': Autonomy.Off, 'failed': Autonomy.Off}) # x:1682 y:589 OperatableStateMachine.add('wait2', WaitState(wait_time=1), transitions={'done': 'go to bin'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:84 y:465, x:727 y:360 _state_machine = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['className'], output_keys=['entity']) _state_machine.userdata.className = "bottle" _state_machine.userdata.entity = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365 _sm_look_at_0 = OperatableStateMachine(outcomes=['end'], input_keys=['ID']) with _sm_look_at_0: # x:75 y:135 OperatableStateMachine.add('look at', KeepLookingAt(), transitions={'failed': 'look at'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:798 y:597 _sm_rotation360_1 = OperatableStateMachine(outcomes=['end']) with _sm_rotation360_1: # x:42 y:34 OperatableStateMachine.add('Set 180 degres', SetKey(Value=3.1416), transitions={'done': 'rotate center'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:610 y:293 OperatableStateMachine.add('Look Right', SaraSetHeadAngle(pitch=0.5, yaw=1), transitions={'done': 'Rotate Right'}, autonomy={'done': Autonomy.Off}) # x:406 y:58 OperatableStateMachine.add('Rotate Left', WaitState(wait_time=5), transitions={'done': 'rotate center'}, autonomy={'done': Autonomy.Off}) # x:410 y:313 OperatableStateMachine.add('Rotate Right', WaitState(wait_time=5), transitions={'done': 'look center 2'}, autonomy={'done': Autonomy.Off}) # x:209 y:50 OperatableStateMachine.add('Look Left', SaraSetHeadAngle(pitch=0.5, yaw=-1), transitions={'done': 'Rotate Left'}, autonomy={'done': Autonomy.Off}) # x:216 y:186 OperatableStateMachine.add('Rotate Left 2', WaitState(wait_time=5), transitions={'done': 'Look Left'}, autonomy={'done': Autonomy.Off}) # x:630 y:170 OperatableStateMachine.add('wait 5', WaitState(wait_time=5), transitions={'done': 'Look Right'}, autonomy={'done': Autonomy.Off}) # x:603 y:47 OperatableStateMachine.add('rotate center', SaraSetHeadAngle(pitch=0.5, yaw=0), transitions={'done': 'wait 5'}, autonomy={'done': Autonomy.Off}) # x:208 y:308 OperatableStateMachine.add('look center 2', SaraSetHeadAngle(pitch=0.5, yaw=0), transitions={'done': 'Rotate Left 2'}, autonomy={'done': Autonomy.Off}) # x:683 y:188 _sm_find_entity_2 = OperatableStateMachine(outcomes=['found'], input_keys=['className'], output_keys=['entity']) with _sm_find_entity_2: # x:181 y:178 OperatableStateMachine.add('find_entity', list_entities_by_name( frontality_level=0.5, distance_max=2), transitions={ 'found': 'Get Entity', 'none_found': 'find_entity' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'className', 'entity_list': 'entity_list', 'number': 'number' }) # x:454 y:178 OperatableStateMachine.add( 'Get Entity', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'entity' }) # x:371 y:306, x:130 y:365, x:230 y:365 _sm_look_for_2_sec_3 = ConcurrencyContainer( outcomes=['done'], input_keys=['ID'], conditions=[('done', [('WaitState 2', 'done')]), ('done', [('Look at', 'end')])]) with _sm_look_for_2_sec_3: # x:84 y:166 OperatableStateMachine.add('Look at', _sm_look_at_0, transitions={'end': 'done'}, autonomy={'end': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:345 y:187 OperatableStateMachine.add('WaitState 2', WaitState(wait_time=3), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:372 y:27, x:370 y:220, x:368 y:100, x:352 y:305, x:460 y:465 _sm_find_entity_while_turning360_4 = ConcurrencyContainer( outcomes=['found', 'not_found'], input_keys=['className'], output_keys=['entity'], conditions=[('not_found', [('Rotation360', 'end')]), ('found', [('Find Entity', 'found')]), ('not_found', [('wait', 'done')])]) with _sm_find_entity_while_turning360_4: # x:131 y:44 OperatableStateMachine.add('Find Entity', _sm_find_entity_2, transitions={'found': 'found'}, autonomy={'found': Autonomy.Inherit}, remapping={ 'className': 'className', 'entity': 'entity' }) # x:129 y:197 OperatableStateMachine.add('Rotation360', _sm_rotation360_1, transitions={'end': 'not_found'}, autonomy={'end': Autonomy.Inherit}) # x:149 y:306 OperatableStateMachine.add('wait', WaitState(wait_time=30), transitions={'done': 'not_found'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:55 y:41 OperatableStateMachine.add( 'Look Front Center', SaraSetHeadAngle(pitch=0.7, yaw=0), transitions={'done': 'Find Entity WHILE Turning360'}, autonomy={'done': Autonomy.Off}) # x:345 y:156 OperatableStateMachine.add('Look Center Not Found', SaraSetHeadAngle(pitch=0.7, yaw=0), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:59 y:376 OperatableStateMachine.add('Log Entity', LogKeyState( text="Found entity: {}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'entity'}) # x:26 y:121 OperatableStateMachine.add('Find Entity WHILE Turning360', _sm_find_entity_while_turning360_4, transitions={ 'found': 'get ID', 'not_found': 'Look Center Not Found' }, autonomy={ 'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit }, remapping={ 'className': 'className', 'entity': 'entity' }) # x:45 y:290 OperatableStateMachine.add('look for 2 sec', _sm_look_for_2_sec_3, transitions={'done': 'Log Entity'}, autonomy={'done': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:62 y:211 OperatableStateMachine.add( 'get ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'look for 2 sec'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'ID' }) return _state_machine
def create(self): # x:1423 y:790, x:784 y:398 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.action_topic = "/move_group" _state_machine.userdata.trajectory_action_topic = "/m1n6s200_driver/arm_controller/follow_joint_trajectory" _state_machine.userdata.move_group = "arm" _state_machine.userdata.config_vertical = "Vertical" _state_machine.userdata.config_home = "Home" _state_machine.userdata.config_retract = "Retract" _state_machine.userdata.components = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'MoveIt', SetupProxyMoveItClientState( robot_description="/robot_description", robot_description_semantic=None, move_group_capabilities="/move_group", action_type_and_topics=[[ "MoveGroupAction", ["/move_group"] ]], enter_wait_duration=0.0), transitions={ 'connected': 'Connected', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'connected': Autonomy.High, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'move_groups': 'move_groups' }) # x:146 y:145 OperatableStateMachine.add('Connected', LogState(text="Connected", severity=Logger.REPORT_HINT), transitions={'done': 'Vertical'}, autonomy={'done': Autonomy.Off}) # x:481 y:644 OperatableStateMachine.add('Vertical', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'GoalTolerances', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Low, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group', 'config_name': 'config_vertical', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:909 y:136 OperatableStateMachine.add( 'Plan', JointValuesToMoveItPlanState(timeout=5.0, enter_wait_duration=0.5, action_topic=None), transitions={ 'planned': 'ExecTraj', 'failed': 'failed', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Full, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'action_topic': 'action_topic', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values', 'joint_trajectory': 'joint_trajectory' }) # x:461 y:479 OperatableStateMachine.add('GoalTolerances', SetJointTrajectoryTolerancesState( position_constraints=[0.05], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'PathTolerances', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_goal_tolerances' }) # x:544 y:189 OperatableStateMachine.add('PathTolerances', SetJointTrajectoryTolerancesState( position_constraints=[0.08], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'DumpGoalTolerance', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_path_tolerances' }) # x:389 y:34 OperatableStateMachine.add( 'DumpGoalTolerance', LogKeyState(text="Goal tolerance {}", severity=Logger.REPORT_HINT), transitions={'done': 'DumpPathTolerance'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_goal_tolerances'}) # x:653 y:50 OperatableStateMachine.add( 'DumpPathTolerance', LogKeyState(text="Path tolerance {}", severity=Logger.REPORT_HINT), transitions={'done': 'Plan'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_path_tolerances'}) # x:760 y:584 OperatableStateMachine.add('Home', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'Plan', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group', 'config_name': 'config_home', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:551 y:799 OperatableStateMachine.add( 'Decision', OperatorDecisionState(outcomes=["home", "vertical", "stop"], hint=None, suggestion=None), transitions={ 'home': 'Home', 'vertical': 'Vertical', 'stop': 'finished' }, autonomy={ 'home': Autonomy.Off, 'vertical': Autonomy.Off, 'stop': Autonomy.Full }) # x:1085 y:601 OperatableStateMachine.add('ExecTraj', ExecuteKnownTrajectoryState( timeout=3.0, max_delay=5.0, wait_duration=0.25, action_topic="/execute_trajectory"), transitions={ 'done': 'Decision', 'failed': 'failed', 'param_error': 'ErrLog' }, autonomy={ 'done': Autonomy.High, 'failed': Autonomy.High, 'param_error': Autonomy.Off }, remapping={ 'action_topic': 'action_topic', 'trajectory': 'joint_trajectory', 'status_text': 'status_text', 'goal_names': 'goal_names', 'goal_values': 'goal_values' }) # x:999 y:715 OperatableStateMachine.add('ErrLog', LogState( text="Param error on exec traj", severity=Logger.REPORT_HINT), transitions={'done': 'StatusLog'}, autonomy={'done': Autonomy.Off}) # x:897 y:761 OperatableStateMachine.add('StatusLog', LogKeyState( text=" Status {}", severity=Logger.REPORT_HINT), transitions={'done': 'Decision'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'status_text'}) return _state_machine
def create(self): session_id = None wait_for_question = 25 wait_for_utter = 15 intents = ['YesNo', 'Complete'] answer_key = 'confirm' detail_levels = 'low' # x:73 y:608, x:539 y:365, x:575 y:287 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'max_retry'], output_keys=['answer']) _state_machine.userdata.question = 'Please, is the gripper complete?' _state_machine.userdata.retry = 2 _state_machine.userdata.answer = '' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:373, x:385 y:382 _sm_continueretry_0 = OperatableStateMachine(outcomes=['true', 'false'], input_keys=['retry'], output_keys=['retry']) with _sm_continueretry_0: # x:158 y:82 OperatableStateMachine.add('decrease_retry', CalculationState(calculation=lambda x: x-1), transitions={'done': 'continue_asking'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'retry', 'output_value': 'retry'}) # x:156 y:221 OperatableStateMachine.add('continue_asking', CheckConditionState(predicate=lambda x: x > 0), transitions={'true': 'true', 'false': 'false'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'retry'}) with _state_machine: # x:416 y:26 OperatableStateMachine.add('AskForComplete', LisaUtterAndWaitForIntentState(context_id=session_id, intents=intents, wait_time=wait_for_question), transitions={'intent_recognized': 'Recognized', 'intent_not_recognized': 'NotRecognized', 'preempt': 'failed', 'timeouted': 'failed', 'error': 'failed'}, autonomy={'intent_recognized': Autonomy.Off, 'intent_not_recognized': Autonomy.Off, 'preempt': Autonomy.Off, 'timeouted': Autonomy.Off, 'error': Autonomy.Off}, remapping={'text_to_utter': 'question', 'payload': 'payload', 'original_sentence': 'original_sentence', 'error_reason': 'error_reason', 'intent_recognized': 'intent_recognized'}) # x:690 y:190 OperatableStateMachine.add('ContinueRetry', _sm_continueretry_0, transitions={'true': 'log_retry_value', 'false': 'max_retry'}, autonomy={'true': Autonomy.Inherit, 'false': Autonomy.Inherit}, remapping={'retry': 'retry'}) # x:52 y:390 OperatableStateMachine.add('GetAnswerKey', LisaGetPayloadKeyState(payload_key=answer_key), transitions={'done': 'log_output', 'error': 'failed'}, autonomy={'done': Autonomy.Off, 'error': Autonomy.Off}, remapping={'payload': 'payload', 'payload_value': 'answer'}) # x:783 y:27 OperatableStateMachine.add('NotRecognized', LisaRecognitionResultToStringState(context_id=session_id, detail_levels=detail_levels), transitions={'done': 'UtterNotRecognizedText'}, autonomy={'done': Autonomy.Off}, remapping={'payload': 'payload', 'original_sentence': 'original_sentence', 'error_reason': 'error_reason', 'intent_recognized': 'intent_recognized', 'text_to_utter': 'text_to_utter'}) # x:42 y:122 OperatableStateMachine.add('Recognized', LisaRecognitionResultToStringState(context_id=session_id, detail_levels=detail_levels), transitions={'done': 'GetAnswerKey'}, autonomy={'done': Autonomy.Off}, remapping={'payload': 'payload', 'original_sentence': 'original_sentence', 'error_reason': 'error_reason', 'intent_recognized': 'intent_recognized', 'text_to_utter': 'text_to_utter'}) # x:878 y:289 OperatableStateMachine.add('UtterNotRecognizedText', LisaUtterState(context_id=session_id, wait_time=wait_for_utter, suspend_time=1), transitions={'done': 'ContinueRetry', 'preempt': 'failed', 'timeouted': 'failed', 'error': 'failed'}, autonomy={'done': Autonomy.Off, 'preempt': Autonomy.Off, 'timeouted': Autonomy.Off, 'error': Autonomy.Off}, remapping={'text_to_utter': 'text_to_utter', 'error_reason': 'error_reason'}) # x:278 y:216 OperatableStateMachine.add('UtterRecognized', LisaUtterState(context_id=session_id, wait_time=wait_for_utter, suspend_time=1), transitions={'done': 'GetAnswerKey', 'preempt': 'failed', 'timeouted': 'failed', 'error': 'failed'}, autonomy={'done': Autonomy.Off, 'preempt': Autonomy.Off, 'timeouted': Autonomy.Off, 'error': Autonomy.Off}, remapping={'text_to_utter': 'text_to_utter', 'error_reason': 'error_reason'}) # x:74 y:506 OperatableStateMachine.add('log_output', LogKeyState(text="GC: exit with answer: {}", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'answer'}) # x:646 y:73 OperatableStateMachine.add('log_retry_value', LogKeyState(text="GC: retry level is {}", severity=Logger.REPORT_HINT), transitions={'done': 'AskForComplete'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'retry'}) return _state_machine
def create(self): # x:942 y:70, x:835 y:318 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.JV = [ 5.034934007384111, 4.749860722186541, 2.0863536874817012, 3.548160146665105, -4.830528404237258, -3.8607153900995290 ] _state_machine.userdata.JN = [ "m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3", "m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6" ] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:142 y:64 OperatableStateMachine.add( 'Get_Values', GetJointValuesState( joints=[ "m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3", "m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6" ], topic_name="/m1n6s200_driver/joint_states"), transitions={'retrieved': 'Log_Values'}, autonomy={'retrieved': Autonomy.Off}, remapping={'joint_values': 'joint_values'}) # x:394 y:102 OperatableStateMachine.add('Log_Values', LogKeyState( text='%d', severity=Logger.REPORT_HINT), transitions={'done': 'Go_Home'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_values'}) # x:677 y:64 OperatableStateMachine.add('Set_Specific_Vals', MoveitToJointsDynState( move_group="arm", action_topic='/move_group'), transitions={ 'reached': 'finished', 'planning_failed': 'failed', 'control_failed': 'failed' }, autonomy={ 'reached': Autonomy.Low, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off }, remapping={ 'joint_values': 'JV', 'joint_names': 'JN' }) # x:505 y:209 OperatableStateMachine.add('Go_Home', SrdfStateToMoveit( config_name="Home", move_group="arm", action_topic='/move_group', robot_name="m1n6s200"), transitions={ 'reached': 'Set_Specific_Vals', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Low, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) 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:126 y:194 OperatableStateMachine.add('movePlace', MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15), transitions={ 'done': 'done', 'failed': 'done' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'GripPose'}) # x:30 y:324, x:130 y:324 _sm_retreate_arm_3 = OperatableStateMachine( outcomes=['failed', 'done'], input_keys=['approach_pose', 'PreGripPose']) with _sm_retreate_arm_3: # x:30 y:40 OperatableStateMachine.add('ReturnApproachPose', MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15), transitions={ 'done': 'ReturnPreGrip', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'approach_pose'}) # x:202 y:41 OperatableStateMachine.add('ReturnPreGrip', MoveitMove(move=True, waitForExecution=False, group="RightArm", watchdog=15), transitions={ 'done': 'done', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'PreGripPose'}) # x:30 y:324, x:130 y:324 _sm_releasing_4 = OperatableStateMachine( outcomes=['object', 'no_object']) with _sm_releasing_4: # 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_5 = OperatableStateMachine(outcomes=['arrived', 'failed']) with _sm_moveback_5: # 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_6 = OperatableStateMachine( outcomes=['failed', 'done'], input_keys=['pos'], output_keys=['approach_pose', 'grip_pose']) with _sm_prepare_grip_6: # 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:324, x:130 y:324 _sm_pregrippose_7 = OperatableStateMachine(outcomes=['done', 'failed'], output_keys=['PreGripPose']) with _sm_pregrippose_7: # x:30 y:40 OperatableStateMachine.add('setPreGripPose', SetKey(Value="PrePlacePose"), transitions={'done': 'gotoPreGrip'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'PreGripPose'}) # x:32 y:106 OperatableStateMachine.add('gotoPreGrip', MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15), transitions={ 'done': 'done', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'PreGripPose'}) # x:30 y:458, x:130 y:458, x:230 y:458 _sm_get_down_8 = ConcurrencyContainer( outcomes=['done'], input_keys=['GripPose'], conditions=[('done', [('Go down', 'done')]), ('done', [('read torque', 'done')])]) with _sm_get_down_8: # 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_9 = OperatableStateMachine(outcomes=['fail', 'done'], input_keys=['pos'], output_keys=['pos']) with _sm_pretraitement_9: # 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_9, transitions={ 'fail': 'failed', 'done': 'PreGripPose' }, 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_8, 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:160 y:127 OperatableStateMachine.add( 'PreGripPose', _sm_pregrippose_7, transitions={ 'done': 'Prepare grip', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'PreGripPose': 'PreGripPose'}) # x:156 y:238 OperatableStateMachine.add('Prepare grip', _sm_prepare_grip_6, 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_5, transitions={ 'arrived': 'close gripper', 'failed': 'failed' }, autonomy={ 'arrived': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:298 y:520 OperatableStateMachine.add('releasing', _sm_releasing_4, transitions={ 'object': 'retreate arm', 'no_object': 'retreate arm' }, autonomy={ 'object': Autonomy.Inherit, 'no_object': Autonomy.Inherit }) # x:459 y:522 OperatableStateMachine.add('retreate arm', _sm_retreate_arm_3, transitions={ 'failed': 'failed', 'done': 'Moveback' }, autonomy={ 'failed': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'approach_pose': 'approach_pose', 'PreGripPose': 'PreGripPose' }) return _state_machine
def create(self): exec_speed = 0.80 #was scary at 0.35 exec_acceleration = 1.00 # was scary at 0.40 coll_threshold = 10 # was 14 with CVL at first, then 12 before switch to D435 coll_repeats = 0 utter_incomplete = 'The gripper is not finished' utter_complete = 'The gripper is finished, remove from the workspace' utter_next_screw = 'move to next screw in the same gripper' utter_unknown = "Gripper is in an unknwon state" # x:1105 y:39, x:24 y:178 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.eSELECT_AVAILABLE_TRAJ = 12 _state_machine.userdata.eEXECUTE_FW = 1 _state_machine.userdata.eREAD_FROM_DISK = 6 _state_machine.userdata.eEXECUTE_BW = 2 _state_machine.userdata.ZERO = 0 _state_machine.userdata.endeffector_frame = 'tool_screwdriver_tip_link' _state_machine.userdata.reference_frame = 'base_link' _state_machine.userdata.tf_frame = 'screw_' _state_machine.userdata.velocity = 1.0 _state_machine.userdata.target_offset = [] _state_machine.userdata.offset_position_to_screw = [0.0, 0.0, 0.003] _state_machine.userdata.offset_position_from_screw = [0.0, 0.0, -0.022] _state_machine.userdata.offset_orientation_jiggle = [ 0.0, 0.0, 0.2, 1.0 ] _state_machine.userdata.no_offset_position = [0.0, 0.0, 0, 0] _state_machine.userdata.no_offset_orientation = [0.0, 0.0, 0.0, 1.0] _state_machine.userdata.eDRIVE_TO_START = 11 _state_machine.userdata.start_step = 0 _state_machine.userdata.eSELECT_AVAILABLE_REPLAN_TRAJ = 14 _state_machine.userdata.eEXECUTE_REPLAN_TRAJ = 15 _state_machine.userdata.eEXECUTE_BW_FROM_STEP = 17 _state_machine.userdata.eEXECUTE_FW_FROM_STEP = 16 _state_machine.userdata.eCREATE_NEW_TRAJ = 19 _state_machine.userdata.eEXECUTE_NEW_TRAJ = 20 _state_machine.userdata.next_trajectory = 0 _state_machine.userdata.list_available_screw_poses = [] _state_machine.userdata.use_same_gripper = False _state_machine.userdata.same_gripper_retry = 3 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:727 y:338, x:493 y:611 _sm_search_and_move_to_next_screw_0 = OperatableStateMachine( outcomes=['error', 'moved_to_next_screw'], input_keys=[ 'eEXECUTE_NEW_TRAJ', 'eCREATE_NEW_TRAJ', 'next_trajectory', 'list_available_screw_poses', 'use_same_gripper', 'same_gripper_retry' ], output_keys=['next_trajectory']) with _sm_search_and_move_to_next_screw_0: # x:92 y:22 OperatableStateMachine.add( 'use_same_gripper', CheckConditionState(predicate=lambda x: x == True), transitions={ 'true': 'log_use_same_grip', 'false': 'log_use_another_grip' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'use_same_gripper'}) # x:10 y:244 OperatableStateMachine.add( 'Available Screws Same Gripper', PartAvailablePoseService(check_only_current_gripper=True), transitions={ 'succeeded': 'Find Collision Free Trajectory', 'aborted': 'decrese_retry_same_gripper' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'last_trajectory_id': 'next_trajectory', 'list_available_parts': 'list_available_screw_poses' }) # x:51 y:484 OperatableStateMachine.add( 'Check_retry', CheckConditionState(predicate=lambda x: x > 0), transitions={ 'true': 'WaitForScrews', 'false': 'set_another_gripper' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'same_gripper_retry'}) # x:1035 y:602 OperatableStateMachine.add('Execute New Trajectory', SharedWsActionState( exec_speed=exec_speed, exec_acceleration=exec_acceleration, coll_threshold=coll_threshold, coll_repeats=coll_repeats), transitions={ 'succeeded': 'set_retry', 'preempted': 'error', 'aborted': 'utter new traj aborted' }, autonomy={ 'succeeded': Autonomy.Off, 'preempted': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'action_id': 'eEXECUTE_NEW_TRAJ', 'trajectory_id': 'next_trajectory', 'results': 'results' }) # x:597 y:178 OperatableStateMachine.add( 'Find Collision Free Trajectory', SharedWsNewTrajActionState(exec_speed=exec_speed, exec_acceleration=exec_acceleration, coll_threshold=coll_threshold, coll_repeats=coll_repeats), transitions={ 'succeeded': 'Log_next_traj_id', 'preempted': 'error', 'aborted': 'utter obstacle' }, autonomy={ 'succeeded': Autonomy.Off, 'preempted': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'action_id': 'eCREATE_NEW_TRAJ', 'trajectory_id': 'next_trajectory', 'list_available_screw_poses': 'list_available_screw_poses', 'results': 'results', 'next_trajectory': 'next_trajectory' }) # x:1081 y:148 OperatableStateMachine.add( 'Log_next_traj_id', LogKeyState(text="MAIN: Next Free Tjact. ID is for screw : {}", severity=Logger.REPORT_HINT), transitions={'done': 'Execute New Trajectory'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'next_trajectory'}) # x:1268 y:2 OperatableStateMachine.add( 'Sleep Until Retry', WaitState(wait_time=2), transitions={'done': 'use_same_gripper'}, autonomy={'done': Autonomy.Off}) # x:316 y:138 OperatableStateMachine.add( 'WaitForScrews', WaitState(wait_time=1), transitions={'done': 'use_same_gripper'}, autonomy={'done': Autonomy.Off}) # x:11 y:385 OperatableStateMachine.add( 'decrese_retry_same_gripper', CalculationState(calculation=lambda x: x - 1), transitions={'done': 'Check_retry'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'same_gripper_retry', 'output_value': 'same_gripper_retry' }) # x:339 y:50 OperatableStateMachine.add( 'log_use_another_grip', LogState(text="SMNS: use another gripper", severity=Logger.REPORT_HINT), transitions={'done': 'Available Screws Request'}, autonomy={'done': Autonomy.Off}) # x:54 y:117 OperatableStateMachine.add( 'log_use_same_grip', LogState(text="SMNS: use same gripper", severity=Logger.REPORT_HINT), transitions={'done': 'Available Screws Same Gripper'}, autonomy={'done': Autonomy.Off}) # x:98 y:633 OperatableStateMachine.add( 'set_another_gripper', CalculationState(calculation=lambda x: False), transitions={'done': 'utter_change gripper'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'use_same_gripper', 'output_value': 'use_same_gripper' }) # x:756 y:596 OperatableStateMachine.add( 'set_retry', CalculationState(calculation=lambda x: 3), transitions={'done': 'moved_to_next_screw'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'same_gripper_retry', 'output_value': 'same_gripper_retry' }) # x:1240 y:366 OperatableStateMachine.add( 'utter new traj aborted', LisaUtterActionState( text_to_utter="Trajectory execution was aborted", wait_time=0), transitions={ 'uttered_all': 'Sleep Until Retry', 'timeout': 'Sleep Until Retry', 'command_error': 'error' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:626 y:524 OperatableStateMachine.add( 'utter obstacle', LisaUtterActionState( text_to_utter= "Part available but no free path to move on it", wait_time=0), transitions={ 'uttered_all': 'WaitForScrews', 'timeout': 'WaitForScrews', 'command_error': 'error' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:288 y:377 OperatableStateMachine.add( 'utter_change gripper', LisaUtterActionState(text_to_utter="Change to another gripper", wait_time=0), transitions={ 'uttered_all': 'WaitForScrews', 'timeout': 'WaitForScrews', 'command_error': 'error' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:628 y:51 OperatableStateMachine.add( 'Available Screws Request', PartAvailablePoseService(check_only_current_gripper=False), transitions={ 'succeeded': 'Find Collision Free Trajectory', 'aborted': 'WaitForScrews' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'last_trajectory_id': 'next_trajectory', 'list_available_parts': 'list_available_screw_poses' }) # x:30 y:538, x:686 y:404, x:230 y:538 _sm_utter_wrong_insertion_1 = OperatableStateMachine( outcomes=['error', 'retry', 'skip'], input_keys=['next_trajectory']) with _sm_utter_wrong_insertion_1: # x:30 y:113 OperatableStateMachine.add( 'utter_not_inserted_text', LisaMotekConcatenateScrewPartString( text_format= "{}, was not inserted, would you like to: retry or, skip?", number_part_screws=4), transitions={'done': 'ask_retry'}, autonomy={'done': Autonomy.Off}, remapping={ 'screw_id': 'next_trajectory', 'text_to_utter': 'text_to_utter' }) # x:619 y:277 OperatableStateMachine.add( 'check_retry', CheckConditionState(predicate=lambda x: x == 'retry'), transitions={ 'true': 'retry', 'false': 'skip' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'intent_recognized'}) # x:392 y:173 OperatableStateMachine.add( 'ask_retry', LisaUtterAndWaitForIntentState(context_id=None, intents=['retry', 'skip'], wait_time=0), transitions={ 'intent_recognized': 'check_retry', 'intent_not_recognized': 'skip', 'preempt': 'skip', 'timeouted': 'skip', 'error': 'error' }, autonomy={ 'intent_recognized': Autonomy.Off, 'intent_not_recognized': Autonomy.Off, 'preempt': Autonomy.Off, 'timeouted': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'text_to_utter': 'text_to_utter', 'payload': 'payload', 'original_sentence': 'original_sentence', 'error_reason': 'error_reason', 'intent_recognized': 'intent_recognized' }) # x:611 y:158 _sm_log_screws_2 = OperatableStateMachine(outcomes=['done'], input_keys=[ 'screw_found', 'insertion_successful', 'next_trajectory' ]) with _sm_log_screws_2: # x:79 y:175 OperatableStateMachine.add( 'log_screw_id', LogKeyState(text="GCE-ENTER: insterd screw id {}", severity=Logger.REPORT_HINT), transitions={'done': 'log_last_insertion'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'next_trajectory'}) # x:423 y:441 OperatableStateMachine.add( 'log_screw_found', LogKeyState(text="GCE-ENTER: screw was found ? {}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'screw_found'}) # x:217 y:276 OperatableStateMachine.add( 'log_last_insertion', LogKeyState(text="GCE-ENTER: last insertion was succesfu? {}", severity=Logger.REPORT_HINT), transitions={'done': 'log_screw_found'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'insertion_successful'}) # x:422 y:304, x:170 y:189 _sm_fix_for_screwed_but_not_found_3 = OperatableStateMachine( outcomes=['found_and_inserted', 'not_inserted'], input_keys=['screw_found', 'screw_inserted'], output_keys=['screw_found']) with _sm_fix_for_screwed_but_not_found_3: # x:156 y:37 OperatableStateMachine.add( 'Check is Inserted', CheckConditionState(predicate=lambda x: x == True), transitions={ 'true': 'check is found', 'false': 'not_inserted' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'screw_inserted'}) # x:287 y:127 OperatableStateMachine.add( 'check is found', CheckConditionState(predicate=lambda x: x == True), transitions={ 'true': 'found_and_inserted', 'false': 'fix for inserted not found' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'screw_found'}) # x:511 y:219 OperatableStateMachine.add( 'fix for inserted not found', CalculationState(calculation=lambda x: True), transitions={'done': 'found_and_inserted'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'screw_found', 'output_value': 'screw_found' }) # x:959 y:321, x:1285 y:172, x:1810 y:717, x:716 y:636, x:900 y:779 _sm_gripper_ended_check_4 = OperatableStateMachine( outcomes=[ 'fail', 'completed', 'screw_available', 'incomplete', 'unknown' ], input_keys=[ 'next_trajectory', 'insertion_successful', 'screw_found', 'use_same_gripper' ], output_keys=['list_available_screw_poses', 'use_same_gripper']) with _sm_gripper_ended_check_4: # x:58 y:31 OperatableStateMachine.add('Fix for screwed but not found', _sm_fix_for_screwed_but_not_found_3, transitions={ 'found_and_inserted': 'Update Screw State', 'not_inserted': 'Utter Wrong Insertion' }, autonomy={ 'found_and_inserted': Autonomy.Inherit, 'not_inserted': Autonomy.Inherit }, remapping={ 'screw_found': 'screw_found', 'screw_inserted': 'insertion_successful' }) # x:1696 y:24 OperatableStateMachine.add( 'Available Screw In current Gripper', PartAvailablePoseService(check_only_current_gripper=True), transitions={ 'succeeded': 'utter_next_traj', 'aborted': 'Ask for Gripper Complete' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'last_trajectory_id': 'next_trajectory', 'list_available_parts': 'list_available_screw_poses' }) # x:92 y:255 OperatableStateMachine.add( 'Log screws', _sm_log_screws_2, transitions={'done': 'Fix for screwed but not found'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'screw_found': 'screw_found', 'insertion_successful': 'insertion_successful', 'next_trajectory': 'next_trajectory' }) # x:318 y:69 OperatableStateMachine.add('Update Screw State', SetScrewServiceState(), transitions={ 'succeeded': 'wait_screw_update', 'aborted': 'log_exit_1' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'screw_id': 'next_trajectory', 'finished_successful': 'insertion_successful', 'screw_found': 'screw_found', 'result': 'result' }) # x:300 y:222 OperatableStateMachine.add( 'Utter Wrong Insertion', _sm_utter_wrong_insertion_1, transitions={ 'error': 'fail', 'retry': 'utter_retry', 'skip': 'Update Screw State' }, autonomy={ 'error': Autonomy.Inherit, 'retry': Autonomy.Inherit, 'skip': Autonomy.Inherit }, remapping={'next_trajectory': 'next_trajectory'}) # x:440 y:334 OperatableStateMachine.add( 'aaaa', WaitState(wait_time=3), transitions={'done': 'is_gripper_terminated'}, autonomy={'done': Autonomy.Off}) # x:1536 y:503 OperatableStateMachine.add( 'check answer', CheckConditionState( predicate=lambda x: x == "yes" or x == "completed"), transitions={ 'true': 'utter_completed', 'false': 'reset_gripper' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'answer'}) # x:839 y:534 OperatableStateMachine.add( 'exit_reset', LogState(text="DBG-EXIT: incomplete, gripper resetted", severity=Logger.REPORT_HINT), transitions={'done': 'utter_exit_not_complete'}, autonomy={'done': Autonomy.Off}) # x:1234 y:742 OperatableStateMachine.add( 'exit_unknwon', LogState(text="DBG-EXIT: unknwon, exit NOT resetted", severity=Logger.REPORT_HINT), transitions={'done': 'utter_unknown'}, autonomy={'done': Autonomy.Off}) # x:691 y:11 OperatableStateMachine.add( 'get_screw_in_gripper', GripperStateListener(input_id_is_screw=True, time_out=1.0, number_part_screws=4), transitions={ 'succeeded': 'aaaa', 'aborted': 'log_err_screws' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'selected_gripper_id': 'next_trajectory', 'gripper_status': 'gripper_status', 'list_gripper_screws_status': 'list_gripper_screws_status', 'gripper_id': 'gripper_id', 'gripper_name': 'gripper_name', 'error_reason': 'error_reason' }) # x:991 y:9 OperatableStateMachine.add( 'is_gripper_terminated', CheckGripperIsFinished(number_part_screws=4), transitions={ 'done': 'set_use_another_gripper', 'screws_available': 'set_use_same_gripper', 'operator_check': 'Ask for Gripper Complete', 'error': 'fail' }, autonomy={ 'done': Autonomy.Off, 'screws_available': Autonomy.Off, 'operator_check': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'gripper_id': 'gripper_id', 'screw_status_list': 'list_gripper_screws_status' }) # x:1706 y:508 OperatableStateMachine.add('log_answer', LogKeyState( text="GCE: User reply {}", severity=Logger.REPORT_HINT), transitions={'done': 'check answer'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'answer'}) # x:790 y:173 OperatableStateMachine.add( 'log_err_screws', LogKeyState(text="GEC: Fail with available screws: {}", severity=Logger.REPORT_HINT), transitions={'done': 'fail'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'error_reason'}) # x:677 y:174 OperatableStateMachine.add( 'log_exit_1', LogState(text="GEC: Updtae Screw State FAIL", severity=Logger.REPORT_HINT), transitions={'done': 'fail'}, autonomy={'done': Autonomy.Off}) # x:1071 y:276 OperatableStateMachine.add( 'log_exit_failed', LogKeyState(text="GCE: exit failed from Ask for gripper: {}", severity=Logger.REPORT_HINT), transitions={'done': 'fail'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'answer'}) # x:1464 y:257 OperatableStateMachine.add( 'reset_gripper', ResetScrewStatesInGripper(number_part_screws=4), transitions={ 'succeeded': 'exit_reset', 'aborted': 'fail', 'nothing_to_do': 'utter_completed' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off, 'nothing_to_do': Autonomy.Off }, remapping={ 'screw_id': 'next_trajectory', 'screw_status_list': 'list_gripper_screws_status', 'result': 'result', 'reason': 'reason' }) # x:1044 y:92 OperatableStateMachine.add( 'set_use_another_gripper', CalculationState(calculation=lambda x: False), transitions={'done': 'utter_completed'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'use_same_gripper', 'output_value': 'use_same_gripper' }) # x:1419 y:5 OperatableStateMachine.add( 'set_use_same_gripper', CalculationState(calculation=lambda x: True), transitions={'done': 'Available Screw In current Gripper'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'use_same_gripper', 'output_value': 'use_same_gripper' }) # x:1034 y:174 OperatableStateMachine.add( 'utter_completed', LisaUtterActionState(text_to_utter=utter_complete, wait_time=0), transitions={ 'uttered_all': 'completed', 'timeout': 'completed', 'command_error': 'fail' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:397 y:535 OperatableStateMachine.add( 'utter_exit_not_complete', LisaUtterActionState(text_to_utter=utter_incomplete, wait_time=0), transitions={ 'uttered_all': 'incomplete', 'timeout': 'incomplete', 'command_error': 'fail' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:1724 y:586 OperatableStateMachine.add( 'utter_next_traj', LisaUtterActionState(text_to_utter=utter_next_screw, wait_time=0), transitions={ 'uttered_all': 'screw_available', 'timeout': 'screw_available', 'command_error': 'fail' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:1474 y:671 OperatableStateMachine.add( 'utter_retry', LisaUtterActionState(text_to_utter="retry the same screw", wait_time=0), transitions={ 'uttered_all': 'screw_available', 'timeout': 'screw_available', 'command_error': 'fail' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:1020 y:745 OperatableStateMachine.add( 'utter_unknown', LisaUtterActionState(text_to_utter=utter_unknown, wait_time=0), transitions={ 'uttered_all': 'unknown', 'timeout': 'unknown', 'command_error': 'fail' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:504 y:2 OperatableStateMachine.add( 'wait_screw_update', WaitState(wait_time=2), transitions={'done': 'get_screw_in_gripper'}, autonomy={'done': Autonomy.Off}) # x:1526 y:155 OperatableStateMachine.add( 'Ask for Gripper Complete', self.use_behavior( GripperCompleteSM, 'Gripper Ended Check/Ask for Gripper Complete'), transitions={ 'finished': 'log_answer', 'failed': 'log_exit_failed', 'max_retry': 'exit_unknwon' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'max_retry': Autonomy.Inherit }, remapping={'answer': 'answer'}) with _state_machine: # x:112 y:30 OperatableStateMachine.add('Load Trajectory', SharedWsActionState( exec_speed=exec_speed, exec_acceleration=exec_acceleration, coll_threshold=coll_threshold, coll_repeats=coll_repeats), transitions={ 'succeeded': 'Move To Home', 'preempted': 'failed', 'aborted': 'failed' }, autonomy={ 'succeeded': Autonomy.Off, 'preempted': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'action_id': 'eREAD_FROM_DISK', 'trajectory_id': 'ZERO', 'results': 'results' }) # x:890 y:186 OperatableStateMachine.add( 'MotekInsertScrew', self.use_behavior(MotekInsertScrewSM, 'MotekInsertScrew'), transitions={ 'finished': 'Gripper Ended Check', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'velocity': 'velocity', 'insertion_successful': 'insertion_successful', 'screw_found': 'screw_found' }) # x:334 y:30 OperatableStateMachine.add( 'Move To Home', SharedWsActionState(exec_speed=exec_speed, exec_acceleration=exec_acceleration, coll_threshold=coll_threshold, coll_repeats=coll_repeats), transitions={ 'succeeded': 'search and move to next screw', 'preempted': 'failed', 'aborted': 'failed' }, autonomy={ 'succeeded': Autonomy.Off, 'preempted': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'action_id': 'eDRIVE_TO_START', 'trajectory_id': 'ZERO', 'results': 'results' }) # x:756 y:374 OperatableStateMachine.add( 'log_avail_screws', LogKeyState(text="MAIN: available screws \n{}", severity=Logger.REPORT_HINT), transitions={'done': 'Gripper Ended Check'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'list_available_screw_poses'}) # x:237 y:214 OperatableStateMachine.add( 'log_completed', LogKeyState(text="MAIN: Gripper complete, use_same_gripper={}", severity=Logger.REPORT_HINT), transitions={'done': 'Move To Home'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'use_same_gripper'}) # x:523 y:123 OperatableStateMachine.add( 'log_uncomplete', LogKeyState( text="MAIN: Gripper uncomplete, use_same_gripper={}", severity=Logger.REPORT_HINT), transitions={'done': 'search and move to next screw'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'use_same_gripper'}) # x:385 y:220 OperatableStateMachine.add('log_unknown', LogKeyState( text="gripper is unknown {}", severity=Logger.REPORT_HINT), transitions={'done': 'Move To Home'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'use_same_gripper'}) # x:599 y:216 OperatableStateMachine.add( 'screw_available', LogKeyState( text="Availavke screw same gripper, use_same_gripper={}", severity=Logger.REPORT_HINT), transitions={'done': 'search and move to next screw'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'use_same_gripper'}) # x:716 y:23 OperatableStateMachine.add('search and move to next screw', _sm_search_and_move_to_next_screw_0, transitions={ 'error': 'failed', 'moved_to_next_screw': 'MotekInsertScrew' }, autonomy={ 'error': Autonomy.Inherit, 'moved_to_next_screw': Autonomy.Inherit }, remapping={ 'eEXECUTE_NEW_TRAJ': 'eEXECUTE_NEW_TRAJ', 'eCREATE_NEW_TRAJ': 'eCREATE_NEW_TRAJ', 'next_trajectory': 'next_trajectory', 'list_available_screw_poses': 'list_available_screw_poses', 'use_same_gripper': 'use_same_gripper', 'same_gripper_retry': 'same_gripper_retry' }) # x:362 y:383 OperatableStateMachine.add('Gripper Ended Check', _sm_gripper_ended_check_4, transitions={ 'fail': 'failed', 'completed': 'log_completed', 'screw_available': 'screw_available', 'incomplete': 'log_uncomplete', 'unknown': 'log_unknown' }, autonomy={ 'fail': Autonomy.Inherit, 'completed': Autonomy.Inherit, 'screw_available': Autonomy.Inherit, 'incomplete': Autonomy.Inherit, 'unknown': Autonomy.Inherit }, remapping={ 'next_trajectory': 'next_trajectory', 'insertion_successful': 'insertion_successful', 'screw_found': 'screw_found', 'use_same_gripper': 'use_same_gripper', 'list_available_screw_poses': 'list_available_screw_poses' }) return _state_machine
def create(self): # x:1474 y:331, x:56 y:575 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.half_turn = 3.1416 _state_machine.userdata.person = "person" _state_machine.userdata.operator_param = "behavior/Operaror/Id" _state_machine.userdata.join = ["Move", "spr/initialpose"] _state_machine.userdata.leave = ["Move", "door2/exit"] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:1182 y:163 _sm_rotate_0 = OperatableStateMachine(outcomes=['finished']) with _sm_rotate_0: # x:103 y:61 OperatableStateMachine.add('Look Left', SaraSetHeadAngle(pitch=-0.35, yaw=0.5), transitions={'done': 'Rotate Left'}, autonomy={'done': Autonomy.Off}) # x:794 y:54 OperatableStateMachine.add('Look Right', SaraSetHeadAngle(pitch=-0.35, yaw=-0.5), transitions={'done': 'Rotate Right'}, autonomy={'done': Autonomy.Off}) # x:325 y:61 OperatableStateMachine.add('Rotate Left', WaitState(wait_time=8), transitions={'done': 'Look Center'}, autonomy={'done': Autonomy.Off}) # x:961 y:65 OperatableStateMachine.add('Rotate Right', WaitState(wait_time=4), transitions={'done': 'Look Center 2'}, autonomy={'done': Autonomy.Off}) # x:1115 y:62 OperatableStateMachine.add('Look Center 2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:484 y:54 OperatableStateMachine.add('Look Center', SaraSetHeadAngle(pitch=-0.35, yaw=0), transitions={'done': 'Rotate Center'}, autonomy={'done': Autonomy.Off}) # x:657 y:49 OperatableStateMachine.add('Rotate Center', WaitState(wait_time=4), transitions={'done': 'Look Right'}, autonomy={'done': Autonomy.Off}) # x:30 y:458 _sm_follow_head_1 = OperatableStateMachine(outcomes=['end'], input_keys=['person']) with _sm_follow_head_1: # x:214 y:48 OperatableStateMachine.add('list all entities', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'Get Nearest', 'none_found': 'list all entities' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'person', 'entity_list': 'entity_list', 'number': 'number' }) # x:456 y:51 OperatableStateMachine.add( 'Get Nearest', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'look'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'ID' }) # x:291 y:177 OperatableStateMachine.add( 'look', KeepLookingAt(), transitions={'failed': 'list all entities'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:12 y:125, x:1130 y:515 _sm_nlu_2 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_nlu_2: # x:156 y:37 OperatableStateMachine.add( 'say ask', SaraSay(sentence="You can ask me your questions.", input_keys=[], emotion=1, block=True), transitions={'done': 'Loop Questions'}, autonomy={'done': Autonomy.Off}) # x:1091 y:84 OperatableStateMachine.add('Listen', GetSpeech(watchdog=10), transitions={ 'done': 'Engine', 'nothing': 'Listen', 'fail': 'Listen' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'sentence'}) # x:1324 y:110 OperatableStateMachine.add('Engine', SaraNLUspr(), transitions={ 'understood': 'Say_Answer', 'not_understood': 'Listen', 'fail': 'Listen' }, autonomy={ 'understood': Autonomy.Off, 'not_understood': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'sentence': 'sentence', 'answer': 'answer' }) # x:632 y:77 OperatableStateMachine.add( 'Select Story', CalculationState(calculation=lambda x: x + 6), transitions={'done': 'Set_a_step'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'index', 'output_value': 'story' }) # x:874 y:78 OperatableStateMachine.add('Set_a_step', Set_a_step(step=1), transitions={'done': 'Listen'}, autonomy={'done': Autonomy.Off}) # x:398 y:81 OperatableStateMachine.add('Loop Questions', ForLoop(repeat=5), transitions={ 'do': 'Select Story', 'end': 'loop step' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:439 y:252 OperatableStateMachine.add( 'Say Blind Game', SaraSay(sentence="Let's play the blind game !", input_keys=[], emotion=1, block=True), transitions={'done': 'Loop Questions'}, autonomy={'done': Autonomy.Off}) # x:189 y:193 OperatableStateMachine.add('loop step', ForLoop(repeat=1), transitions={ 'do': 'Say Blind Game', 'end': 'finished' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:721 y:205 OperatableStateMachine.add('Say_Answer', SaraSay(sentence=lambda x: str(x), input_keys=[], emotion=0, block=True), transitions={'done': 'Loop Questions'}, autonomy={'done': Autonomy.Off}) # x:817 y:123, x:130 y:458 _sm_analyse_crowd_3 = OperatableStateMachine( outcomes=['finished', 'error']) with _sm_analyse_crowd_3: # x:87 y:97 OperatableStateMachine.add('clear database', WonderlandClearPeoples(), transitions={ 'done': 'Rotate', 'error': 'error' }, autonomy={ 'done': Autonomy.Off, 'error': Autonomy.Off }) # x:531 y:112 OperatableStateMachine.add('Add Update Persons', WonderlandAddUpdatePeople(), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:311 y:97 OperatableStateMachine.add( 'Rotate', _sm_rotate_0, transitions={'finished': 'Add Update Persons'}, autonomy={'finished': Autonomy.Inherit}) # x:1203 y:11, x:1006 y:366 _sm_init_scenario_4 = OperatableStateMachine( outcomes=['done', 'error']) with _sm_init_scenario_4: # x:30 y:42 OperatableStateMachine.add( 'Generate Vizbox Story', Set_Story(titre="Speech and Person Recognition", storyline=[ "Waiting Begining", "Join Game Room", "Waiting Crowd Placement", "Analysing Crowd", "Begin Game", "Find Operator", "Question 1", "Question 2", "Question 3", "Question 4", "Question 5", "Leave Arena" ]), transitions={'done': 'Set Story Step'}, autonomy={'done': Autonomy.Off}) # x:559 y:44 OperatableStateMachine.add('WaitForBegining', ContinueButton(), transitions={ 'true': 'Reset Persons', 'false': 'Reset Persons' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }) # x:807 y:61 OperatableStateMachine.add('Reset Persons', WonderlandClearPeoples(), transitions={ 'done': 'done', 'error': 'error' }, autonomy={ 'done': Autonomy.Off, 'error': Autonomy.Off }) # x:247 y:49 OperatableStateMachine.add('Set Story Step', Set_a_step(step=0), transitions={'done': 'setIDLE'}, autonomy={'done': Autonomy.Off}) # x:388 y:208 OperatableStateMachine.add('Reset Arm', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'Reset Persons', 'failed': 'error' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'target'}) # x:427 y:40 OperatableStateMachine.add('setIDLE', SetKey(Value="IdlePose"), transitions={'done': 'Reset Arm'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:30 y:458, x:230 y:458 _sm_join_area_5 = OperatableStateMachine( outcomes=['failed', 'finished'], input_keys=['join']) with _sm_join_area_5: # x:95 y:40 OperatableStateMachine.add( 'Say Join Area', SaraSay(sentence="I will join the playing room !", input_keys=[], emotion=1, block=True), transitions={'done': 'Join Arena'}, autonomy={'done': Autonomy.Off}) # x:92 y:134 OperatableStateMachine.add( 'Join Arena', self.use_behavior(sara_flexbe_behaviors__ActionWrapper_MoveSM, 'Join Area/Join Arena'), transitions={ 'finished': 'finished', 'failed': 'failed', 'critical_fail': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit }, remapping={'Action': 'join'}) # x:489 y:56, x:604 y:278 _sm_waiting_and_turn_6 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['half_turn']) with _sm_waiting_and_turn_6: # x:50 y:51 OperatableStateMachine.add( 'Want Play', SaraSay(sentence="Hum, I want to play riddles !", input_keys=[], emotion=1, block=True), transitions={'done': 'Wait 10s'}, autonomy={'done': Autonomy.Off}) # x:272 y:121 OperatableStateMachine.add( 'action_turn', self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'Waiting And Turn/action_turn'), transitions={ 'finished': 'finished', 'failed': 'Cant turn' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'half_turn'}) # x:437 y:240 OperatableStateMachine.add('Cant turn', SaraSay(sentence="I can't turn !", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:63 y:178 OperatableStateMachine.add( 'Wait 10s', WaitState(wait_time=10), transitions={'done': 'Look In Front Of'}, autonomy={'done': Autonomy.Off}) # x:61 y:260 OperatableStateMachine.add('Look In Front Of', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}) # x:472 y:69, x:476 y:113, x:470 y:196, x:330 y:458, x:430 y:458 _sm_questions_7 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['person'], conditions=[('finished', [('NLU', 'finished')]), ('failed', [('NLU', 'failed')]), ('finished', [('Follow Head', 'end')])]) with _sm_questions_7: # x:85 y:58 OperatableStateMachine.add('NLU', _sm_nlu_2, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:84 y:185 OperatableStateMachine.add('Follow Head', _sm_follow_head_1, transitions={'end': 'finished'}, autonomy={'end': Autonomy.Inherit}, remapping={'person': 'person'}) # x:283 y:294, x:60 y:571 _sm_find_operator_8 = OperatableStateMachine( outcomes=['not_found', 'done'], input_keys=['person', 'operator_param']) with _sm_find_operator_8: # x:51 y:40 OperatableStateMachine.add( 'Ask Player', SaraSay(sentence="Who wan't to play with me ?", input_keys=[], emotion=1, block=True), transitions={'done': 'Wait Operator'}, autonomy={'done': Autonomy.Off}) # x:39 y:297 OperatableStateMachine.add( 'Get operator id', CalculationState(calculation=lambda x: x.entities[0].face.id), transitions={'done': 'Set Operator Id'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'operator' }) # x:33 y:384 OperatableStateMachine.add('Set Operator Id', SetRosParamKey(), transitions={'done': 'Operator Id'}, autonomy={'done': Autonomy.Off}, remapping={ 'Value': 'operator_param', 'ParamName': 'operator' }) # x:46 y:117 OperatableStateMachine.add('Wait Operator', WaitState(wait_time=6), transitions={'done': 'Find Operator'}, autonomy={'done': Autonomy.Off}) # x:35 y:495 OperatableStateMachine.add('Operator Id', LogKeyState( text="Operator find. Id: {}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'operator'}) # x:30 y:205 OperatableStateMachine.add('Find Operator', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'Get operator id', 'none_found': 'Find Operator' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'person', 'entity_list': 'entity_list', 'number': 'number' }) # x:703 y:198, x:88 y:199 _sm_tell_basic_stats_9 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_tell_basic_stats_9: # x:50 y:40 OperatableStateMachine.add('wait', WaitState(wait_time=0), transitions={'done': 'GetPeopleStats'}, autonomy={'done': Autonomy.Off}) # x:218 y:43 OperatableStateMachine.add('GetPeopleStats', WonderlandGetPersonStat(), transitions={ 'done': 'GenerateSentence', 'none': 'Nobody', 'error': 'failed' }, autonomy={ 'done': Autonomy.Off, 'none': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'women': 'women', 'men': 'men', 'others': 'others' }) # x:466 y:46 OperatableStateMachine.add('Nobody', SaraSay( sentence="There is nobody here !", input_keys=[], emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:162 y:243 OperatableStateMachine.add( 'GenerateSentence', FlexibleCalculationState(calculation=lambda x: "There is " + str(x[0] + x[1] + x[2]) + " persons.", input_keys=['men', 'women', 'others']), transitions={'done': 'Tell_Stats'}, autonomy={'done': Autonomy.Off}, remapping={ 'men': 'men', 'women': 'women', 'others': 'others', 'output_value': 'sentence' }) # x:151 y:345 OperatableStateMachine.add( 'Generate Sentence 2', FlexibleCalculationState( calculation=lambda x: "I recognize " + str(x[ 1]) + " women and " + str(x[0]) + " men.", input_keys=['men', 'women', 'others']), transitions={'done': 'Tell_Stats 2'}, autonomy={'done': Autonomy.Off}, remapping={ 'men': 'men', 'women': 'women', 'others': 'others', 'output_value': 'sentence' }) # x:380 y:172 OperatableStateMachine.add( 'Tell_Stats', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'Generate Sentence 2'}, autonomy={'done': Autonomy.Off}) # x:409 y:276 OperatableStateMachine.add('Tell_Stats 2', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:30 y:40 OperatableStateMachine.add('continue', ContinueButton(), transitions={ 'true': 'Init Scenario', 'false': 'Init Scenario' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }) # x:715 y:127 OperatableStateMachine.add('Tell basic stats', _sm_tell_basic_stats_9, transitions={ 'finished': 'Set Find Operator', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:899 y:126 OperatableStateMachine.add('Find Operator', _sm_find_operator_8, transitions={ 'not_found': 'Find Operator', 'done': 'Questions' }, autonomy={ 'not_found': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'person': 'person', 'operator_param': 'operator_param' }) # x:1120 y:126 OperatableStateMachine.add('Questions', _sm_questions_7, transitions={ 'finished': 'set head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'person': 'person'}) # x:343 y:49 OperatableStateMachine.add( 'Set Story Waiting', Set_a_step(step=2), transitions={'done': 'Waiting And Turn'}, autonomy={'done': Autonomy.Off}) # x:532 y:48 OperatableStateMachine.add('Set Analyse', Set_a_step(step=3), transitions={'done': 'Analyse Crowd'}, autonomy={'done': Autonomy.Off}) # x:696 y:47 OperatableStateMachine.add( 'Set Begin Game', Set_a_step(step=4), transitions={'done': 'Tell basic stats'}, autonomy={'done': Autonomy.Off}) # x:899 y:49 OperatableStateMachine.add('Set Find Operator', Set_a_step(step=5), transitions={'done': 'Find Operator'}, autonomy={'done': Autonomy.Off}) # x:332 y:127 OperatableStateMachine.add('Waiting And Turn', _sm_waiting_and_turn_6, transitions={ 'finished': 'Set Analyse', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'half_turn': 'half_turn'}) # x:1333 y:232 OperatableStateMachine.add('Set Go Out', Set_a_step(step=11), transitions={'done': 'Say And Of Game'}, autonomy={'done': Autonomy.Off}) # x:1517 y:148 OperatableStateMachine.add( 'Leave Arena', self.use_behavior(sara_flexbe_behaviors__ActionWrapper_MoveSM, 'Leave Arena'), transitions={ 'finished': 'finished', 'failed': 'finished', 'critical_fail': 'finished' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit }, remapping={'Action': 'leave'}) # x:176 y:42 OperatableStateMachine.add( 'Set Join', Set_a_step(step=1), transitions={'done': 'Set Story Waiting'}, autonomy={'done': Autonomy.Off}) # x:177 y:120 OperatableStateMachine.add('Join Area', _sm_join_area_5, transitions={ 'failed': 'failed', 'finished': 'Set Story Waiting' }, autonomy={ 'failed': Autonomy.Inherit, 'finished': Autonomy.Inherit }, remapping={'join': 'join'}) # x:1302 y:140 OperatableStateMachine.add( 'Say And Of Game', SaraSay( sentence= "The game is finished. I will leave the arena. Thank you for playing with me.", input_keys=[], emotion=1, block=True), transitions={'done': 'Leave Arena'}, autonomy={'done': Autonomy.Off}) # x:1120 y:237 OperatableStateMachine.add('set head', SaraSetHeadAngle(pitch=-0.2, yaw=0), transitions={'done': 'Set Go Out'}, autonomy={'done': Autonomy.Off}) # x:34 y:130 OperatableStateMachine.add('Init Scenario', _sm_init_scenario_4, transitions={ 'done': 'Set Join', 'error': 'failed' }, autonomy={ 'done': Autonomy.Inherit, 'error': Autonomy.Inherit }) # x:517 y:124 OperatableStateMachine.add('Analyse Crowd', _sm_analyse_crowd_3, transitions={ 'finished': 'Set Begin Game', 'error': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'error': Autonomy.Inherit }) return _state_machine
def create(self): # x:273 y:335, x:947 y:170 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.action_topic = "/move_group" _state_machine.userdata.trajectory_action_topic = "/m1n6s200_driver/arm_controller/follow_joint_trajectory" _state_machine.userdata.gripper_action_topic = "/m1n6s200_driver/gripper_controller/follow_joint_trajectory" _state_machine.userdata.move_group_arm = "arm" _state_machine.userdata.move_group_gripper = "gripper" _state_machine.userdata.config_name_vertical = "Vertical" _state_machine.userdata.config_name_home = "Home" _state_machine.userdata.config_name_retract = "Retract" _state_machine.userdata.config_name_open = "Open" _state_machine.userdata.config_name_close = "Close" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'MoveIt', SetupProxyMoveItClientState( robot_description="/robot_description", robot_description_semantic=None, move_group_capabilities="/move_group", action_type_and_topics=[[ "MoveGroupAction", ["/move_group"] ]], enter_wait_duration=2.0), transitions={ 'connected': 'Connected', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'connected': Autonomy.High, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'move_groups': 'move_groups' }) # x:132 y:177 OperatableStateMachine.add('Connected', LogState(text="Connected", severity=Logger.REPORT_HINT), transitions={'done': 'Decision'}, autonomy={'done': Autonomy.Off}) # x:303 y:531 OperatableStateMachine.add('Vertical', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'GoalTolerances', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Low, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group_arm', 'config_name': 'config_name_vertical', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:823 y:29 OperatableStateMachine.add( 'Plan', JointValuesToMoveItPlanState(timeout=5.0, enter_wait_duration=0.5, action_topic=None), transitions={ 'planned': 'SwitchController', 'failed': 'failed', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'planned': Autonomy.Low, 'failed': Autonomy.Full, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'action_topic': 'action_topic', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values', 'joint_trajectory': 'joint_trajectory' }) # x:1611 y:323 OperatableStateMachine.add( 'Execute', TrajectoryToFollowJointTrajectoryActionState( goal_time_tolerance=3.0, max_delay=-1.0, wait_duration=2.0, timeout=1.0, action_topic=None), transitions={ 'reached': 'Decision', 'goal_failed': 'LogFail', 'path_failed': 'LogFail', 'invalid_request': 'InvalidRequestError', 'param_error': 'failed', 'failed': 'LogFail' }, autonomy={ 'reached': Autonomy.Full, 'goal_failed': Autonomy.Off, 'path_failed': Autonomy.Off, 'invalid_request': Autonomy.Off, 'param_error': Autonomy.Full, 'failed': Autonomy.Off }, remapping={ 'trajectory_action_topic': 'trajectory_action_topic', 'joint_trajectory': 'joint_trajectory', 'joint_goal_tolerances': 'joint_goal_tolerances', 'joint_path_tolerances': 'joint_path_tolerances', 'status_text': 'status_text', 'goal_names': 'goal_names', 'goal_values': 'goal_values' }) # x:469 y:348 OperatableStateMachine.add('GoalTolerances', SetJointTrajectoryTolerancesState( position_constraints=[0.05], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'PathTolerances', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_goal_tolerances' }) # x:372 y:200 OperatableStateMachine.add('PathTolerances', SetJointTrajectoryTolerancesState( position_constraints=[0.08], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'DumpGoalTolerance', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_path_tolerances' }) # x:348 y:16 OperatableStateMachine.add( 'DumpGoalTolerance', LogKeyState(text="Goal tolerance {}", severity=Logger.REPORT_HINT), transitions={'done': 'DumpPathTolerance'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_goal_tolerances'}) # x:530 y:21 OperatableStateMachine.add( 'DumpPathTolerance', LogKeyState(text="Path tolerance {}", severity=Logger.REPORT_HINT), transitions={'done': 'LogActiveMoveGroup'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_path_tolerances'}) # x:912 y:495 OperatableStateMachine.add('Home', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'GoalTolerances', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Low, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group_arm', 'config_name': 'config_name_home', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:187 y:726 OperatableStateMachine.add('Decision', OperatorDecisionState(outcomes=[ "home", "vertical", "stop", "close", "open" ], hint=None, suggestion=None), transitions={ 'home': 'Home', 'vertical': 'Vertical', 'stop': 'finished', 'close': 'Close', 'open': 'Open' }, autonomy={ 'home': Autonomy.Low, 'vertical': Autonomy.Low, 'stop': Autonomy.Full, 'close': Autonomy.Low, 'open': Autonomy.Low }) # x:748 y:676 OperatableStateMachine.add('Close', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'GoalTolerances', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Low, 'param_error': Autonomy.High }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group_gripper', 'config_name': 'config_name_close', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:1083 y:49 OperatableStateMachine.add('SwitchController', DecisionState( outcomes=["arm", "gripper"], conditions=lambda mg: "gripper" if mg == "gripper" else "arm"), transitions={ 'arm': 'Execute', 'gripper': 'Gripper' }, autonomy={ 'arm': Autonomy.Low, 'gripper': Autonomy.Low }, remapping={'input_value': 'move_group'}) # x:1244 y:428 OperatableStateMachine.add( 'Gripper', TrajectoryToFollowJointTrajectoryActionState( goal_time_tolerance=3.0, max_delay=-1.0, wait_duration=2.0, timeout=1.0, action_topic=None), transitions={ 'reached': 'Decision', 'goal_failed': 'LogFail', 'path_failed': 'LogFail', 'invalid_request': 'InvalidRequestError', 'param_error': 'failed', 'failed': 'LogFail' }, autonomy={ 'reached': Autonomy.Off, 'goal_failed': Autonomy.Off, 'path_failed': Autonomy.Off, 'invalid_request': Autonomy.Off, 'param_error': Autonomy.High, 'failed': Autonomy.Off }, remapping={ 'trajectory_action_topic': 'gripper_action_topic', 'joint_trajectory': 'joint_trajectory', 'joint_goal_tolerances': 'joint_goal_tolerances', 'joint_path_tolerances': 'joint_path_tolerances', 'status_text': 'status_text', 'goal_names': 'goal_names', 'goal_values': 'goal_values' }) # x:612 y:757 OperatableStateMachine.add('Open', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'GoalTolerances', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Low, 'param_error': Autonomy.High }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group_gripper', 'config_name': 'config_name_open', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:745 y:1002 OperatableStateMachine.add('InvalidRequestError', LogState(text="Invalid request", severity=Logger.REPORT_HINT), transitions={'done': 'Decision'}, autonomy={'done': Autonomy.Off}) # x:1315 y:971 OperatableStateMachine.add('LogFail', LogKeyState( text="Failed {}", severity=Logger.REPORT_HINT), transitions={'done': 'Decision'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'status_text'}) # x:716 y:88 OperatableStateMachine.add('LogActiveMoveGroup', LogKeyState( text="Active Move Group is {}", severity=Logger.REPORT_HINT), transitions={'done': 'Plan'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'move_group'}) return _state_machine