def create(self):
# x:30 y:365
_state_machine = OperatableStateMachine(outcomes=['failed'])
_state_machine.userdata.ObjectName = "person"
_state_machine.userdata.ID = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:130 y:365
_sm_keeplookingforatime_0 = ConcurrencyContainer(
outcomes=['failed'],
input_keys=['ID'],
conditions=[('failed', [('keep looking', 'failed'),
('wait 3', 'done')])])
with _sm_keeplookingforatime_0:
# x:210 y:103
OperatableStateMachine.add('wait 3',
WaitState(wait_time=3),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:37 y:128
OperatableStateMachine.add('keep looking',
KeepLookingAt(),
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
with _state_machine:
# x:49 y:113
OperatableStateMachine.add('list',
list_entities_by_name(
frontality_level=0.8,
distance_max=10),
transitions={
'found': 'getclosest',
'none_found': 'list'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'ObjectName',
'entity_list': 'entity_list',
'number': 'number'
})
# x:296 y:131
OperatableStateMachine.add(
'getclosest',
CalculationState(calculation=lambda x: x[0].ID),
transitions={'done': 'KeepLookingForATime'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'entity_list',
'output_value': 'ID'
})
# x:220 y:258
OperatableStateMachine.add('KeepLookingForATime',
_sm_keeplookingforatime_0,
transitions={'failed': 'list'},
autonomy={'failed': Autonomy.Inherit},
remapping={'ID': 'ID'})
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:1195 y:433, x:132 y:431, x:750 y:42, x:991 y:471
_state_machine = OperatableStateMachine(outcomes=['Given', 'Person_not_found', 'No_object_in_hand', 'fail'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:130 y:365
_sm_lookat_0 = OperatableStateMachine(outcomes=['failed'], input_keys=['ID'])
with _sm_lookat_0:
# x:114 y:127
OperatableStateMachine.add('look',
KeepLookingAt(),
transitions={'failed': 'look'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:299 y:300, x:263 y:535
_sm_give_1 = OperatableStateMachine(outcomes=['failed', 'given'], input_keys=['Object'])
with _sm_give_1:
# x:67 y:27
OperatableStateMachine.add('SetPose',
SetKey(Value="ShowGripper"),
transitions={'done': 'say_give'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'target'})
# x:53 y:413
OperatableStateMachine.add('read torque',
ReadTorque(watchdog=5, Joint="right_elbow_pitch_joint", Threshold=2, min_time=1),
transitions={'threshold': 'open gripper', 'watchdog': 'read torque', 'fail': 'failed'},
autonomy={'threshold': Autonomy.Off, 'watchdog': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'torque': 'torque'})
# x:52 y:500
OperatableStateMachine.add('open gripper',
SetGripperState(width=0.15, effort=1),
transitions={'object': 'given', 'no_object': 'given'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:64 y:248
OperatableStateMachine.add('say pull',
SaraSay(sentence="You can pull on it", input_keys=[], emotion=1, block=False),
transitions={'done': 'wait 1'},
autonomy={'done': Autonomy.Off})
# x:64 y:325
OperatableStateMachine.add('wait 1',
WaitState(wait_time=1),
transitions={'done': 'read torque'},
autonomy={'done': Autonomy.Off})
# x:57 y:175
OperatableStateMachine.add('moveArm',
MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15),
transitions={'done': 'say pull', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'target': 'target'})
# x:60 y:88
OperatableStateMachine.add('say_give',
SaraSay(sentence=lambda x: "Hi. I'm giving you this "+str(x), input_keys=[], emotion=0, block=True),
transitions={'done': 'moveArm'},
autonomy={'done': Autonomy.Off})
# x:596 y:480
_sm_follow_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID'])
with _sm_follow_2:
# x:180 y:123
OperatableStateMachine.add('follow',
SaraFollow(distance=1.5, ReplanPeriod=0.5),
transitions={'failed': 'follow'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:313 y:247, x:301 y:177, x:103 y:293, x:343 y:113, x:397 y:411, x:311 y:27, x:630 y:365
_sm_give_3 = ConcurrencyContainer(outcomes=['failed', 'given', 'continue'], input_keys=['ID', 'Object'], conditions=[
('failed', [('Give', 'failed')]),
('given', [('Give', 'given')]),
('given', [('Follow', 'finished')]),
('failed', [('LookAt', 'failed')])
])
with _sm_give_3:
# x:91 y:50
OperatableStateMachine.add('Follow',
_sm_follow_2,
transitions={'finished': 'given'},
autonomy={'finished': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:84 y:164
OperatableStateMachine.add('Give',
_sm_give_1,
transitions={'failed': 'failed', 'given': 'given'},
autonomy={'failed': Autonomy.Inherit, 'given': Autonomy.Inherit},
remapping={'Object': 'Object'})
# x:175 y:371
OperatableStateMachine.add('LookAt',
_sm_lookat_0,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit},
remapping={'ID': 'ID'})
with _state_machine:
# x:77 y:29
OperatableStateMachine.add('Get hand content',
GetRosParam(ParamName="behavior/GripperContent"),
transitions={'done': 'is object in hand?', 'failed': 'fail'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'Value': 'Object'})
# x:58 y:108
OperatableStateMachine.add('is object in hand?',
CheckConditionState(predicate=lambda x: x),
transitions={'true': 'name', 'false': 'log empty hand'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'Object'})
# x:70 y:277
OperatableStateMachine.add('list persons',
list_entities_by_name(frontality_level=0.5, distance_max=10),
transitions={'found': 'get id', 'none_found': 'Person_not_found'},
autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off},
remapping={'name': 'name', 'entity_list': 'People_list', 'number': 'number'})
# x:414 y:37
OperatableStateMachine.add('log empty hand',
LogState(text="The hand is empty. Set the GripperContent rosParam", severity=Logger.REPORT_HINT),
transitions={'done': 'No_object_in_hand'},
autonomy={'done': Autonomy.Off})
# x:754 y:223
OperatableStateMachine.add('log moveitfail',
LogState(text="moveit failed", severity=Logger.REPORT_HINT),
transitions={'done': 'fail'},
autonomy={'done': Autonomy.Off})
# x:606 y:371
OperatableStateMachine.add('log movebase fail',
LogState(text="giving Failed", severity=Logger.REPORT_HINT),
transitions={'done': 'fail'},
autonomy={'done': Autonomy.Off})
# x:402 y:133
OperatableStateMachine.add('set idle pose',
SetKey(Value="IdlePose"),
transitions={'done': 'say_good'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'target'})
# x:751 y:108
OperatableStateMachine.add('moveArm2',
MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15),
transitions={'done': 'set none', 'failed': 'log moveitfail'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'target': 'target'})
# x:920 y:278
OperatableStateMachine.add('close gripper',
SetGripperState(width=0, effort=1),
transitions={'object': 'Given', 'no_object': 'Given'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:1048 y:236
OperatableStateMachine.add('remove gripper content',
SetRosParam(ParamName="GripperContent"),
transitions={'done': 'close gripper'},
autonomy={'done': Autonomy.Off},
remapping={'Value': 'none'})
# x:910 y:182
OperatableStateMachine.add('set none',
SetKey(Value=None),
transitions={'done': 'close gripper'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'none'})
# x:408 y:333
OperatableStateMachine.add('give',
_sm_give_3,
transitions={'failed': 'log movebase fail', 'given': 'set idle pose', 'continue': 'give'},
autonomy={'failed': Autonomy.Inherit, 'given': Autonomy.Inherit, 'continue': Autonomy.Inherit},
remapping={'ID': 'ID', 'Object': 'Object'})
# x:256 y:278
OperatableStateMachine.add('get id',
CalculationState(calculation=lambda x: x[0].ID),
transitions={'done': 'give'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'People_list', 'output_value': 'ID'})
# x:68 y:192
OperatableStateMachine.add('name',
SetKey(Value="person"),
transitions={'done': 'list persons'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'name'})
# x:591 y:138
OperatableStateMachine.add('say_good',
SaraSay(sentence=lambda x: "Good, enjoy your "+str(x), input_keys=[], emotion=0, block=True),
transitions={'done': 'moveArm2'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:860 y:786, x:837 y:171, x:828 y:43
_state_machine = OperatableStateMachine(
outcomes=['found', 'failed', 'not_found'],
input_keys=['question'],
output_keys=['entityFound'])
_state_machine.userdata.question = ""
_state_machine.userdata.entityFound = ""
_state_machine.userdata.personKey = "person"
_state_machine.userdata.index = -1
_state_machine.userdata.rotation180degres = -3.14
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:458, x:130 y:458
_sm_keep_looking_at_person_0 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['personID'])
with _sm_keep_looking_at_person_0:
# x:79 y:77
OperatableStateMachine.add('keep looking',
KeepLookingAt(),
transitions={'failed': 'keep looking'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'personID'})
# x:30 y:458, x:130 y:458
_sm_ask_1 = OperatableStateMachine(outcomes=['finished', 'failed'],
input_keys=['question'],
output_keys=['answer'])
with _sm_ask_1:
# x:57 y:66
OperatableStateMachine.add('wait',
WaitState(wait_time=3),
transitions={'done': 'Action_Ask'},
autonomy={'done': Autonomy.Off})
# x:46 y:155
OperatableStateMachine.add(
'Action_Ask',
self.use_behavior(
sara_flexbe_behaviors__Action_AskSM,
'ask while looking at person/ask/Action_Ask'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'question': 'question',
'answer': 'answer'
})
# x:871 y:712, x:874 y:127
_sm_get_real_id_2 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['entity', 'personKey', 'FIFO'],
output_keys=['personID'])
with _sm_get_real_id_2:
# x:47 y:32
OperatableStateMachine.add(
'get posittion face or entity',
CalculationState(calculation=lambda x: x.face.boundingBox.
Center if x.face.id != '' else x.position),
transitions={'done': 'get direction to point'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'entity',
'output_value': 'pointToLook'
})
# x:27 y:351
OperatableStateMachine.add('look at point',
SaraSetHeadAngleKey(),
transitions={'done': 'wait 1'},
autonomy={'done': Autonomy.Off},
remapping={
'yaw': 'yaw',
'pitch': 'pitch'
})
# x:40 y:423
OperatableStateMachine.add(
'wait 1',
WaitState(wait_time=1),
transitions={'done': 'list entity in front'},
autonomy={'done': Autonomy.Off})
# x:24 y:491
OperatableStateMachine.add(
'list entity in front',
list_entities_by_name(frontality_level=0.5, distance_max=3),
transitions={
'found':
'fitler the entity list to remove id already checked',
'none_found': 'set key'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'personKey',
'entity_list': 'entity_list',
'number': 'number'
})
# x:42 y:632
OperatableStateMachine.add(
'calcul first entity ID',
CalculationState(calculation=lambda x: x[0].ID),
transitions={'done': 'say real id'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'filteredEntityList',
'output_value': 'personID'
})
# x:518 y:578
OperatableStateMachine.add(
'say real id',
SaraSay(sentence=lambda x: "The real ID is " + str(x[0]) + ".",
input_keys=["personID"],
emotion=0,
block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'personID': 'personID'})
# x:539 y:164
OperatableStateMachine.add('set key',
SetKey(Value=0),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'personID'})
# x:34 y:234
OperatableStateMachine.add('pitch to 0.1 if no face',
FlexibleCalculationState(
calculation=lambda x: x[0]
if x[1].face.id != '' else 0.1,
input_keys=["pitch", "entity"]),
transitions={'done': 'look at point'},
autonomy={'done': Autonomy.Off},
remapping={
'pitch': 'pitch',
'entity': 'entity',
'output_value': 'pitch'
})
# x:37 y:149
OperatableStateMachine.add('get direction to point',
Get_direction_to_point(
frame_origin="base_link",
frame_reference="head_link"),
transitions={
'done': 'pitch to 0.1 if no face',
'fail': 'set key'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'targetPoint': 'pointToLook',
'yaw': 'yaw',
'pitch': 'pitch'
})
# x:29 y:560
OperatableStateMachine.add(
'fitler the entity list to remove id already checked',
FilterKey(filter_function=lambda x: x[0].ID not in x[1],
input_keys=["input_list", "FIFO"]),
transitions={
'not_empty': 'calcul first entity ID',
'empty': 'set key'
},
autonomy={
'not_empty': Autonomy.Off,
'empty': Autonomy.Off
},
remapping={
'input_list': 'entity_list',
'FIFO': 'FIFO',
'output_list': 'filteredEntityList'
})
# x:30 y:458, x:710 y:24
_sm_tourne_tete_et_base_3 = OperatableStateMachine(
outcomes=['done', 'failed'],
input_keys=['index', 'rotation180degres'])
with _sm_tourne_tete_et_base_3:
# x:57 y:27
OperatableStateMachine.add(
'decide',
DecisionState(outcomes=["_0", "_1", "_2", "_3", "_4"],
conditions=lambda x: "_" + str(x)),
transitions={
'_0': 'look center',
'_1': 'look center_2',
'_2': 'look center_3',
'_3': 'action_turn',
'_4': 'look center_5'
},
autonomy={
'_0': Autonomy.Off,
'_1': Autonomy.Off,
'_2': Autonomy.Off,
'_3': Autonomy.Off,
'_4': Autonomy.Off
},
remapping={'input_value': 'index'})
# x:186 y:236
OperatableStateMachine.add('look right',
SaraSetHeadAngle(pitch=0.1, yaw=-1.5),
transitions={'done': 'wait right'},
autonomy={'done': Autonomy.Off})
# x:208 y:307
OperatableStateMachine.add('wait right',
WaitState(wait_time=3),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:322 y:109
OperatableStateMachine.add('look center_3',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'wait center_3'},
autonomy={'done': Autonomy.Off})
# x:325 y:174
OperatableStateMachine.add('wait center_3',
WaitState(wait_time=3),
transitions={'done': 'look right_2'},
autonomy={'done': Autonomy.Off})
# x:318 y:240
OperatableStateMachine.add('look right_2',
SaraSetHeadAngle(pitch=0.1, yaw=1.5),
transitions={'done': 'wait right_2'},
autonomy={'done': Autonomy.Off})
# x:345 y:310
OperatableStateMachine.add('wait right_2',
WaitState(wait_time=3),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:489 y:178
OperatableStateMachine.add('look left_2_2',
SaraSetHeadAngle(pitch=0.1, yaw=1.5),
transitions={'done': 'waitleft_2_2'},
autonomy={'done': Autonomy.Off})
# x:506 y:247
OperatableStateMachine.add('waitleft_2_2',
WaitState(wait_time=3),
transitions={'done': 'look center_4'},
autonomy={'done': Autonomy.Off})
# x:501 y:319
OperatableStateMachine.add('look center_4',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'wait center_4'},
autonomy={'done': Autonomy.Off})
# x:520 y:407
OperatableStateMachine.add('wait center_4',
WaitState(wait_time=3),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:476 y:109
OperatableStateMachine.add(
'action_turn',
self.use_behavior(sara_flexbe_behaviors__action_turnSM,
'tourne tete et base/action_turn'),
transitions={
'finished': 'look left_2_2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'rotation': 'rotation180degres'})
# x:199 y:108
OperatableStateMachine.add('look center_2',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'wait center_2'},
autonomy={'done': Autonomy.Off})
# x:51 y:113
OperatableStateMachine.add('look center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'wait center'},
autonomy={'done': Autonomy.Off})
# x:56 y:174
OperatableStateMachine.add('wait center',
WaitState(wait_time=4),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:206 y:173
OperatableStateMachine.add('wait center_2',
WaitState(wait_time=2),
transitions={'done': 'look right'},
autonomy={'done': Autonomy.Off})
# x:730 y:110
OperatableStateMachine.add('look center_5',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'wait center_5'},
autonomy={'done': Autonomy.Off})
# x:741 y:193
OperatableStateMachine.add('wait center_5',
WaitState(wait_time=3),
transitions={'done': 'look right_5'},
autonomy={'done': Autonomy.Off})
# x:733 y:296
OperatableStateMachine.add('look right_5',
SaraSetHeadAngle(pitch=0.1, yaw=-1.5),
transitions={'done': 'wait right_5'},
autonomy={'done': Autonomy.Off})
# x:731 y:422
OperatableStateMachine.add('wait right_5',
WaitState(wait_time=3),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:449 y:45, x:454 y:135, x:447 y:195, x:446 y:252, x:430 y:458, x:530 y:458
_sm_ask_while_looking_at_person_4 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
input_keys=['personID', 'question'],
output_keys=['answer'],
conditions=[('finished', [('ask', 'finished')]),
('failed', [('ask', 'failed')]),
('finished', [('keep looking at person', 'finished')]),
('failed', [('keep looking at person', 'failed')])])
with _sm_ask_while_looking_at_person_4:
# x:97 y:55
OperatableStateMachine.add('ask',
_sm_ask_1,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'question': 'question',
'answer': 'answer'
})
# x:65 y:197
OperatableStateMachine.add('keep looking at person',
_sm_keep_looking_at_person_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'personID': 'personID'})
with _state_machine:
# x:30 y:115
OperatableStateMachine.add(
'Init_Sequence',
self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM,
'Init_Sequence'),
transitions={
'finished': 'create fifo',
'failed': 'create fifo'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:164 y:62
OperatableStateMachine.add('for loop',
ForLoopWithInput(repeat=4),
transitions={
'do': 'tourne tete et base',
'end': 'not_found'
},
autonomy={
'do': Autonomy.Off,
'end': Autonomy.Off
},
remapping={
'index_in': 'index',
'index_out': 'index'
})
# x:260 y:557
OperatableStateMachine.add('ask while looking at person',
_sm_ask_while_looking_at_person_4,
transitions={
'finished': 'answer contains yes',
'failed': 'add id'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'personID': 'personID',
'question': 'question',
'answer': 'answer'
})
# x:289 y:480
OperatableStateMachine.add(
'get personID',
CalculationState(calculation=lambda x: x.ID),
transitions={'done': 'ask while looking at person'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'personEntity',
'output_value': 'personID'
})
# x:270 y:643
OperatableStateMachine.add(
'answer contains yes',
RegexTester(
regex=".*((yes)|(Yes)|(yep)|(sure)|(of course)).*"),
transitions={
'true': 'say ty',
'false': 'add id'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'text': 'answer',
'result': 'result'
})
# x:173 y:260
OperatableStateMachine.add('look center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'for loop'},
autonomy={'done': Autonomy.Off})
# x:372 y:89
OperatableStateMachine.add('tourne tete et base',
_sm_tourne_tete_et_base_3,
transitions={
'done': 'get list of person',
'failed': 'not_found'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'index': 'index',
'rotation180degres':
'rotation180degres'
})
# x:263 y:322
OperatableStateMachine.add(
'fitler the entity list to remove id already checked',
FilterKey(filter_function=lambda x: x[0].ID not in x[1],
input_keys=["input_list", "FIFO"]),
transitions={
'not_empty': 'get first entity',
'empty': 'look center'
},
autonomy={
'not_empty': Autonomy.Off,
'empty': Autonomy.Off
},
remapping={
'input_list': 'entityList',
'FIFO': 'FIFO',
'output_list': 'filteredEntityList'
})
# x:268 y:388
OperatableStateMachine.add(
'get first entity',
CalculationState(calculation=lambda x: x[0]),
transitions={'done': 'get real id'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'filteredEntityList',
'output_value': 'personEntity'
})
# x:296 y:168
OperatableStateMachine.add(
'get list of person',
list_entities_by_name(frontality_level=0.5, distance_max=3),
transitions={
'found':
'fitler the entity list to remove id already checked',
'none_found': 'for loop'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'personKey',
'entity_list': 'entityList',
'number': 'numberOfEntity'
})
# x:111 y:533
OperatableStateMachine.add(
'add id',
FIFO_Add(),
transitions={'done': 'say keep looking'},
autonomy={'done': Autonomy.Off},
remapping={
'Entry': 'personID',
'FIFO': 'FIFO'
})
# x:609 y:651
OperatableStateMachine.add('say ty',
SaraSay(sentence="Thank you.",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'found'},
autonomy={'done': Autonomy.Off})
# x:113 y:386
OperatableStateMachine.add('say keep looking',
SaraSay(sentence="Ok, never mind.",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'for loop'},
autonomy={'done': Autonomy.Off})
# x:472 y:438
OperatableStateMachine.add('get real id',
_sm_get_real_id_2,
transitions={
'finished':
'ask while looking at person',
'failed': 'get personID'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'entity': 'personEntity',
'personKey': 'personKey',
'FIFO': 'FIFO',
'personID': 'personID'
})
# x:35 y:53
OperatableStateMachine.add('create fifo',
FIFO_New(),
transitions={'done': 'for loop'},
autonomy={'done': Autonomy.Off},
remapping={'FIFO': 'FIFO'})
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: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:391 y:586, x:438 y:304
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.StartPosition = "Operator"
_state_machine.userdata.PositionBras = "IdlePose"
_state_machine.userdata.title = "GPSR"
_state_machine.userdata.EntryName = "door1/enter"
_state_machine.userdata.ExitName = "door2/exit"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:135 y:542, x:527 y:395
_sm_validate_0 = OperatableStateMachine(outcomes=['done', 'bad'],
input_keys=['sentence'])
with _sm_validate_0:
# x:56 y:62
OperatableStateMachine.add(
'Say_Command',
SaraSay(
sentence=lambda x: "I heard. " + x + ". Is that correct?",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'get speech'},
autonomy={'done': Autonomy.Off})
# x:44 y:165
OperatableStateMachine.add('get speech',
GetSpeech(watchdog=10),
transitions={
'done': 'is yes',
'nothing': 'say repeate',
'fail': 'say repeate'
},
autonomy={
'done': Autonomy.Off,
'nothing': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'words': 'words'})
# x:91 y:307
OperatableStateMachine.add(
'is yes',
CheckConditionState(predicate=lambda x: "yes" in x or "right"
in x or "sure" in x),
transitions={
'true': 'done',
'false': 'say repeate'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'words'})
# x:423 y:332
OperatableStateMachine.add(
'say repeate',
SaraSay(sentence="Please, repeat your command.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'bad'},
autonomy={'done': Autonomy.Off})
# x:30 y:324
_sm_look_at_op_1 = OperatableStateMachine(outcomes=['fail'])
with _sm_look_at_op_1:
# x:61 y:31
OperatableStateMachine.add('set name',
SetKey(Value="person"),
transitions={'done': 'list persons'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'name'})
# x:44 y:110
OperatableStateMachine.add('list persons',
list_entities_by_name(
frontality_level=0.5,
distance_max=10),
transitions={
'found': 'get closest',
'none_found': 'list persons'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'name',
'entity_list': 'entity_list',
'number': 'number'
})
# x:43 y:211
OperatableStateMachine.add(
'get closest',
CalculationState(calculation=lambda x: x[0].ID),
transitions={'done': 'look at'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'entity_list',
'output_value': 'ID'
})
# x:210 y:198
OperatableStateMachine.add('look at',
KeepLookingAt(),
transitions={'failed': 'list persons'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:307 y:35, x:335 y:491
_sm_get_commands_2 = OperatableStateMachine(
outcomes=['fail', 'understood'],
output_keys=['ActionForms', 'sentence'])
with _sm_get_commands_2:
# x:50 y:48
OperatableStateMachine.add(
'say ready',
SaraSay(sentence="I'm ready for your commands.",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'GetSpeech'},
autonomy={'done': Autonomy.Off})
# x:32 y:408
OperatableStateMachine.add('SaraNLUgpsr',
SaraNLUgpsr(),
transitions={
'understood': 'say understood',
'not_understood': 'say sorry',
'fail': 'say sorry'
},
autonomy={
'understood': Autonomy.Off,
'not_understood': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'sentence': 'sentence',
'ActionForms': 'ActionForms'
})
# x:597 y:223
OperatableStateMachine.add(
'say sorry',
SaraSay(
sentence="Sorry, I could not understand what you said.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'GetSpeech'},
autonomy={'done': Autonomy.Off})
# x:30 y:491
OperatableStateMachine.add('say understood',
SaraSay(sentence="Ok",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'understood'},
autonomy={'done': Autonomy.Off})
# x:44 y:131
OperatableStateMachine.add('GetSpeech',
GetSpeech(watchdog=5),
transitions={
'done': 'validate',
'nothing': 'GetSpeech',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'nothing': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'words': 'sentence'})
# x:29 y:259
OperatableStateMachine.add('validate',
_sm_validate_0,
transitions={
'done': 'SaraNLUgpsr',
'bad': 'GetSpeech'
},
autonomy={
'done': Autonomy.Inherit,
'bad': Autonomy.Inherit
},
remapping={'sentence': 'sentence'})
# x:122 y:413
_sm_end_3 = OperatableStateMachine(outcomes=['done'],
input_keys=['ExitName'])
with _sm_end_3:
# x:30 y:40
OperatableStateMachine.add(
'win',
SaraSay(sentence="Thank you. I'm going now.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'Action_Pass_Door'},
autonomy={'done': Autonomy.Off})
# x:33 y:163
OperatableStateMachine.add(
'Action_Pass_Door',
self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM,
'End/Action_Pass_Door'),
transitions={
'Done': 'say yay',
'Fail': 'done'
},
autonomy={
'Done': Autonomy.Inherit,
'Fail': Autonomy.Inherit
},
remapping={'DoorName': 'ExitName'})
# x:210 y:262
OperatableStateMachine.add(
'say yay',
SaraSay(sentence="I did it. I'm the best robot.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:320 y:82, x:322 y:143, x:265 y:407, x:306 y:225, x:430 y:324
_sm_interact_operator_4 = ConcurrencyContainer(
outcomes=['fail', 'understood'],
output_keys=['ActionForms', 'sentence'],
conditions=[('understood', [('Get Commands', 'understood')]),
('fail', [('Get Commands', 'fail')]),
('fail', [('look at op', 'fail')])])
with _sm_interact_operator_4:
# x:95 y:45
OperatableStateMachine.add('Get Commands',
_sm_get_commands_2,
transitions={
'fail': 'fail',
'understood': 'understood'
},
autonomy={
'fail': Autonomy.Inherit,
'understood': Autonomy.Inherit
},
remapping={
'ActionForms': 'ActionForms',
'sentence': 'sentence'
})
# x:99 y:198
OperatableStateMachine.add('look at op',
_sm_look_at_op_1,
transitions={'fail': 'fail'},
autonomy={'fail': Autonomy.Inherit})
# x:325 y:387, x:314 y:190
_sm_fail_state_5 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['StartPosition'])
with _sm_fail_state_5:
# x:36 y:29
OperatableStateMachine.add(
'say failed',
SaraSay(sentence="I failed. I'm going back to tell my master.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:42 y:301
OperatableStateMachine.add(
'get error',
GetRosParam(ParamName="behavior/GPSR/CauseOfFailure"),
transitions={
'done': 'Say_Error',
'failed': 'finished'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'Value': 'Error'})
# x:48 y:414
OperatableStateMachine.add(
'Say_Error',
SaraSay(sentence=lambda x: "Sorry, I failed because " + x,
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:25 y:172
OperatableStateMachine.add(
'Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM,
'Fail state/Action_Move'),
transitions={
'finished': 'get error',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pose': 'StartPosition'})
# x:588 y:141, x:590 y:545, x:642 y:410
_sm_do_the_actions_6 = OperatableStateMachine(
outcomes=['finished', 'failed', 'critical fail'],
input_keys=['ActionForms', 'OriginalPose'])
with _sm_do_the_actions_6:
# x:85 y:33
OperatableStateMachine.add('set i',
SetKey(Value=0),
transitions={'done': 'is form?'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'index'})
# x:48 y:339
OperatableStateMachine.add(
'GetForm',
FlexibleCalculationState(calculation=lambda x: x[0][x[1]],
input_keys=["ActionForms", "index"]),
transitions={'done': 'Action_Executor'},
autonomy={'done': Autonomy.Off},
remapping={
'ActionForms': 'ActionForms',
'index': 'index',
'output_value': 'ActionForm'
})
# x:57 y:131
OperatableStateMachine.add(
'is form?',
FlexibleCheckConditionState(
predicate=lambda x: x[1] < len(x[0]),
input_keys=["ActionForms", "index"]),
transitions={
'true': 'set setp',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'ActionForms': 'ActionForms',
'index': 'index'
})
# x:204 y:237
OperatableStateMachine.add(
'++i',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'is form?'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'index',
'output_value': 'index'
})
# x:72 y:479
OperatableStateMachine.add(
'Action_Executor',
self.use_behavior(sara_flexbe_behaviors__Action_ExecutorSM,
'Do the actions/Action_Executor'),
transitions={
'finished': '++i',
'failed': 'failed',
'critical_fail': 'critical fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical_fail': Autonomy.Inherit
},
remapping={'Action': 'ActionForm'})
# x:45 y:241
OperatableStateMachine.add('set setp',
StoryboardSetStepKey(),
transitions={'done': 'GetForm'},
autonomy={'done': Autonomy.Off},
remapping={'step': 'index'})
# x:424 y:175
_sm_initialisation_7 = OperatableStateMachine(
outcomes=['done'], input_keys=['PositionBras', 'EntryName'])
with _sm_initialisation_7:
# x:62 y:79
OperatableStateMachine.add(
'Action_Pass_Door',
self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM,
'Initialisation/Action_Pass_Door'),
transitions={
'Done': 'set step',
'Fail': 'done'
},
autonomy={
'Done': Autonomy.Inherit,
'Fail': Autonomy.Inherit
},
remapping={'DoorName': 'EntryName'})
# x:70 y:306
OperatableStateMachine.add('set story',
Set_Story(titre="GPSR", storyline=[]),
transitions={'done': 'bras en lair'},
autonomy={'done': Autonomy.Off})
# x:288 y:375
OperatableStateMachine.add(
'say start',
SaraSay(sentence="I'm ready to start the GPSR scenario.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:78 y:205
OperatableStateMachine.add('set step',
Set_a_step(step=0),
transitions={'done': 'set story'},
autonomy={'done': Autonomy.Off})
# x:101 y:395
OperatableStateMachine.add('bras en lair',
MoveitMove(move=True,
waitForExecution=False,
group="RightArm"),
transitions={
'done': 'say start',
'failed': 'say start'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PositionBras'})
with _state_machine:
# x:33 y:103
OperatableStateMachine.add('Initialisation',
_sm_initialisation_7,
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Inherit},
remapping={
'PositionBras': 'PositionBras',
'EntryName': 'EntryName'
})
# x:767 y:196
OperatableStateMachine.add('Do the actions',
_sm_do_the_actions_6,
transitions={
'finished': 'say succseed',
'failed': 'Fail state',
'critical fail': 'critical'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical fail': Autonomy.Inherit
},
remapping={
'ActionForms': 'ActionForms',
'OriginalPose': 'OriginalPose'
})
# x:250 y:126
OperatableStateMachine.add(
'Fail state',
_sm_fail_state_5,
transitions={
'finished': 'lift head',
'failed': 'critical'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'StartPosition': 'StartPosition'})
# x:250 y:257
OperatableStateMachine.add(
'critical',
SaraSay(
sentence="Critical failure! I require medical assistance.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:52 y:468
OperatableStateMachine.add('for 3',
ForLoop(repeat=3),
transitions={
'do': 'GetOriginalPose',
'end': 'End'
},
autonomy={
'do': Autonomy.Off,
'end': Autonomy.Off
},
remapping={'index': 'index'})
# x:747 y:475
OperatableStateMachine.add('say succseed',
SaraSay(
sentence="I succeed my mission.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'for 3'},
autonomy={'done': Autonomy.Off})
# x:204 y:368
OperatableStateMachine.add('Interact operator',
_sm_interact_operator_4,
transitions={
'fail': 'critical',
'understood': 'set step'
},
autonomy={
'fail': Autonomy.Inherit,
'understood': Autonomy.Inherit
},
remapping={
'ActionForms': 'ActionForms',
'sentence': 'sentence'
})
# x:553 y:316
OperatableStateMachine.add('set story',
StoryboardSetStoryFromAction(),
transitions={'done': 'Do the actions'},
autonomy={'done': Autonomy.Off},
remapping={
'titre': 'title',
'actionList': 'ActionForms'
})
# x:404 y:377
OperatableStateMachine.add('set step',
Set_a_step(step=0),
transitions={'done': 'set story'},
autonomy={'done': Autonomy.Off})
# x:41 y:375
OperatableStateMachine.add(
'GetOriginalPose',
Get_Robot_Pose(),
transitions={'done': 'Interact operator'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'OriginalPose'})
# x:37 y:564
OperatableStateMachine.add('End',
_sm_end_3,
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Inherit},
remapping={'ExitName': 'ExitName'})
# x:30 y:288
OperatableStateMachine.add('lift head',
SaraSetHeadAngle(pitch=-0.3, yaw=0),
transitions={'done': 'GetOriginalPose'},
autonomy={'done': Autonomy.Off})
# x:48 y:23
OperatableStateMachine.add('ContinueButton',
ContinueButton(),
transitions={
'true': 'Initialisation',
'false': 'Initialisation'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
})
# x:21 y:185
OperatableStateMachine.add(
'Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM,
'Action_Move'),
transitions={
'finished': 'lift head',
'failed': 'critical'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pose': 'StartPosition'})
return _state_machine
def create(self):
# x: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:902 y:757, x:882 y:161
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:1232 y:907, x:1267 y:613, x:1271 y:566
_sm_find_table_and_place_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'no_table'])
with _sm_find_table_and_place_0:
# x:877 y:673
OperatableStateMachine.add('run traj',
RunTrajectory(file="receive_object", duration=0),
transitions={'done': 'say release'},
autonomy={'done': Autonomy.Off})
# x:532 y:572
OperatableStateMachine.add('Action_find_2',
self.use_behavior(Action_findSM, 'take objects and bring the order to customer/find table and place/Action_find_2'),
transitions={'done': 'get table position', 'failed': 'say do not find table'},
autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'className': 'tableKey', 'entity': 'tableEntity'})
# x:576 y:514
OperatableStateMachine.add('set tableKey',
SetKey(Value="table"),
transitions={'done': 'Action_find_2'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'tableKey'})
# x:538 y:638
OperatableStateMachine.add('get table position',
CalculationState(calculation=lambda x: x.position),
transitions={'done': 'Action_place'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'tableEntity', 'output_value': 'pose'})
# x:736 y:677
OperatableStateMachine.add('say do not find table',
SaraSay(sentence="I can not find the table. Please be ready to grab the object.", input_keys=[], emotion=0, block=True),
transitions={'done': 'run traj'},
autonomy={'done': Autonomy.Off})
# x:41 y:231
OperatableStateMachine.add('find a table and a free spot',
GetPositionToPlaceOnTable(),
transitions={'done': 'Action_place', 'not_found': 'look down_left'},
autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'})
# x:291 y:805
OperatableStateMachine.add('Action_place',
self.use_behavior(Action_placeSM, 'take objects and bring the order to customer/find table and place/Action_place'),
transitions={'finished': 'finished', 'failed': 'say do not find table'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pos': 'pose'})
# x:50 y:102
OperatableStateMachine.add('look down',
SaraSetHeadAngle(pitch=0, yaw=0.7),
transitions={'done': 'wait'},
autonomy={'done': Autonomy.Off})
# x:214 y:378
OperatableStateMachine.add('find a table and a free spot_2',
GetPositionToPlaceOnTable(),
transitions={'done': 'Action_place', 'not_found': 'look down_right'},
autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'})
# x:240 y:232
OperatableStateMachine.add('look down_left',
SaraSetHeadAngle(pitch=0.7, yaw=0.6),
transitions={'done': 'wait_2'},
autonomy={'done': Autonomy.Off})
# x:387 y:513
OperatableStateMachine.add('find a table and a free spot_2_2',
GetPositionToPlaceOnTable(),
transitions={'done': 'Action_place', 'not_found': 'set tableKey'},
autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'})
# x:402 y:378
OperatableStateMachine.add('look down_right',
SaraSetHeadAngle(pitch=-0.7, yaw=0.6),
transitions={'done': 'wait_3'},
autonomy={'done': Autonomy.Off})
# x:66 y:169
OperatableStateMachine.add('wait',
WaitState(wait_time=3),
transitions={'done': 'find a table and a free spot'},
autonomy={'done': Autonomy.Off})
# x:242 y:294
OperatableStateMachine.add('wait_2',
WaitState(wait_time=3),
transitions={'done': 'find a table and a free spot_2'},
autonomy={'done': Autonomy.Off})
# x:411 y:444
OperatableStateMachine.add('wait_3',
WaitState(wait_time=3),
transitions={'done': 'find a table and a free spot_2_2'},
autonomy={'done': Autonomy.Off})
# x:1000 y:670
OperatableStateMachine.add('say release',
SaraSay(sentence="Here is your order. I will open my gripper in 3, 2, 1, now.", input_keys=[], emotion=0, block=True),
transitions={'done': 'open gripper'},
autonomy={'done': Autonomy.Off})
# x:1122 y:666
OperatableStateMachine.add('open gripper',
SetGripperState(width=0.1, effort=1),
transitions={'object': 'say good day', 'no_object': 'say good day'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:1170 y:771
OperatableStateMachine.add('say good day',
SaraSay(sentence="Thank you. Have a good day.", input_keys=[], emotion=6, block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:62 y:29
OperatableStateMachine.add('set distance',
SetKey(Value=0.3),
transitions={'done': 'look down'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distanceFromEdge'})
# x:30 y:458, x:130 y:458
_sm_keep_looking_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['personID'])
with _sm_keep_looking_1:
# x:30 y:40
OperatableStateMachine.add('keep',
KeepLookingAt(),
transitions={'failed': 'keep'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'personID'})
# x:30 y:458, x:130 y:458
_sm_ask_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['question'], output_keys=['answer'])
with _sm_ask_2:
# x:30 y:40
OperatableStateMachine.add('Action_Ask',
self.use_behavior(Action_AskSM, 'ask and save order/ask AND look person/ask/Action_Ask'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'question': 'question', 'answer': 'answer'})
# x:30 y:458, x:130 y:458
_sm_confirm_order_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['orderList'])
with _sm_confirm_order_3:
# x:114 y:67
OperatableStateMachine.add('produce question',
CalculationState(calculation=lambda x: "Did you order :" + str(x[0].data)),
transitions={'done': 'Action_Ask'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'orderList', 'output_value': 'question'})
# x:423 y:171
OperatableStateMachine.add('Action_Ask',
self.use_behavior(Action_AskSM, 'ask and save order/confirm order/Action_Ask'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'question': 'question', 'answer': 'answer'})
# x:348 y:66, x:351 y:97, x:339 y:159, x:336 y:195, x:518 y:89, x:515 y:175
_sm_ask_and_look_person_4 = ConcurrencyContainer(outcomes=['finished', 'failed'], input_keys=['personID', 'question'], output_keys=['answer'], conditions=[
('finished', [('ask', 'finished')]),
('failed', [('ask', 'failed')]),
('finished', [('keep looking', 'finished')]),
('failed', [('keep looking', 'failed')])
])
with _sm_ask_and_look_person_4:
# x:30 y:40
OperatableStateMachine.add('ask',
_sm_ask_2,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'question': 'question', 'answer': 'answer'})
# x:30 y:138
OperatableStateMachine.add('keep looking',
_sm_keep_looking_1,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'personID': 'personID'})
# x:30 y:458
_sm_look_around_2_5 = OperatableStateMachine(outcomes=['finished'])
with _sm_look_around_2_5:
# x:215 y:156
OperatableStateMachine.add('center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w2'},
autonomy={'done': Autonomy.Off})
# x:394 y:262
OperatableStateMachine.add('right',
SaraSetHeadAngle(pitch=0.1, yaw=-1.5),
transitions={'done': 'w3'},
autonomy={'done': Autonomy.Off})
# x:206 y:372
OperatableStateMachine.add('center2',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w4'},
autonomy={'done': Autonomy.Off})
# x:38 y:596
OperatableStateMachine.add('w1',
WaitState(wait_time=4),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:413 y:159
OperatableStateMachine.add('w2',
WaitState(wait_time=4),
transitions={'done': 'right'},
autonomy={'done': Autonomy.Off})
# x:415 y:370
OperatableStateMachine.add('w3',
WaitState(wait_time=4),
transitions={'done': 'center2'},
autonomy={'done': Autonomy.Off})
# x:39 y:371
OperatableStateMachine.add('w4',
WaitState(wait_time=4),
transitions={'done': 'left'},
autonomy={'done': Autonomy.Off})
# x:25 y:482
OperatableStateMachine.add('left',
SaraSetHeadAngle(pitch=0.1, yaw=1.5),
transitions={'done': 'w1'},
autonomy={'done': Autonomy.Off})
# x:30 y:458
_sm_look_around_6 = OperatableStateMachine(outcomes=['finished'])
with _sm_look_around_6:
# x:215 y:156
OperatableStateMachine.add('center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w2'},
autonomy={'done': Autonomy.Off})
# x:394 y:262
OperatableStateMachine.add('right',
SaraSetHeadAngle(pitch=0.1, yaw=-1.5),
transitions={'done': 'w3'},
autonomy={'done': Autonomy.Off})
# x:206 y:372
OperatableStateMachine.add('center2',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w4'},
autonomy={'done': Autonomy.Off})
# x:38 y:596
OperatableStateMachine.add('w1',
WaitState(wait_time=4),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:413 y:159
OperatableStateMachine.add('w2',
WaitState(wait_time=4),
transitions={'done': 'right'},
autonomy={'done': Autonomy.Off})
# x:415 y:370
OperatableStateMachine.add('w3',
WaitState(wait_time=4),
transitions={'done': 'center2'},
autonomy={'done': Autonomy.Off})
# x:39 y:371
OperatableStateMachine.add('w4',
WaitState(wait_time=4),
transitions={'done': 'left'},
autonomy={'done': Autonomy.Off})
# x:25 y:482
OperatableStateMachine.add('left',
SaraSetHeadAngle(pitch=0.1, yaw=1.5),
transitions={'done': 'w1'},
autonomy={'done': Autonomy.Off})
# x:30 y:458, x:130 y:458
_sm_turn_around_7 = OperatableStateMachine(outcomes=['finished', 'failed'])
with _sm_turn_around_7:
# x:80 y:26
OperatableStateMachine.add('look around',
_sm_look_around_6,
transitions={'finished': 'pose with 180 rotation'},
autonomy={'finished': Autonomy.Inherit})
# x:66 y:286
OperatableStateMachine.add('rotation',
SaraMoveBase(reference="base_link"),
transitions={'arrived': 'look around_2', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'pose': 'pose'})
# x:56 y:388
OperatableStateMachine.add('look around_2',
_sm_look_around_2_5,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit})
# x:63 y:133
OperatableStateMachine.add('pose with 180 rotation',
GenPoseEuler(x=0, y=0, z=0, roll=0, pitch=0, yaw=3.14),
transitions={'done': 'rotation'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'pose'})
# x:30 y:458
_sm_get_waving_people_8 = OperatableStateMachine(outcomes=['finished'], output_keys=['waving_person'])
with _sm_get_waving_people_8:
# x:47 y:45
OperatableStateMachine.add('set name',
SetKey(Value="person"),
transitions={'done': 'list people'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'name'})
# x:40 y:236
OperatableStateMachine.add('filter waving',
FilterKey(filter_function=lambda x: x[0].pose.right_arm_up or x[0].pose.left_arm_up, input_keys=["input_list"]),
transitions={'not_empty': 'get first', 'empty': 'list people'},
autonomy={'not_empty': Autonomy.Off, 'empty': Autonomy.Off},
remapping={'input_list': 'entity_list', 'output_list': 'waving_peoples'})
# x:53 y:326
OperatableStateMachine.add('get first',
CalculationState(calculation=lambda x: x[0]),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'waving_peoples', 'output_value': 'waving_person'})
# x:40 y:133
OperatableStateMachine.add('list people',
list_entities_by_name(frontality_level=0.5, distance_max=5),
transitions={'found': 'filter waving', 'none_found': 'list people'},
autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off},
remapping={'name': 'name', 'entity_list': 'entity_list', 'number': 'number'})
# x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458
_sm_look_for_waving_people_9 = ConcurrencyContainer(outcomes=['finished', 'failed'], output_keys=['waving_person'], conditions=[
('failed', [('turn around', 'failed')]),
('failed', [('turn around', 'finished')]),
('finished', [('get waving people', 'finished')])
])
with _sm_look_for_waving_people_9:
# x:268 y:118
OperatableStateMachine.add('get waving people',
_sm_get_waving_people_8,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit},
remapping={'waving_person': 'waving_person'})
# x:60 y:126
OperatableStateMachine.add('turn around',
_sm_turn_around_7,
transitions={'finished': 'failed', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:441 y:583, x:93 y:568
_sm_repeate_if_first_commande_10 = OperatableStateMachine(outcomes=['finished', 'repeate'], input_keys=['commandNumber'], output_keys=['commandNumber'])
with _sm_repeate_if_first_commande_10:
# x:242 y:97
OperatableStateMachine.add('if first command',
CheckConditionState(predicate=lambda x: x == 1),
transitions={'true': 'set second command', 'false': 'finished'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'commandNumber'})
# x:73 y:329
OperatableStateMachine.add('set second command',
SetKey(Value=2),
transitions={'done': 'repeate'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'commandNumber'})
# x:1053 y:687, x:1068 y:149
_sm_take_objects_and_bring_the_order_to_customer_11 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['barPosition', 'orderList', 'robotPositionToCustomer'])
with _sm_take_objects_and_bring_the_order_to_customer_11:
# x:73 y:26
OperatableStateMachine.add('set indexkey',
SetKey(Value=0),
transitions={'done': 'one element by one element from the list'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'indexKey'})
# x:591 y:684
OperatableStateMachine.add('increment indexKey',
CalculationState(calculation=lambda x: x+1),
transitions={'done': 'check if end of the list'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'indexKey', 'output_value': 'indexKey'})
# x:796 y:679
OperatableStateMachine.add('check if end of the list',
FlexibleCheckConditionState(predicate=lambda x: len(x[0]) <= x[1], input_keys=["orderList", "indexKey"]),
transitions={'true': 'finished', 'false': 'Action_Move_2'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'orderList': 'orderList', 'indexKey': 'indexKey'})
# x:31 y:243
OperatableStateMachine.add('Action_find',
self.use_behavior(Action_findSM, 'take objects and bring the order to customer/Action_find'),
transitions={'done': 'get entity ID', 'failed': 'say cannot find'},
autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'className': 'item', 'entity': 'entity'})
# x:76 y:183
OperatableStateMachine.add('say search and grip',
SaraSay(sentence=lambda x: "I am now searching for the "+x[0]+".", input_keys=["item"], emotion=0, block=True),
transitions={'done': 'Action_find'},
autonomy={'done': Autonomy.Off},
remapping={'item': 'item'})
# x:258 y:261
OperatableStateMachine.add('say cannot find',
SaraSay(sentence="I cannot find the item.", input_keys=[], emotion=0, block=True),
transitions={'done': 'place arm'},
autonomy={'done': Autonomy.Off})
# x:63 y:393
OperatableStateMachine.add('Action_pick',
self.use_behavior(Action_pickSM, 'take objects and bring the order to customer/Action_pick'),
transitions={'success': 'say go to the customer', 'unreachable': 'say cannot pick', 'not found': 'say cannot pick', 'dropped': 'say cannot pick'},
autonomy={'success': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'not found': Autonomy.Inherit, 'dropped': Autonomy.Inherit},
remapping={'objectID': 'entityID'})
# x:61 y:548
OperatableStateMachine.add('Action_Move',
self.use_behavior(Action_MoveSM, 'take objects and bring the order to customer/Action_Move'),
transitions={'finished': 'find table and place', 'failed': 'say cant get back to customer'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'robotPositionToCustomer'})
# x:83 y:326
OperatableStateMachine.add('get entity ID',
CalculationState(calculation=lambda x: x.ID),
transitions={'done': 'Action_pick'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'entity', 'output_value': 'entityID'})
# x:568 y:407
OperatableStateMachine.add('say put in the gripper',
SaraSay(sentence=lambda x: "Please, put the "+x[0]+" in my gripper. I will close it in 3, 2, 1, now.", input_keys=["item"], emotion=0, block=True),
transitions={'done': 'close gripper'},
autonomy={'done': Autonomy.Off},
remapping={'item': 'item'})
# x:243 y:387
OperatableStateMachine.add('say cannot pick',
SaraSay(sentence="I can not pick the item.", input_keys=[], emotion=0, block=True),
transitions={'done': 'place arm'},
autonomy={'done': Autonomy.Off})
# x:388 y:502
OperatableStateMachine.add('say thank you',
SaraSay(sentence="Thank you.", input_keys=[], emotion=0, block=True),
transitions={'done': 'place arm 2'},
autonomy={'done': Autonomy.Off})
# x:72 y:465
OperatableStateMachine.add('say go to the customer',
SaraSay(sentence="I will serve it to the customer.", input_keys=[], emotion=0, block=True),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:522 y:505
OperatableStateMachine.add('close gripper',
SetGripperState(width=0, effort=1),
transitions={'object': 'say thank you', 'no_object': 'open gripper'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:357 y:569
OperatableStateMachine.add('say cant get back to customer',
SaraSay(sentence="I am not able to go back to the customer.", input_keys=[], emotion=0, block=True),
transitions={'done': 'increment indexKey'},
autonomy={'done': Autonomy.Off})
# x:541 y:317
OperatableStateMachine.add('open gripper',
SetGripperState(width=0.10, effort=1),
transitions={'object': 'say put in the gripper', 'no_object': 'say put in the gripper'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:63 y:110
OperatableStateMachine.add('one element by one element from the list',
FlexibleCalculationState(calculation=lambda x: x[0][x[1]].data, input_keys=["orderList", "indexKey"]),
transitions={'done': 'say search and grip'},
autonomy={'done': Autonomy.Off},
remapping={'orderList': 'orderList', 'indexKey': 'indexKey', 'output_value': 'item'})
# x:376 y:324
OperatableStateMachine.add('place arm',
RunTrajectory(file="receive_object", duration=0),
transitions={'done': 'open gripper'},
autonomy={'done': Autonomy.Off})
# x:638 y:184
OperatableStateMachine.add('Action_Move_2',
self.use_behavior(Action_MoveSM, 'take objects and bring the order to customer/Action_Move_2'),
transitions={'finished': 'increment indexKey', 'failed': 'say cannot go back to bar'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'barPosition'})
# x:817 y:178
OperatableStateMachine.add('say cannot go back to bar',
SaraSay(sentence="I am not able to go back to the barman. I will try a new command.", input_keys=[], emotion=0, block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:85 y:740
OperatableStateMachine.add('find table and place',
_sm_find_table_and_place_0,
transitions={'finished': 'retour repos', 'failed': 'retour repos', 'no_table': 'retour repos'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'no_table': Autonomy.Inherit})
# x:248 y:505
OperatableStateMachine.add('place arm 2',
RunTrajectory(file="transport", duration=0),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:362 y:679
OperatableStateMachine.add('retour repos',
RunTrajectory(file="repos", duration=0),
transitions={'done': 'increment indexKey'},
autonomy={'done': Autonomy.Off})
# x:242 y:352, x:1086 y:243
_sm_get_the_order_12 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['orderList'])
with _sm_get_the_order_12:
# x:68 y:39
OperatableStateMachine.add('length 1',
CheckConditionState(predicate=lambda x: len(x) == 1),
transitions={'true': 'say order', 'false': 'length 2'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'orderList'})
# x:236 y:41
OperatableStateMachine.add('length 2',
CheckConditionState(predicate=lambda x: len(x) ==2),
transitions={'true': 'say order_2', 'false': 'say order_3'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'orderList'})
# x:228 y:111
OperatableStateMachine.add('say order_2',
SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+" and one "+x[0][1].data+", please.", input_keys=["orderList"], emotion=0, block=True),
transitions={'done': 'wait 5'},
autonomy={'done': Autonomy.Off},
remapping={'orderList': 'orderList'})
# x:374 y:109
OperatableStateMachine.add('say order_3',
SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+", one "+x[0][1].data+" and one "+x[0][2].data+", please.", input_keys=["orderList"], emotion=0, block=True),
transitions={'done': 'wait 5'},
autonomy={'done': Autonomy.Off},
remapping={'orderList': 'orderList'})
# x:67 y:117
OperatableStateMachine.add('say order',
SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+", please.", input_keys=["orderList"], emotion=0, block=True),
transitions={'done': 'wait 5'},
autonomy={'done': Autonomy.Off},
remapping={'orderList': 'orderList'})
# x:224 y:256
OperatableStateMachine.add('wait 5',
WaitState(wait_time=0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:216 y:528, x:572 y:238
_sm_go_to_the_barman_13 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['barPosition', 'barmanID'])
with _sm_go_to_the_barman_13:
# x:30 y:102
OperatableStateMachine.add('Action_Move',
self.use_behavior(Action_MoveSM, 'go to the barman/Action_Move'),
transitions={'finished': 'if barman id is 0', 'failed': 'say cannot'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'barPosition'})
# x:365 y:99
OperatableStateMachine.add('say cannot',
SaraSay(sentence="I can not reach my destination. I will take another order. If you have one, please raise your hand.", input_keys=[], emotion=0, block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:30 y:320
OperatableStateMachine.add('Action_findPersonByID',
self.use_behavior(Action_findPersonByIDSM, 'go to the barman/Action_findPersonByID',
default_keys=['className']),
transitions={'found': 'finished', 'not_found': 'finished'},
autonomy={'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit},
remapping={'personID': 'barmanID', 'personEntity': 'personEntity'})
# x:115 y:230
OperatableStateMachine.add('if barman id is 0',
CheckConditionState(predicate=lambda x: x == 0),
transitions={'true': 'finished', 'false': 'Action_findPersonByID'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'barmanID'})
# x:898 y:561, x:871 y:123
_sm_ask_and_save_order_14 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['customerID'], output_keys=['orderList'])
with _sm_ask_and_save_order_14:
# x:70 y:112
OperatableStateMachine.add('say ready',
SaraSay(sentence="Hello.", input_keys=[], emotion=0, block=True),
transitions={'done': 'set question'},
autonomy={'done': Autonomy.Off})
# x:69 y:187
OperatableStateMachine.add('set question',
SetKey(Value="What do you want to order?"),
transitions={'done': 'ask AND look person'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'question'})
# x:411 y:138
OperatableStateMachine.add('say cannot',
SaraSay(sentence="I am not able to understand your order. I will take another order.", input_keys=[], emotion=0, block=True),
transitions={'done': 'set question'},
autonomy={'done': Autonomy.Off})
# x:59 y:272
OperatableStateMachine.add('ask AND look person',
_sm_ask_and_look_person_4,
transitions={'finished': 'nlu restaurant', 'failed': 'say cannot'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'personID': 'customerID', 'question': 'question', 'answer': 'answer'})
# x:411 y:453
OperatableStateMachine.add('nlu restaurant',
SaraNLUrestaurant(),
transitions={'understood': 'confirm order', 'fail': 'say cannot'},
autonomy={'understood': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'sentence': 'answer', 'orderList': 'orderList'})
# x:657 y:417
OperatableStateMachine.add('confirm order',
_sm_confirm_order_3,
transitions={'finished': 'say bring', 'failed': 'set question'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'orderList': 'orderList'})
# x:852 y:402
OperatableStateMachine.add('say bring',
SaraSay(sentence=lambda x: "understood. I will bring you the "+str(x[0][0].data), input_keys=["orderList"], emotion=0, block=False),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'orderList': 'orderList'})
# x:677 y:318, x:735 y:160
_sm_move_to_table_and_save_position_15 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['customerPosition'], output_keys=['robotPositionToCustomer'])
with _sm_move_to_table_and_save_position_15:
# x:69 y:24
OperatableStateMachine.add('set distance to person',
SetKey(Value=0.7),
transitions={'done': 'compute robot pose to customer'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distanceToPerson'})
# x:344 y:115
OperatableStateMachine.add('say cannot',
SaraSay(sentence="I can not reach my destination. I will take another order. If you have one, please raise your hand.", input_keys=[], emotion=0, block=True),
transitions={'done': 'save pose to not get flexbe problems'},
autonomy={'done': Autonomy.Off})
# x:497 y:129
OperatableStateMachine.add('save pose to not get flexbe problems',
Get_Robot_Pose(),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'pose'})
# x:47 y:98
OperatableStateMachine.add('compute robot pose to customer',
Get_Reacheable_Waypoint(),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off},
remapping={'pose_in': 'customerPosition', 'distance': 'distanceToPerson', 'pose_out': 'robotPositionToCustomer'})
# x:78 y:199
OperatableStateMachine.add('Action_Move',
self.use_behavior(Action_MoveSM, 'move to table and save position/Action_Move'),
transitions={'finished': 'finished', 'failed': 'say cannot'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'robotPositionToCustomer'})
# x:112 y:525, x:363 y:46
_sm_detect_people_waving_16 = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['customerPosition', 'customerID'])
with _sm_detect_people_waving_16:
# x:84 y:25
OperatableStateMachine.add('say looking',
SaraSay(sentence="I am now looking for customers. If you want to order something, please raise your hand.", input_keys=[], emotion=0, block=False),
transitions={'done': 'Look for waving people'},
autonomy={'done': Autonomy.Off})
# x:73 y:375
OperatableStateMachine.add('reachable position_2',
Get_Reacheable_Waypoint(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'pose_in': 'customerPosition', 'distance': 'distance', 'pose_out': 'customerPosition'})
# x:71 y:291
OperatableStateMachine.add('set distance to customer_2',
SetKey(Value=0.5),
transitions={'done': 'reachable position_2'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distance'})
# x:94 y:216
OperatableStateMachine.add('get attributes',
GetAttribute(attributes=["ID", "position"]),
transitions={'done': 'set distance to customer_2'},
autonomy={'done': Autonomy.Off},
remapping={'object': 'waving_person', 'ID': 'customerID', 'position': 'customerPosition'})
# x:63 y:120
OperatableStateMachine.add('Look for waving people',
_sm_look_for_waving_people_9,
transitions={'finished': 'get attributes', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'waving_person': 'waving_person'})
# x:874 y:499, x:747 y:100
_sm_save_bar_position_and_initiation_17 = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['barPosition', 'commandNumber', 'barmanID'])
with _sm_save_bar_position_and_initiation_17:
# x:66 y:96
OperatableStateMachine.add('set question barman',
SetKey(Value="Are you the bartender?"),
transitions={'done': 'Action_FindPersonByQuestion'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'questionBarman'})
# x:488 y:85
OperatableStateMachine.add('say failed to find the barman',
SaraSay(sentence="I am not able to find the barman. I will get the order and come back here to get the items.", input_keys=[], emotion=0, block=True),
transitions={'done': 'barman ID to 0'},
autonomy={'done': Autonomy.Off})
# x:83 y:406
OperatableStateMachine.add('save barman ID',
CalculationState(calculation=lambda x: x.ID),
transitions={'done': 'get current pose'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'personFound', 'output_value': 'barmanID'})
# x:512 y:183
OperatableStateMachine.add('barman ID to 0',
SetKey(Value=0),
transitions={'done': 'get current pose'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'barmanID'})
# x:488 y:391
OperatableStateMachine.add('get current pose',
Get_Robot_Pose(),
transitions={'done': 'set first command'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'barPosition'})
# x:55 y:214
OperatableStateMachine.add('Action_FindPersonByQuestion',
self.use_behavior(Action_FindPersonByQuestionSM, 'save bar position and initiation/Action_FindPersonByQuestion'),
transitions={'found': 'save barman ID', 'failed': 'say failed to find the barman', 'not_found': 'say failed to find the barman'},
autonomy={'found': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'not_found': Autonomy.Inherit},
remapping={'question': 'questionBarman', 'entityFound': 'personFound'})
# x:489 y:478
OperatableStateMachine.add('set first command',
SetKey(Value=1),
transitions={'done': 'say instructions'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'commandNumber'})
# x:689 y:477
OperatableStateMachine.add('say instructions',
SaraSay(sentence="Good! I wil now start to serve people.", input_keys=[], emotion=0, block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('save bar position and initiation',
_sm_save_bar_position_and_initiation_17,
transitions={'finished': 'detect people waving', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'barPosition': 'barPosition', 'commandNumber': 'commandNumber', 'barmanID': 'barmanID'})
# x:131 y:141
OperatableStateMachine.add('detect people waving',
_sm_detect_people_waving_16,
transitions={'finished': 'move to table and save position', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'customerPosition': 'customerPosition', 'customerID': 'customerID'})
# x:233 y:250
OperatableStateMachine.add('move to table and save position',
_sm_move_to_table_and_save_position_15,
transitions={'finished': 'ask and save order', 'failed': 'detect people waving'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'customerPosition': 'customerPosition', 'robotPositionToCustomer': 'robotPositionToCustomer'})
# x:286 y:337
OperatableStateMachine.add('ask and save order',
_sm_ask_and_save_order_14,
transitions={'finished': 'go to the barman', 'failed': 'detect people waving'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'customerID': 'customerID', 'orderList': 'orderList'})
# x:264 y:434
OperatableStateMachine.add('go to the barman',
_sm_go_to_the_barman_13,
transitions={'finished': 'get the order', 'failed': 'detect people waving'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'barPosition': 'barPosition', 'barmanID': 'barmanID'})
# x:278 y:526
OperatableStateMachine.add('get the order',
_sm_get_the_order_12,
transitions={'finished': 'take objects and bring the order to customer', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'orderList': 'orderList'})
# x:260 y:626
OperatableStateMachine.add('take objects and bring the order to customer',
_sm_take_objects_and_bring_the_order_to_customer_11,
transitions={'finished': 'repeate if first commande', 'failed': 'repeate if first commande'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'barPosition': 'barPosition', 'orderList': 'orderList', 'robotPositionToCustomer': 'robotPositionToCustomer'})
# x:118 y:735
OperatableStateMachine.add('repeate if first commande',
_sm_repeate_if_first_commande_10,
transitions={'finished': 'say finish', 'repeate': 'detect people waving'},
autonomy={'finished': Autonomy.Inherit, 'repeate': Autonomy.Inherit},
remapping={'commandNumber': 'commandNumber'})
# x:431 y:743
OperatableStateMachine.add('say finish',
SaraSay(sentence="I reach the end of my scenario. I will go back to the bar.", input_keys=[], emotion=0, block=True),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:616 y:735
OperatableStateMachine.add('Action_Move',
self.use_behavior(Action_MoveSM, 'Action_Move'),
transitions={'finished': 'finished', 'failed': 'finished'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'barPosition'})
return _state_machine
def create(self):
# x: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: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