def create(self):
# x:31 y:469, x:29 y:271
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.ID = 3
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:255 y:101
OperatableStateMachine.add('GetEntity',
GetEntityByID(),
transitions={
'found': 'GetGrasp',
'not_found': 'failed'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'ID',
'Entity': 'Entity'
})
# x:505 y:135
OperatableStateMachine.add('GetGrasp',
GetGraspFromEntity(
approachDistance=0,
distanceScoringMultiplier=0.5,
orientationScoringMultiplier=0.5,
graspScoringMultiplier=0.5),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'Entity': 'Entity',
'ApproachPose': 'ApproachPose',
'GraspingPose': 'GraspingPose'
})
return _state_machine
def create(self):
# x:93 y:313, x:514 y:143
_state_machine = OperatableStateMachine(outcomes=['found', 'not_found'], input_keys=['className', 'personID'], output_keys=['personEntity'])
_state_machine.userdata.className = "person"
_state_machine.userdata.personID = 0
_state_machine.userdata.personEntity = ""
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:707 y:760
_sm_rotation_0 = OperatableStateMachine(outcomes=['end'], output_keys=['personEntity'])
with _sm_rotation_0:
# x:51 y:38
OperatableStateMachine.add('Set 180 degres',
SetKey(Value=3.1416),
transitions={'done': 'set cpt to 0'},
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': 'check is cpt is 1', 'failed': 'check is cpt is 1'},
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:59 y:128
OperatableStateMachine.add('set cpt to 0',
SetKey(Value=0),
transitions={'done': 'Look Center'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'cpt'})
# x:400 y:499
OperatableStateMachine.add('check is cpt is 1',
CheckConditionState(predicate=lambda x: x==1),
transitions={'true': 'set entity to unknown', 'false': 'set cpt to 1'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'cpt'})
# x:414 y:210
OperatableStateMachine.add('set cpt to 1',
SetKey(Value=1),
transitions={'done': 'Look Right'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'cpt'})
# x:605 y:659
OperatableStateMachine.add('set entity to unknown',
SetKey(Value="unknown"),
transitions={'done': 'end'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'personEntity'})
# x:683 y:188
_sm_find_entity_1 = OperatableStateMachine(outcomes=['found'], input_keys=['personID'], output_keys=['personEntity'])
with _sm_find_entity_1:
# x:226 y:188
OperatableStateMachine.add('get person',
GetEntityByID(),
transitions={'found': 'found', 'not_found': 'WaitState'},
autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ID': 'personID', 'Entity': 'personEntity'})
# x:194 y:40
OperatableStateMachine.add('WaitState',
WaitState(wait_time=1),
transitions={'done': 'get person'},
autonomy={'done': Autonomy.Off})
# x:372 y:27, x:392 y:217, x:400 y:139, x:330 y:458
_sm_container_2 = ConcurrencyContainer(outcomes=['found', 'not_found'], input_keys=['className', 'personID'], output_keys=['personEntity'], 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={'personID': 'personID', 'personEntity': 'personEntity'})
# x:135 y:199
OperatableStateMachine.add('Rotation',
_sm_rotation_0,
transitions={'end': 'not_found'},
autonomy={'end': Autonomy.Inherit},
remapping={'personEntity': 'personEntity'})
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': 'not_found'},
autonomy={'done': Autonomy.Off})
# 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', 'personID': 'personID', 'personEntity': 'personEntity'})
# x:67 y:222
OperatableStateMachine.add('WaitState',
WaitState(wait_time=1),
transitions={'done': 'found'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:666 y:331, x:502 y:199, x:797 y:103
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Position', 'ID'])
_state_machine.userdata.relative = False
_state_machine.userdata.Position = 0
_state_machine.userdata.ID = 0
_state_machine.userdata.className = "person"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:379 y:221
_sm_wait_to_compte_0 = OperatableStateMachine(outcomes=['finished'])
with _sm_wait_to_compte_0:
# x:77 y:195
OperatableStateMachine.add('one more wait',
WaitState(wait_time=60),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:535 y:314
_sm_turn_around_1 = OperatableStateMachine(outcomes=['failed'])
with _sm_turn_around_1:
# x:47 y:45
OperatableStateMachine.add('set orientation',
SetKey(Value=1.57),
transitions={'done': 'action_turn'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'rotation'})
# x:42 y:213
OperatableStateMachine.add('move head',
SaraSetHeadAngle(pitch=0, yaw=1.57),
transitions={'done': 'waitwait'},
autonomy={'done': Autonomy.Off})
# x:53 y:294
OperatableStateMachine.add('waitwait',
WaitState(wait_time=10),
transitions={'done': 'action_turn_2'},
autonomy={'done': Autonomy.Off})
# x:33 y:122
OperatableStateMachine.add('action_turn',
self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'Try to reach/check person behind/move head and base/turn around/action_turn'),
transitions={'finished': 'move head', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'rotation': 'rotation'})
# x:31 y:434
OperatableStateMachine.add('action_turn_2',
self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'Try to reach/check person behind/move head and base/turn around/action_turn_2'),
transitions={'finished': 'head left right', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'rotation': 'rotation'})
# x:35 y:595
OperatableStateMachine.add('head left right',
SaraSetHeadAngle(pitch=0, yaw=1.57),
transitions={'done': 'wait turn head'},
autonomy={'done': Autonomy.Off})
# x:308 y:537
OperatableStateMachine.add('wait turn head',
WaitState(wait_time=10),
transitions={'done': 'head right left'},
autonomy={'done': Autonomy.Off})
# x:495 y:604
OperatableStateMachine.add('head right left',
SaraSetHeadAngle(pitch=0, yaw=-1.57),
transitions={'done': 'wait wait wait'},
autonomy={'done': Autonomy.Off})
# x:262 y:652
OperatableStateMachine.add('wait wait wait',
WaitState(wait_time=10),
transitions={'done': 'head left right'},
autonomy={'done': Autonomy.Off})
# x:845 y:395
_sm_find_human_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID'])
with _sm_find_human_2:
# x:143 y:40
OperatableStateMachine.add('set name',
SetKey(Value="person"),
transitions={'done': 'list'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'name'})
# x:361 y:53
OperatableStateMachine.add('list',
list_entities_by_name(frontality_level=0.5, distance_max=3),
transitions={'found': 'finished', 'none_found': 'list'},
autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off},
remapping={'name': 'name', 'entity_list': 'entity_list', 'number': 'number'})
# x:549 y:135, x:555 y:269, x:230 y:458
_sm_move_head_and_base_3 = ConcurrencyContainer(outcomes=['failed'], conditions=[
('failed', [('turn around', 'failed')]),
('failed', [('wait to compte', 'finished')])
])
with _sm_move_head_and_base_3:
# x:268 y:77
OperatableStateMachine.add('turn around',
_sm_turn_around_1,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
# x:263 y:246
OperatableStateMachine.add('wait to compte',
_sm_wait_to_compte_0,
transitions={'finished': 'failed'},
autonomy={'finished': Autonomy.Inherit})
# x:493 y:206
_sm_container_4 = OperatableStateMachine(outcomes=['check'])
with _sm_container_4:
# x:230 y:160
OperatableStateMachine.add('wait long',
WaitState(wait_time=40),
transitions={'done': 'check'},
autonomy={'done': Autonomy.Off})
# x:30 y:458, x:706 y:447
_sm_navigate_to_the_point_5 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['pose'])
with _sm_navigate_to_the_point_5:
# x:174 y:122
OperatableStateMachine.add('set relative',
SetKey(Value=False),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'relative'})
# x:347 y:191
OperatableStateMachine.add('Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'Try to reach/Container/navigate to the point/Action_Move'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'pose', 'relative': 'relative'})
# x:728 y:335, x:792 y:103, x:738 y:249, x:724 y:448
_sm_check_person_behind_6 = ConcurrencyContainer(outcomes=['finished', 'failed'], input_keys=['ID'], conditions=[
('failed', [('move head and base', 'failed')]),
('finished', [('find human', 'finished')])
])
with _sm_check_person_behind_6:
# x:250 y:72
OperatableStateMachine.add('move head and base',
_sm_move_head_and_base_3,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
# x:261 y:238
OperatableStateMachine.add('find human',
_sm_find_human_2,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:635 y:61, x:634 y:159, x:597 y:300, x:330 y:458, x:430 y:458, x:530 y:458
_sm_container_7 = ConcurrencyContainer(outcomes=['finished', 'failed', 'check'], input_keys=['waypoint'], conditions=[
('check', [('Container', 'check')]),
('finished', [('navigate to the point', 'finished')]),
('failed', [('navigate to the point', 'failed')])
])
with _sm_container_7:
# x:315 y:51
OperatableStateMachine.add('navigate to the point',
_sm_navigate_to_the_point_5,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'waypoint'})
# x:322 y:257
OperatableStateMachine.add('Container',
_sm_container_4,
transitions={'check': 'check'},
autonomy={'check': Autonomy.Inherit})
# x:30 y:458
_sm_groupwait_8 = OperatableStateMachine(outcomes=['end'])
with _sm_groupwait_8:
# x:30 y:40
OperatableStateMachine.add('waitwait',
WaitState(wait_time=20),
transitions={'done': 'end'},
autonomy={'done': Autonomy.Off})
# x:534 y:319
_sm_move_head_and_base_at_the_end_9 = OperatableStateMachine(outcomes=['failed'])
with _sm_move_head_and_base_at_the_end_9:
# x:52 y:31
OperatableStateMachine.add('setkeyorientation',
SetKey(Value=1.5),
transitions={'done': 'action_turn'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'rotation'})
# x:51 y:114
OperatableStateMachine.add('action_turn',
self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'operator is still there/Move head and base end /move head and base at the end/action_turn'),
transitions={'finished': 'turn right head', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'rotation': 'rotation'})
# x:48 y:272
OperatableStateMachine.add('wait while head turn',
WaitState(wait_time=4),
transitions={'done': 'action_turn_2'},
autonomy={'done': Autonomy.Off})
# x:47 y:199
OperatableStateMachine.add('turn right head',
SaraSetHeadAngle(pitch=0, yaw=1.57),
transitions={'done': 'wait while head turn'},
autonomy={'done': Autonomy.Off})
# x:45 y:348
OperatableStateMachine.add('action_turn_2',
self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'operator is still there/Move head and base end /move head and base at the end/action_turn_2'),
transitions={'finished': 'left to rigth', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'rotation': 'rotation'})
# x:43 y:592
OperatableStateMachine.add('left to rigth',
SaraSetHeadAngle(pitch=0, yaw=-1.57),
transitions={'done': 'waitwait1'},
autonomy={'done': Autonomy.Off})
# x:420 y:602
OperatableStateMachine.add('right to left',
SaraSetHeadAngle(pitch=0, yaw=1.57),
transitions={'done': 'waitwait2'},
autonomy={'done': Autonomy.Off})
# x:266 y:541
OperatableStateMachine.add('waitwait1',
WaitState(wait_time=8),
transitions={'done': 'right to left'},
autonomy={'done': Autonomy.Off})
# x:236 y:674
OperatableStateMachine.add('waitwait2',
WaitState(wait_time=8),
transitions={'done': 'left to rigth'},
autonomy={'done': Autonomy.Off})
# x:231 y:538
_sm_find_a_human_10 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID'])
with _sm_find_a_human_10:
# x:168 y:102
OperatableStateMachine.add('set name',
SetKey(Value="person"),
transitions={'done': 'list'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'name'})
# x:275 y:203
OperatableStateMachine.add('list',
list_entities_by_name(frontality_level=0.5, distance_max=3),
transitions={'found': 'finished', 'none_found': 'list'},
autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off},
remapping={'name': 'name', 'entity_list': 'entity_list', 'number': 'number'})
# x:415 y:99, x:318 y:246, x:442 y:295
_sm_move_head_and_base_end__11 = ConcurrencyContainer(outcomes=['failed'], conditions=[
('failed', [('move head and base at the end', 'failed')]),
('failed', [('Groupwait', 'end')])
])
with _sm_move_head_and_base_end__11:
# x:132 y:57
OperatableStateMachine.add('move head and base at the end',
_sm_move_head_and_base_at_the_end_9,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
# x:121 y:218
OperatableStateMachine.add('Groupwait',
_sm_groupwait_8,
transitions={'end': 'failed'},
autonomy={'end': Autonomy.Inherit})
# x:323 y:632, x:638 y:631
_sm_try_to_reach_12 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['waypoint', 'relative', 'ID'])
with _sm_try_to_reach_12:
# x:186 y:130
OperatableStateMachine.add('Container',
_sm_container_7,
transitions={'finished': 'Say_Reached', 'failed': 'say_not_reached', 'check': 'say check'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'check': Autonomy.Inherit},
remapping={'waypoint': 'waypoint'})
# x:581 y:163
OperatableStateMachine.add('check person behind',
_sm_check_person_behind_6,
transitions={'finished': 'say found', 'failed': 'say lost'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:706 y:484
OperatableStateMachine.add('say lost',
SaraSay(sentence="Oh no! I lost my operator!", input_keys=[], emotion=1, block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:445 y:69
OperatableStateMachine.add('say check',
SaraSay(sentence="I check if my operator is still there", input_keys=[], emotion=1, block=True),
transitions={'done': 'check person behind'},
autonomy={'done': Autonomy.Off})
# x:443 y:187
OperatableStateMachine.add('say found',
SaraSay(sentence="Great. You are still there.", input_keys=[], emotion=1, block=True),
transitions={'done': 'Container'},
autonomy={'done': Autonomy.Off})
# x:520 y:409
OperatableStateMachine.add('say_not_reached',
SaraSay(sentence=lambda x: "I have not reach the destination", input_keys=[], emotion=0, block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:139 y:339
OperatableStateMachine.add('Say_Reached',
SaraSay(sentence=lambda x: "I have reach the destination", input_keys=[], emotion=0, block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:626 y:228, x:607 y:71, x:230 y:458, x:330 y:458
_sm_operator_is_still_there_13 = ConcurrencyContainer(outcomes=['finished', 'failed'], input_keys=['ID'], conditions=[
('finished', [('find a human', 'finished')]),
('failed', [('Move head and base end ', 'failed')])
])
with _sm_operator_is_still_there_13:
# x:207 y:54
OperatableStateMachine.add('Move head and base end ',
_sm_move_head_and_base_end__11,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
# x:222 y:190
OperatableStateMachine.add('find a human',
_sm_find_a_human_10,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit},
remapping={'ID': 'ID'})
with _state_machine:
# x:52 y:98
OperatableStateMachine.add('GetPerson',
GetEntityByID(),
transitions={'found': 'sayfollowme', 'not_found': 'Action_findPerson'},
autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ID': 'ID', 'Entity': 'Entity'})
# x:29 y:416
OperatableStateMachine.add('operator is still there',
_sm_operator_is_still_there_13,
transitions={'finished': 'head to middle', 'failed': 'say lost operator'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:290 y:324
OperatableStateMachine.add('say lost operator',
SaraSay(sentence="I have reach my goal but I lost the person I was guiding.", input_keys=[], emotion=1, block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:43 y:214
OperatableStateMachine.add('sayfollowme',
SaraSay(sentence="Follow me please.", input_keys=[], emotion=1, block=True),
transitions={'done': 'Try to reach'},
autonomy={'done': Autonomy.Off})
# x:413 y:441
OperatableStateMachine.add('head to middle',
SaraSetHeadAngle(pitch=0, yaw=0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:39 y:330
OperatableStateMachine.add('Try to reach',
_sm_try_to_reach_12,
transitions={'finished': 'operator is still there', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'waypoint': 'Position', 'relative': 'relative', 'ID': 'ID'})
# x:443 y:95
OperatableStateMachine.add('Cant Find Person',
SaraSay(sentence="I can't find a person.", input_keys=[], emotion=1, block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:201 y:57
OperatableStateMachine.add('Action_findPerson',
self.use_behavior(sara_flexbe_behaviors__Action_findPersonSM, 'Action_findPerson'),
transitions={'done': 'sayfollowme', 'pas_done': 'Cant Find Person'},
autonomy={'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit},
remapping={'className': 'className', 'entity': 'Entity'})
return _state_machine
def create(self):
# x:960 y:715, x:912 y:318, x:904 y:185, x:941 y:610
_state_machine = OperatableStateMachine(
outcomes=['success', 'unreachable', 'not found', 'dropped'],
input_keys=['objectID'])
_state_machine.userdata.objectID = 539
_state_machine.userdata.PreGripPose = "pre_grip_pose"
_state_machine.userdata.entity = 0
_state_machine.userdata.grasp_pose = 0
_state_machine.userdata.approach_pose = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:130 y:365, x:230 y:365
_sm_look_at_it_for_3s_0 = ConcurrencyContainer(
outcomes=['done'],
input_keys=['ID'],
conditions=[('done', [('look', 'failed')]),
('done', [('wait', 'done')])])
with _sm_look_at_it_for_3s_0:
# x:30 y:54
OperatableStateMachine.add('look',
KeepLookingAt(),
transitions={'failed': 'done'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:187 y:111
OperatableStateMachine.add('wait',
WaitState(wait_time=6),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:334 y:336, x:96 y:627
_sm_get_object_1 = OperatableStateMachine(
outcomes=['not_found', 'finished'],
input_keys=['objectID'],
output_keys=['entity'])
with _sm_get_object_1:
# x:67 y:27
OperatableStateMachine.add(
'start segmentation',
SetSegmentationRosParam(ValueTableSegmentation=True,
ValueObjectSegmentation=True),
transitions={'done': 'Look at it for 3s'},
autonomy={'done': Autonomy.Off})
# x:61 y:410
OperatableStateMachine.add(
'Say_See_It',
SaraSay(sentence=lambda x: "I see the " + x[0].name,
input_keys=["Entity"],
emotion=0,
block=True),
transitions={'done': 'pregrip'},
autonomy={'done': Autonomy.Off},
remapping={'Entity': 'entity'})
# x:72 y:322
OperatableStateMachine.add('getobject',
GetEntityByID(),
transitions={
'found': 'Say_See_It',
'not_found': 'not_found'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'objectID',
'Entity': 'entity'
})
# x:62 y:224
OperatableStateMachine.add('stop segmentation',
SetSegmentationRosParam(
ValueTableSegmentation=False,
ValueObjectSegmentation=False),
transitions={'done': 'getobject'},
autonomy={'done': Autonomy.Off})
# x:70 y:122
OperatableStateMachine.add(
'Look at it for 3s',
_sm_look_at_it_for_3s_0,
transitions={'done': 'stop segmentation'},
autonomy={'done': Autonomy.Inherit},
remapping={'ID': 'objectID'})
# x:57 y:513
OperatableStateMachine.add('pregrip',
RunTrajectory(file="pre_grip_pose",
duration=5),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:153 y:506, x:350 y:396
_sm_get_closer_2 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['entity'])
with _sm_get_closer_2:
# x:120 y:61
OperatableStateMachine.add('set distance',
SetKey(Value=0.6),
transitions={'done': 'get att'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distance'})
# x:113 y:172
OperatableStateMachine.add('get att',
GetAttribute(attributes=["position"]),
transitions={'done': 'get waypoint'},
autonomy={'done': Autonomy.Off},
remapping={
'object': 'entity',
'position': 'position'
})
# x:116 y:390
OperatableStateMachine.add('move to waypoint',
SaraMoveBase(reference="map"),
transitions={
'arrived': 'finished',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:107 y:272
OperatableStateMachine.add(
'get waypoint',
Get_Reacheable_Waypoint(),
transitions={'done': 'move to waypoint'},
autonomy={'done': Autonomy.Off},
remapping={
'pose_in': 'position',
'distance': 'distance',
'pose_out': 'pose'
})
# x:280 y:183, x:92 y:289
_sm_get_on_it_3 = OperatableStateMachine(outcomes=['failed', 'done'],
input_keys=['grasp_pose'])
with _sm_get_on_it_3:
# x:76 y:26
OperatableStateMachine.add(
'open gripper',
SetGripperState(width=0.1, effort=0),
transitions={
'object': 'move forward',
'no_object': 'move forward'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:64 y:158
OperatableStateMachine.add('move forward',
MoveitMoveCartesian(
move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'targetPose': 'grasp_pose'})
# x:30 y:324, x:130 y:324
_sm_get_away_from_failure_4 = OperatableStateMachine(
outcomes=['done', 'failed'])
with _sm_get_away_from_failure_4:
# x:30 y:40
OperatableStateMachine.add(
'open 2',
SetGripperState(width=0.1, effort=1),
transitions={
'object': 'go back',
'no_object': 'go back'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:44 y:167
OperatableStateMachine.add('go back',
RunTrajectory(file="pre_grip_pose",
duration=0),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:68 y:579, x:349 y:211
_sm_lift_object_5 = OperatableStateMachine(
outcomes=['done', 'failed'],
input_keys=['approach_pose', 'up_pose'])
with _sm_lift_object_5:
# x:45 y:34
OperatableStateMachine.add('move up',
MoveitMoveCartesian(
move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'genpose',
'failed': 'genpose'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'targetPose': 'up_pose'})
# x:39 y:480
OperatableStateMachine.add('place arm',
RunTrajectory(file="transport",
duration=0),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:40 y:144
OperatableStateMachine.add('genpose',
GenPoseEuler(x=-0.2,
y=0,
z=0,
roll=0,
pitch=0,
yaw=0),
transitions={'done': 'move_back'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'backPose'})
# x:42 y:382
OperatableStateMachine.add('move_back',
SaraMoveBase(reference="base_link"),
transitions={
'arrived': 'place arm',
'failed': 'place arm'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'backPose'})
# x:263 y:214, x:271 y:492
_sm_validation_and_approach_6 = OperatableStateMachine(
outcomes=['failed', 'done'],
input_keys=['grasp_pose', 'approach_pose'])
with _sm_validation_and_approach_6:
# x:85 y:30
OperatableStateMachine.add('checkifposeaccess',
MoveitMove(move=False,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'say can reach',
'failed': 'sayapp'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'grasp_pose'})
# x:48 y:236
OperatableStateMachine.add('move approach',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'approach_pose'})
# x:71 y:115
OperatableStateMachine.add('say can reach',
SaraSay(sentence="I will grab it",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'move approach'},
autonomy={'done': Autonomy.Off})
# x:342 y:93
OperatableStateMachine.add('sayapp',
SaraSay(sentence="unreachable",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:40 y:26
OperatableStateMachine.add('start loop',
SetKey(Value=0),
transitions={'done': 'Get object'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'i'})
# x:256 y:616
OperatableStateMachine.add(
'gripclose',
SetGripperState(width=0, effort=250),
transitions={
'object': 'say picked',
'no_object': 'get away from failure'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:255 y:507
OperatableStateMachine.add('cant reach',
SaraSay(
sentence="Hum! I can't reach it.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'for 2 try'},
autonomy={'done': Autonomy.Off})
# x:261 y:732
OperatableStateMachine.add('say picked',
SaraSay(sentence="I think I got it",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'Lift object'},
autonomy={'done': Autonomy.Off})
# x:751 y:715
OperatableStateMachine.add('welcome',
SaraSay(sentence="you are welcome",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'success'},
autonomy={'done': Autonomy.Off})
# x:9 y:403
OperatableStateMachine.add('validation and approach',
_sm_validation_and_approach_6,
transitions={
'failed': 'say plan',
'done': 'gen up pose'
},
autonomy={
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'grasp_pose': 'grasp_pose',
'approach_pose': 'approach_pose'
})
# x:516 y:717
OperatableStateMachine.add('Lift object',
_sm_lift_object_5,
transitions={
'done': 'welcome',
'failed': 'welcome'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'approach_pose': 'approach_pose',
'up_pose': 'up_pose'
})
# x:480 y:609
OperatableStateMachine.add('get away from failure',
_sm_get_away_from_failure_4,
transitions={
'done': 'say missed',
'failed': 'say missed'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:25 y:311
OperatableStateMachine.add('get grasps',
GetGraspFromEntity(
approachDistance=0.2,
distanceScoringMultiplier=1,
orientationScoringMultiplier=2,
graspScoringMultiplier=1),
transitions={
'done': 'validation and approach',
'failed': 'say cant handle'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'Entity': 'entity',
'ApproachPose': 'approach_pose',
'GraspingPose': 'grasp_pose'
})
# x:738 y:602
OperatableStateMachine.add('say missed',
SaraSay(sentence="Oops! I missed.",
input_keys=[],
emotion=2,
block=True),
transitions={'done': 'dropped'},
autonomy={'done': Autonomy.Off})
# x:257 y:279
OperatableStateMachine.add(
'say cant handle',
SaraSay(sentence="I don't understand how to grab it.",
input_keys=[],
emotion=3,
block=True),
transitions={'done': 'for 2 try'},
autonomy={'done': Autonomy.Off})
# x:18 y:605
OperatableStateMachine.add('get on it',
_sm_get_on_it_3,
transitions={
'failed': 'cant reach',
'done': 'gripclose'
},
autonomy={
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={'grasp_pose': 'grasp_pose'})
# x:261 y:387
OperatableStateMachine.add('say plan',
SaraSay(sentence="Planing failed",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'for 2 try'},
autonomy={'done': Autonomy.Off})
# x:20 y:496
OperatableStateMachine.add('gen up pose',
GenGripperPose(l=0, z=0.1,
planar=False),
transitions={
'done': 'get on it',
'fail': 'cant reach'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'grasp_pose',
'pose_out': 'up_pose'
})
# x:558 y:231
OperatableStateMachine.add('get closer',
_sm_get_closer_2,
transitions={
'finished': 'Get object',
'failed': 'unreachable'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'entity': 'entity'})
# x:553 y:390
OperatableStateMachine.add('for 2 try',
ForLoopWithInput(repeat=1),
transitions={
'do': 'get closer',
'end': 'unreachable'
},
autonomy={
'do': Autonomy.Off,
'end': Autonomy.Off
},
remapping={
'index_in': 'i',
'index_out': 'i'
})
# x:28 y:217
OperatableStateMachine.add('Get object',
_sm_get_object_1,
transitions={
'not_found': 'not found',
'finished': 'get grasps'
},
autonomy={
'not_found': Autonomy.Inherit,
'finished': Autonomy.Inherit
},
remapping={
'objectID': 'objectID',
'entity': 'entity'
})
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):
# x:763 y:423, x:866 y:181, x:896 y:268
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action'])
_state_machine.userdata.Action = ["Follow", "rachel"]
_state_machine.userdata.distance = 1
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:368
_sm_wait_stop_0 = OperatableStateMachine(outcomes=['finished'])
with _sm_wait_stop_0:
# x:112 y:116
OperatableStateMachine.add('Get Command',
GetSpeech(watchdog=1000),
transitions={'done': 'Command Stop', 'nothing': 'Get Command', 'fail': 'Get Command'},
autonomy={'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'words': 'words'})
# x:115 y:270
OperatableStateMachine.add('Command Stop',
CheckConditionState(predicate=lambda x: True in [True for match in ['stop', 'arrived', 'reached', 'here', 'ok'] if match in x]),
transitions={'true': 'finished', 'false': 'Get Command'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'words'})
# x:395 y:153, x:390 y:47, x:359 y:362, x:304 y:368
_sm_follow_loop_1 = ConcurrencyContainer(outcomes=['finished', 'error'], input_keys=['ID', 'distance'], conditions=[
('finished', [('Wait Stop', 'finished')]),
('error', [('Action_follow', 'failed')])
])
with _sm_follow_loop_1:
# x:190 y:37
OperatableStateMachine.add('Action_follow',
self.use_behavior(Action_followSM, 'Follow Loop/Action_follow'),
transitions={'failed': 'error'},
autonomy={'failed': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:190 y:139
OperatableStateMachine.add('Wait Stop',
_sm_wait_stop_0,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit})
with _state_machine:
# x:85 y:51
OperatableStateMachine.add('GetName',
CalculationState(calculation=lambda x: x[1]),
transitions={'done': 'Get Person ID'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'Action', 'output_value': 'name'})
# x:97 y:141
OperatableStateMachine.add('Get Person ID',
GetRosParam(ParamName="/behavior/FoundPerson/Id"),
transitions={'done': 'Get Entity Location', 'failed': 'Say_Lost'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'Value': 'personId'})
# x:92 y:225
OperatableStateMachine.add('Get Entity Location',
GetEntityByID(),
transitions={'found': 'Tell_Follow', 'not_found': 'Say_Lost'},
autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ID': 'personId', 'Entity': 'Entity'})
# x:231 y:389
OperatableStateMachine.add('Tell Way',
SaraSay(sentence="Show me the way !", input_keys=[], emotion=1, block=True),
transitions={'done': 'Follow Loop'},
autonomy={'done': Autonomy.Off})
# x:391 y:374
OperatableStateMachine.add('Follow Loop',
_sm_follow_loop_1,
transitions={'finished': 'Stop_Follow', 'error': 'Cant_Follow'},
autonomy={'finished': Autonomy.Inherit, 'error': Autonomy.Inherit},
remapping={'ID': 'personId', 'distance': 'distance'})
# x:533 y:140
OperatableStateMachine.add('cause1',
SetKey(Value="I lost the person I was following."),
transitions={'done': 'setrosparamcause'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Key'})
# x:540 y:219
OperatableStateMachine.add('cause2',
SetKey(Value="I was unable to follow the person."),
transitions={'done': 'setrosparamcause'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Key'})
# x:697 y:178
OperatableStateMachine.add('setrosparamcause',
SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off},
remapping={'Value': 'Key'})
# x:96 y:341
OperatableStateMachine.add('Tell_Follow',
SaraSay(sentence=lambda x: "I will follow you, " + x[0] + " !", input_keys=["name"], emotion=0, block=True),
transitions={'done': 'Tell Way'},
autonomy={'done': Autonomy.Off},
remapping={'name': 'name'})
# x:571 y:411
OperatableStateMachine.add('Stop_Follow',
SaraSay(sentence=lambda x: "Ok " + x[0] + ", I will stop to follow you !", input_keys=["name"], emotion=0, block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'name': 'name'})
# x:359 y:262
OperatableStateMachine.add('Cant_Follow',
SaraSay(sentence=lambda x: "I can't follow you, " + x[0] + " !", input_keys=["name"], emotion=0, block=True),
transitions={'done': 'cause2'},
autonomy={'done': Autonomy.Off},
remapping={'name': 'name'})
# x:345 y:149
OperatableStateMachine.add('Say_Lost',
SaraSay(sentence=lambda x: "I have lost " + x[0] + " !", input_keys=["name"], emotion=0, block=True),
transitions={'done': 'cause1'},
autonomy={'done': Autonomy.Off},
remapping={'name': 'name'})
return _state_machine
def create(self):
# x:746 y:544, x:65 y:361
_state_machine = OperatableStateMachine(
outcomes=['finished', 'not found'], input_keys=['Position'])
_state_machine.userdata.Position = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365
_sm_verifie_presence_0 = OperatableStateMachine(outcomes=['not found'])
with _sm_verifie_presence_0:
# x:90 y:107
OperatableStateMachine.add(
'getID',
GetRosParam(ParamName="behavior/Operator/Id"),
transitions={
'done': 'getOperator',
'failed': 'not found'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'Value': 'ID'})
# x:297 y:176
OperatableStateMachine.add('getOperator',
GetEntityByID(),
transitions={
'found': 'getID',
'not_found': 'not found'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'ID',
'Entity': 'Operator'
})
# x:208 y:123, x:289 y:206, x:75 y:376, x:330 y:365, x:430 y:365
_sm_deplacement_et_verification_de_presence_1 = ConcurrencyContainer(
outcomes=['arrived', 'failed'],
input_keys=['Position'],
conditions=[('failed', [('verifie presence', 'not found')]),
('arrived', [('saramove', 'arrived')]),
('failed', [('saramove', 'failed')])])
with _sm_deplacement_et_verification_de_presence_1:
# x:70 y:106
OperatableStateMachine.add('saramove',
SaraMoveBase(reference="map"),
transitions={
'arrived': 'arrived',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'Position'})
# x:255 y:90
OperatableStateMachine.add(
'verifie presence',
_sm_verifie_presence_0,
transitions={'not found': 'failed'},
autonomy={'not found': Autonomy.Inherit})
with _state_machine:
# x:55 y:57
OperatableStateMachine.add(
'Get_operator',
self.use_behavior(sara_flexbe_behaviors__Get_operatorSM,
'Get_operator'),
transitions={
'Found': 'foundyou!',
'NotFound': 'not found'
},
autonomy={
'Found': Autonomy.Inherit,
'NotFound': Autonomy.Inherit
},
remapping={'Operator': 'Operator'})
# x:162 y:210
OperatableStateMachine.add(
'foundyou!',
SaraSay(sentence="Operator, please follow me",
emotion=1,
block=True),
transitions={
'done': 'deplacement et verification de presence'
},
autonomy={'done': Autonomy.Off})
# x:398 y:500
OperatableStateMachine.add(
'destinationreached',
SaraSay(sentence="We have reached our destination",
emotion=1,
block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:247 y:319
OperatableStateMachine.add(
'deplacement et verification de presence',
_sm_deplacement_et_verification_de_presence_1,
transitions={
'arrived': 'destinationreached',
'failed': 'lostyou'
},
autonomy={
'arrived': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'Position': 'Position'})
# x:599 y:180
OperatableStateMachine.add(
'lostyou',
SaraSay(sentence="I lost you, please stay where you are",
emotion=1,
block=True),
transitions={'done': 'Get_operator'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:814 y:45, x:514 y:274
_state_machine = OperatableStateMachine(outcomes=['Found', 'NotFound'],
output_keys=['Operator'])
_state_machine.userdata.Operator = None
_state_machine.userdata.Name = "person"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:506 y:393, x:515 y:462
_sm_move_to_person_0 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['Operator'])
with _sm_move_to_person_0:
# x:30 y:83
OperatableStateMachine.add(
'Getpos',
CalculationState(calculation=lambda x: x.position),
transitions={'done': 'setDistance'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'Operator',
'output_value': 'pose_in'
})
# x:35 y:450
OperatableStateMachine.add('Action_Move',
self.use_behavior(
Action_MoveSM,
'Move to person/Action_Move'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pose': 'Pose'})
# x:47 y:368
OperatableStateMachine.add('set not rel',
SetKey(Value=False),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'relative'})
# x:41 y:179
OperatableStateMachine.add('setDistance',
SetKey(Value=1.5),
transitions={'done': 'Close position'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distance'})
# x:27 y:280
OperatableStateMachine.add('Close position',
Get_Reacheable_Waypoint(),
transitions={'done': 'set not rel'},
autonomy={'done': Autonomy.Off},
remapping={
'pose_in': 'pose_in',
'distance': 'distance',
'pose_out': 'Pose'
})
with _state_machine:
# x:64 y:35
OperatableStateMachine.add(
'Get previous ID',
GetRosParam(ParamName="behavior/Operator/Id"),
transitions={
'done': 'Get Operator',
'failed': 'for 3'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'Value': 'ID'})
# x:271 y:37
OperatableStateMachine.add('Get Operator',
GetEntityByID(),
transitions={
'found': 'Found',
'not_found': 'Say lost operator'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'ID',
'Entity': 'Operator'
})
# x:263 y:155
OperatableStateMachine.add('Say lost operator',
SaraSay(sentence="I lost my operator",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'for 3'},
autonomy={'done': Autonomy.Off})
# x:780 y:517
OperatableStateMachine.add('ask if operator',
SaraSay(sentence="Are you my operator?",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'get speech'},
autonomy={'done': Autonomy.Off})
# x:70 y:273
OperatableStateMachine.add('for 3',
ForLoop(repeat=3),
transitions={
'do': 'for 3_2',
'end': 'set None'
},
autonomy={
'do': Autonomy.Off,
'end': Autonomy.Off
},
remapping={'index': 'index'})
# x:249 y:357
OperatableStateMachine.add('say where are you',
SaraSay(
sentence="Operator. Where are you?",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'for 3'},
autonomy={'done': Autonomy.Off})
# x:281 y:265
OperatableStateMachine.add('set None',
SetKey(Value=None),
transitions={'done': 'NotFound'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Operator'})
# x:49 y:511
OperatableStateMachine.add('Get persons',
list_entities_by_name(
frontality_level=0.5,
distance_max=10),
transitions={
'found': 'get next closest',
'none_found': 'say where are you'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'Name',
'entity_list': 'entity_list',
'number': 'number'
})
# x:461 y:475
OperatableStateMachine.add('Move to person',
_sm_move_to_person_0,
transitions={
'finished': 'ask if operator',
'failed': 'NotFound'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'Operator': 'Operator'})
# x:783 y:161
OperatableStateMachine.add(
'set new ID',
SetRosParam(ParamName="behavior/Operator/Id"),
transitions={'done': 'Found'},
autonomy={'done': Autonomy.Off},
remapping={'Value': 'ID'})
# x:775 y:269
OperatableStateMachine.add(
'get ID',
CalculationState(calculation=lambda x: x.ID),
transitions={'done': 'set new ID'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'Operator',
'output_value': 'ID'
})
# x:784 y:433
OperatableStateMachine.add('get speech',
GetSpeech(watchdog=5),
transitions={
'done': 'Yes ?',
'nothing': 'for 3_2',
'fail': 'NotFound'
},
autonomy={
'done': Autonomy.Off,
'nothing': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'words': 'words'})
# x:69 y:402
OperatableStateMachine.add('for 3_2',
ForLoop(repeat=3),
transitions={
'do': 'Get persons',
'end': 'set None'
},
autonomy={
'do': Autonomy.Off,
'end': Autonomy.Off
},
remapping={'index': 'index2'})
# x:744 y:332
OperatableStateMachine.add(
'Yes ?',
CheckConditionState(predicate=lambda x: "yes" in x),
transitions={
'true': 'get ID',
'false': 'for 3_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'words'})
# x:263 y:535
OperatableStateMachine.add(
'get next closest',
FlexibleCalculationState(calculation=lambda x: x[0][x[1]],
input_keys=["entity_list", "index"]),
transitions={'done': 'ask if operator'},
autonomy={'done': Autonomy.Off},
remapping={
'entity_list': 'entity_list',
'index': 'index',
'output_value': 'Operator'
})
return _state_machine
def create(self):
# x:272 y:55
_state_machine = OperatableStateMachine(outcomes=['failed'], input_keys=['ID'])
_state_machine.userdata.ID = 1
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:470 y:563
_sm_delai_0 = OperatableStateMachine(outcomes=['finished'])
with _sm_delai_0:
# x:167 y:26
OperatableStateMachine.add('say lost',
SaraSay(sentence="Sorry, I lost you.", input_keys=[], emotion=0, block=False),
transitions={'done': 'delay'},
autonomy={'done': Autonomy.Off})
# x:167 y:190
OperatableStateMachine.add('say wait',
SaraSay(sentence=" Please wait for me!", input_keys=[], emotion=0, block=False),
transitions={'done': 'delay2'},
autonomy={'done': Autonomy.Off})
# x:161 y:362
OperatableStateMachine.add('say face 2',
SaraSay(sentence="Make sure to let me see your face, please.", input_keys=[], emotion=0, block=False),
transitions={'done': 'delay3'},
autonomy={'done': Autonomy.Off})
# x:173 y:278
OperatableStateMachine.add('delay2',
WaitState(wait_time=3),
transitions={'done': 'say face 2'},
autonomy={'done': Autonomy.Off})
# x:169 y:448
OperatableStateMachine.add('delay3',
WaitState(wait_time=4),
transitions={'done': 'say get closer'},
autonomy={'done': Autonomy.Off})
# x:176 y:108
OperatableStateMachine.add('delay',
WaitState(wait_time=3),
transitions={'done': 'say wait'},
autonomy={'done': Autonomy.Off})
# x:142 y:547
OperatableStateMachine.add('say get closer',
SaraSay(sentence="I need you to get closer please.", input_keys=[], emotion=0, block=False),
transitions={'done': 'delais4'},
autonomy={'done': Autonomy.Off})
# x:315 y:543
OperatableStateMachine.add('delais4',
WaitState(wait_time=5),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:30 y:365
_sm_turn_head_1 = OperatableStateMachine(outcomes=['fail'])
with _sm_turn_head_1:
# x:54 y:55
OperatableStateMachine.add('turn r',
SaraSetHeadAngle(pitch=-0.1, yaw=-1.5),
transitions={'done': 'w3'},
autonomy={'done': Autonomy.Off})
# x:749 y:183
OperatableStateMachine.add('wait1',
WaitState(wait_time=5),
transitions={'done': 'turn right'},
autonomy={'done': Autonomy.Off})
# x:727 y:309
OperatableStateMachine.add('turn right',
SaraSetHeadAngle(pitch=-0.1, yaw=-1.5),
transitions={'done': 'wait2'},
autonomy={'done': Autonomy.Off})
# x:582 y:311
OperatableStateMachine.add('wait2',
WaitState(wait_time=5),
transitions={'done': 'center2'},
autonomy={'done': Autonomy.Off})
# x:246 y:54
OperatableStateMachine.add('w3',
WaitState(wait_time=4),
transitions={'done': 'turn left'},
autonomy={'done': Autonomy.Off})
# x:383 y:57
OperatableStateMachine.add('turn left',
SaraSetHeadAngle(pitch=-0.1, yaw=1.5),
transitions={'done': 'wait3'},
autonomy={'done': Autonomy.Off})
# x:728 y:69
OperatableStateMachine.add('center1',
SaraSetHeadAngle(pitch=-0.1, yaw=0),
transitions={'done': 'wait1'},
autonomy={'done': Autonomy.Off})
# x:576 y:63
OperatableStateMachine.add('wait3',
WaitState(wait_time=5),
transitions={'done': 'center1'},
autonomy={'done': Autonomy.Off})
# x:389 y:308
OperatableStateMachine.add('center2',
SaraSetHeadAngle(pitch=-0.1, yaw=0),
transitions={'done': 'wait4'},
autonomy={'done': Autonomy.Off})
# x:409 y:177
OperatableStateMachine.add('wait4',
WaitState(wait_time=5),
transitions={'done': 'turn left'},
autonomy={'done': Autonomy.Off})
# x:30 y:365
_sm_search_2 = OperatableStateMachine(outcomes=['found'], input_keys=['ID'])
with _sm_search_2:
# x:91 y:163
OperatableStateMachine.add('get en',
GetEntityByID(),
transitions={'found': 'found', 'not_found': 'get en'},
autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ID': 'ID', 'Entity': 'Entity'})
# x:30 y:365, x:530 y:244, x:230 y:365, x:330 y:365, x:430 y:365
_sm_find_back_3 = ConcurrencyContainer(outcomes=['back', 'not_found'], input_keys=['ID'], conditions=[
('back', [('search', 'found')]),
('back', [('Turn head', 'fail')]),
('not_found', [('Delai', 'finished')])
])
with _sm_find_back_3:
# x:145 y:108
OperatableStateMachine.add('search',
_sm_search_2,
transitions={'found': 'back'},
autonomy={'found': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:469 y:136
OperatableStateMachine.add('Turn head',
_sm_turn_head_1,
transitions={'fail': 'back'},
autonomy={'fail': Autonomy.Inherit})
# x:281 y:192
OperatableStateMachine.add('Delai',
_sm_delai_0,
transitions={'finished': 'not_found'},
autonomy={'finished': Autonomy.Inherit})
# x:30 y:458
_sm_look_at_4 = OperatableStateMachine(outcomes=['fail'], input_keys=['ID'])
with _sm_look_at_4:
# x:231 y:115
OperatableStateMachine.add('look',
KeepLookingAt(),
transitions={'failed': 'Find back'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:78 y:199
OperatableStateMachine.add('Found you',
SaraSay(sentence="Here you are!", input_keys=[], emotion=1, block=False),
transitions={'done': 'look'},
autonomy={'done': Autonomy.Off})
# x:248 y:264
OperatableStateMachine.add('Find back',
_sm_find_back_3,
transitions={'back': 'Found you', 'not_found': 'fail'},
autonomy={'back': Autonomy.Inherit, 'not_found': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:30 y:365
_sm_follow_5 = OperatableStateMachine(outcomes=['failed'], input_keys=['ID'])
with _sm_follow_5:
# x:65 y:167
OperatableStateMachine.add('follow',
SaraFollow(distance=1.5, ReplanPeriod=3),
transitions={'failed': 'follow'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:368 y:271, x:423 y:158, x:230 y:458
_sm_follow_6 = ConcurrencyContainer(outcomes=['not_found'], input_keys=['ID'], conditions=[
('not_found', [('Look at', 'fail')]),
('not_found', [('Follow', 'failed')])
])
with _sm_follow_6:
# x:185 y:134
OperatableStateMachine.add('Follow',
_sm_follow_5,
transitions={'failed': 'not_found'},
autonomy={'failed': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:123 y:236
OperatableStateMachine.add('Look at',
_sm_look_at_4,
transitions={'fail': 'not_found'},
autonomy={'fail': Autonomy.Inherit},
remapping={'ID': 'ID'})
with _state_machine:
# x:74 y:38
OperatableStateMachine.add('Follow',
_sm_follow_6,
transitions={'not_found': 'failed'},
autonomy={'not_found': Autonomy.Inherit},
remapping={'ID': 'ID'})
return _state_machine
def create(self):
# x:831 y:173, x:520 y:222, x:335 y:32, x:701 y:385
_state_machine = OperatableStateMachine(
outcomes=['success', 'unreachable', 'not found', 'dropped'],
input_keys=['objectID', 'Entity'])
_state_machine.userdata.objectID = 1585
_state_machine.userdata.PreGripPose = "PreGripPose"
_state_machine.userdata.PostGripPose = "PostGripPose"
_state_machine.userdata.Entity = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:130 y:365, x:230 y:365
_sm_look_at_it_for_3s_0 = ConcurrencyContainer(
outcomes=['done'],
input_keys=['ID'],
conditions=[('done', [('look', 'failed')]),
('done', [('wait', 'done')])])
with _sm_look_at_it_for_3s_0:
# x:30 y:54
OperatableStateMachine.add('look',
KeepLookingAt(),
transitions={'failed': 'done'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:187 y:111
OperatableStateMachine.add('wait',
WaitState(wait_time=3),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:30 y:324
_sm_getting_close_1 = OperatableStateMachine(outcomes=['done'],
input_keys=['pose_app'])
with _sm_getting_close_1:
# x:36 y:40
OperatableStateMachine.add(
'gripperopen',
SetGripperState(width=0.15, effort=1),
transitions={
'object': 'move_approach',
'no_object': 'move_approach'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:30 y:129
OperatableStateMachine.add('move_approach',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'almost have it',
'failed': 'almost have it'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'pose_app'})
# x:39 y:204
OperatableStateMachine.add('almost have it',
SaraSay(sentence="I am close",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:30 y:324, x:130 y:324
_sm_get_away_from_failure_2 = OperatableStateMachine(
outcomes=['done', 'failed'], input_keys=['PostGripPose'])
with _sm_get_away_from_failure_2:
# x:30 y:40
OperatableStateMachine.add(
'open 2',
SetGripperState(width=0.1, effort=1),
transitions={
'object': 'move back',
'no_object': 'move back'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:235 y:50
OperatableStateMachine.add('move back',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PostGripPose'})
# x:30 y:324, x:130 y:324
_sm_lift_object_3 = OperatableStateMachine(
outcomes=['done', 'failed'], input_keys=['pose_lift', 'pose_ret'])
with _sm_lift_object_3:
# x:30 y:176
OperatableStateMachine.add('move_lift_object',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'genpose',
'failed': 'genpose'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'pose_lift'})
# x:228 y:176
OperatableStateMachine.add('move_Return',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'pose_ret'})
# x:38 y:45
OperatableStateMachine.add('genpose',
GenPoseEuler(x=-0.2,
y=0,
z=0,
roll=0,
pitch=0,
yaw=0),
transitions={'done': 'move_back'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'backPose'})
# x:211 y:40
OperatableStateMachine.add('move_back',
SaraMoveBase(reference="base_link"),
transitions={
'arrived': 'move_Return',
'failed': 'move_Return'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'backPose'})
# x:499 y:227, x:263 y:214, x:271 y:492
_sm_pregrip_4 = OperatableStateMachine(
outcomes=['fail', 'failed', 'done'],
input_keys=['PreGripPose', 'posobjet'],
output_keys=['pose_app', 'grippose', 'pose_lift', 'pose_ret'])
with _sm_pregrip_4:
# x:70 y:40
OperatableStateMachine.add('gen_gripPose',
GenGripperPose(l=0.0, z=0, planar=True),
transitions={
'done': 'checkifposeaccess',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'posobjet',
'pose_out': 'grippose'
})
# x:56 y:256
OperatableStateMachine.add('move_PreGrip',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'gen_returnPose',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PreGripPose'})
# x:30 y:398
OperatableStateMachine.add('gen_approachPose',
GenGripperPose(l=0.15, z=0,
planar=True),
transitions={
'done': 'gen_liftPose',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'posobjet',
'pose_out': 'pose_app'
})
# x:40 y:469
OperatableStateMachine.add('gen_liftPose',
GenGripperPose(l=0, z=0.1, planar=True),
transitions={
'done': 'done',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'posobjet',
'pose_out': 'pose_lift'
})
# x:43 y:326
OperatableStateMachine.add('gen_returnPose',
GenGripperPose(l=0.2,
z=0.1,
planar=True),
transitions={
'done': 'gen_approachPose',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'posobjet',
'pose_out': 'pose_ret'
})
# x:63 y:184
OperatableStateMachine.add('say can reach',
SaraSay(sentence="I will grab it",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'move_PreGrip'},
autonomy={'done': Autonomy.Off})
# x:58 y:114
OperatableStateMachine.add('checkifposeaccess',
MoveitMove(move=False,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'say can reach',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'grippose'})
# x:315 y:40, x:130 y:465
_sm_get_object_5 = OperatableStateMachine(
outcomes=['not_found', 'finished'],
input_keys=['objectID'],
output_keys=['posobjet'])
with _sm_get_object_5:
# x:55 y:40
OperatableStateMachine.add('getobject',
GetEntityByID(),
transitions={
'found': 'Say_See_It',
'not_found': 'not_found'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'objectID',
'Entity': 'Entity'
})
# x:77 y:370
OperatableStateMachine.add(
'getpose',
CalculationState(calculation=lambda x: x.position),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'Entity',
'output_value': 'posobjet'
})
# x:71 y:262
OperatableStateMachine.add('Look at it for 3s',
_sm_look_at_it_for_3s_0,
transitions={'done': 'getpose'},
autonomy={'done': Autonomy.Inherit},
remapping={'ID': 'objectID'})
# x:31 y:133
OperatableStateMachine.add(
'Say_See_It',
SaraSay(sentence=lambda x: "I see the " + x.name,
input_keys=["Entity"],
emotion=0,
block=True),
transitions={'done': 'Look at it for 3s'},
autonomy={'done': Autonomy.Off},
remapping={'Entity': 'Entity'})
with _state_machine:
# x:77 y:23
OperatableStateMachine.add('Get object',
_sm_get_object_5,
transitions={
'not_found': 'not found',
'finished': 'transform point'
},
autonomy={
'not_found': Autonomy.Inherit,
'finished': Autonomy.Inherit
},
remapping={
'objectID': 'objectID',
'posobjet': 'posobjet'
})
# x:258 y:385
OperatableStateMachine.add(
'gripclose',
SetGripperState(width=0, effort=250),
transitions={
'object': 'say picked',
'no_object': 'get away from failure'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:53 y:374
OperatableStateMachine.add('move_on_object',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'gripclose',
'failed': 'cant reach'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'grippose'})
# x:650 y:290
OperatableStateMachine.add('move_PostGrip',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'welcome',
'failed': 'unreachable'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PostGripPose'})
# x:277 y:200
OperatableStateMachine.add('cant reach',
SaraSay(
sentence="Hum. I can't reach it.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'unreachable'},
autonomy={'done': Autonomy.Off})
# x:265 y:291
OperatableStateMachine.add('say picked',
SaraSay(sentence="I think I got it",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'Lift object'},
autonomy={'done': Autonomy.Off})
# x:837 y:291
OperatableStateMachine.add('welcome',
SaraSay(sentence="you are welcome",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'success'},
autonomy={'done': Autonomy.Off})
# x:62 y:190
OperatableStateMachine.add('PreGrip',
_sm_pregrip_4,
transitions={
'fail': 'cant reach',
'failed': 'cant reach',
'done': 'Getting close'
},
autonomy={
'fail': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'PreGripPose': 'PreGripPose',
'posobjet': 'posobjet',
'pose_app': 'pose_app',
'grippose': 'grippose',
'pose_lift': 'pose_lift',
'pose_ret': 'pose_ret'
})
# x:67 y:114
OperatableStateMachine.add('transform point',
TF_transformation(in_ref="map",
out_ref="base_link"),
transitions={
'done': 'PreGrip',
'fail': 'Get object'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'in_pos': 'posobjet',
'out_pos': 'posobjet'
})
# x:439 y:287
OperatableStateMachine.add('Lift object',
_sm_lift_object_3,
transitions={
'done': 'move_PostGrip',
'failed': 'move_PostGrip'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'pose_lift': 'pose_lift',
'pose_ret': 'pose_ret'
})
# x:441 y:391
OperatableStateMachine.add(
'get away from failure',
_sm_get_away_from_failure_2,
transitions={
'done': 'dropped',
'failed': 'dropped'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'PostGripPose': 'PostGripPose'})
# x:47 y:279
OperatableStateMachine.add('Getting close',
_sm_getting_close_1,
transitions={'done': 'move_on_object'},
autonomy={'done': Autonomy.Inherit},
remapping={'pose_app': 'pose_app'})
return _state_machine
