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
