def create(self):
# x:30 y:353, x:425 y:365
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:353, x:130 y:353, x:230 y:353, x:330 y:353, x:430 y:353, x:530 y:353
_sm_randheadmovements_0 = ConcurrencyContainer(outcomes=['finished', 'failed'], conditions=[
('finished', [('LookAt', 'done')]),
('failed', [('LookAt', 'failure')]),
('finished', [('RandPose', 'done')]),
('failed', [('RandPose', 'failure')])
])
with _sm_randheadmovements_0:
# x:48 y:117
OperatableStateMachine.add('LookAt',
SetOperational(controller='motion/controller/look_at', operational=True, resources=['head'], sync=True),
transitions={'done': 'finished', 'failure': 'failed'},
autonomy={'done': Autonomy.Off, 'failure': Autonomy.Off})
# x:286 y:122
OperatableStateMachine.add('RandPose',
RandPoseGenerator(topic='motion/controller/look_at/in_pose_ref', duration=self.timeout, interval=[1.0,4.0], maxXYZ=[1,0.3,0.6], minXYZ=[1.0,-0.3,0.2], frame_xyz='base_link', frame_out='odom_combined'),
transitions={'done': 'finished', 'failure': 'failed'},
autonomy={'done': Autonomy.Off, 'failure': Autonomy.Off})
with _state_machine:
# x:163 y:40
OperatableStateMachine.add('CheckCrouched',
CheckJointState(outcomes=['body_crouched', 'unknown'], pose_ns='saved_msgs/joint_state', tolerance=0.17, joint_topic="joint_states", timeout=1.0),
transitions={'body_crouched': 'StandUp', 'unknown': 'RandHeadMovements'},
autonomy={'body_crouched': Autonomy.Off, 'unknown': Autonomy.Off})
# x:412 y:68
OperatableStateMachine.add('StandUp',
ExecuteJointTrajectory(action_topic='motion/controller/joint_trajectory', trajectory_param='crouch_end', trajectory_ns='saved_msgs/joint_trajectory'),
transitions={'success': 'RandHeadMovements', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed'},
autonomy={'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off})
# x:150 y:341
OperatableStateMachine.add('HeadNominal',
SetJointState(controller='motion/controller/joint_state_head', pose_param='head_nominal', pose_ns='saved_msgs/joint_state', tolerance=0.017, timeout=10.0, joint_topic="joint_states"),
transitions={'done': 'finished', 'failed': 'failed', 'timeout': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off, 'timeout': Autonomy.Off})
# x:154 y:179
OperatableStateMachine.add('RandHeadMovements',
_sm_randheadmovements_0,
transitions={'finished': 'HeadNominal', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
return _state_machine
def create(self):
# x:30 y:322, x:130 y:322
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:322, x:130 y:322, x:230 y:322
_sm_container_0 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('finished', [('behavior_exploration', 'finished'),
('GetCloserToVictim', 'done')])])
with _sm_container_0:
# x:30 y:40
OperatableStateMachine.add('behavior_exploration',
self.use_behavior(
behavior_explorationSM,
'Container/behavior_exploration'),
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit})
# x:30 y:122
OperatableStateMachine.add('GetCloserToVictim',
self.use_behavior(
GetCloserToVictimSM,
'Container/GetCloserToVictim'),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Inherit})
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Container',
_sm_container_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
return _state_machine
def create(self):
# x:296 y:78, x:278 y:18
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365, x:530 y:365
_sm_dual_arm_container_0 = ConcurrencyContainer(outcomes=['finished', 'failed'], conditions=[
('finished', [('Single Arm Sample Left', 'finished')]),
('failed', [('Single Arm Sample Left', 'failed')]),
('finished', [('Single Arm Sample Right', 'finished')]),
('failed', [('Single Arm Sample Right', 'failed')])
])
with _sm_dual_arm_container_0:
# x:95 y:90
OperatableStateMachine.add('Single Arm Sample Left',
self.use_behavior(SingleArmSampleSM, 'Dual Arm Container/Single Arm Sample Left',
parameters={'robot_name': "left_arm", 'en_sim': True, 'pose_1': "0.2, 0.25, -0.5"}),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:343 y:89
OperatableStateMachine.add('Single Arm Sample Right',
self.use_behavior(SingleArmSampleSM, 'Dual Arm Container/Single Arm Sample Right',
parameters={'robot_name': "right_arm", 'en_sim': True, 'pose_1': "0.2, -0.25, -0.5"}),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
with _state_machine:
# x:93 y:27
OperatableStateMachine.add('Dual Arm Container',
_sm_dual_arm_container_0,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
return _state_machine
def create(self):
# x:30 y:365, x:129 y:365
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365, x:530 y:365
_sm_container_0 = ConcurrencyContainer(outcomes=['finished', 'failed'], conditions=[
('finished', [('NewExp', 'finished')]),
('failed', [('NewExp', 'failed')]),
('finished', [('Detect', 'continue')]),
('finished', [('Detect', 'found')])
])
with _sm_container_0:
# x:30 y:40
OperatableStateMachine.add('NewExp',
self.use_behavior(NewExpSM, 'Container/NewExp'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:320 y:57
OperatableStateMachine.add('Detect',
Object_Detection(),
transitions={'continue': 'finished', 'found': 'finished'},
autonomy={'continue': Autonomy.Off, 'found': Autonomy.Off},
remapping={'pose': 'pose'})
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Container',
_sm_container_0,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
return _state_machine
def create(self):
# x:93 y:313, x:514 y:143
_state_machine = OperatableStateMachine(outcomes=['found', 'not_found'], input_keys=['className', 'personID'], output_keys=['personEntity'])
_state_machine.userdata.className = "person"
_state_machine.userdata.personID = 0
_state_machine.userdata.personEntity = ""
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:707 y:760
_sm_rotation_0 = OperatableStateMachine(outcomes=['end'], output_keys=['personEntity'])
with _sm_rotation_0:
# x:51 y:38
OperatableStateMachine.add('Set 180 degres',
SetKey(Value=3.1416),
transitions={'done': 'set cpt to 0'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'rotation'})
# x:613 y:470
OperatableStateMachine.add('action_turn',
self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'Container/Rotation/action_turn'),
transitions={'finished': 'check is cpt is 1', 'failed': 'check is cpt is 1'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'rotation': 'rotation'})
# x:421 y:54
OperatableStateMachine.add('Look Right',
SaraSetHeadAngle(pitch=0.1, yaw=-1.5),
transitions={'done': 'w2'},
autonomy={'done': Autonomy.Off})
# x:265 y:56
OperatableStateMachine.add('w1',
WaitState(wait_time=4),
transitions={'done': 'Look Right'},
autonomy={'done': Autonomy.Off})
# x:630 y:56
OperatableStateMachine.add('w2',
WaitState(wait_time=4),
transitions={'done': 'center'},
autonomy={'done': Autonomy.Off})
# x:250 y:177
OperatableStateMachine.add('Look Center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w1'},
autonomy={'done': Autonomy.Off})
# x:618 y:304
OperatableStateMachine.add('Look Left 2',
SaraSetHeadAngle(pitch=0.1, yaw=1.5),
transitions={'done': 'w4'},
autonomy={'done': Autonomy.Off})
# x:612 y:138
OperatableStateMachine.add('center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w3'},
autonomy={'done': Autonomy.Off})
# x:635 y:214
OperatableStateMachine.add('w3',
WaitState(wait_time=4),
transitions={'done': 'Look Left 2'},
autonomy={'done': Autonomy.Off})
# x:636 y:394
OperatableStateMachine.add('w4',
WaitState(wait_time=4),
transitions={'done': 'action_turn'},
autonomy={'done': Autonomy.Off})
# x:59 y:128
OperatableStateMachine.add('set cpt to 0',
SetKey(Value=0),
transitions={'done': 'Look Center'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'cpt'})
# x:400 y:499
OperatableStateMachine.add('check is cpt is 1',
CheckConditionState(predicate=lambda x: x==1),
transitions={'true': 'set entity to unknown', 'false': 'set cpt to 1'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'cpt'})
# x:414 y:210
OperatableStateMachine.add('set cpt to 1',
SetKey(Value=1),
transitions={'done': 'Look Right'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'cpt'})
# x:605 y:659
OperatableStateMachine.add('set entity to unknown',
SetKey(Value="unknown"),
transitions={'done': 'end'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'personEntity'})
# x:683 y:188
_sm_find_entity_1 = OperatableStateMachine(outcomes=['found'], input_keys=['personID'], output_keys=['personEntity'])
with _sm_find_entity_1:
# x:226 y:188
OperatableStateMachine.add('get person',
GetEntityByID(),
transitions={'found': 'found', 'not_found': 'WaitState'},
autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ID': 'personID', 'Entity': 'personEntity'})
# x:194 y:40
OperatableStateMachine.add('WaitState',
WaitState(wait_time=1),
transitions={'done': 'get person'},
autonomy={'done': Autonomy.Off})
# x:372 y:27, x:392 y:217, x:400 y:139, x:330 y:458
_sm_container_2 = ConcurrencyContainer(outcomes=['found', 'not_found'], input_keys=['className', 'personID'], output_keys=['personEntity'], conditions=[
('not_found', [('Rotation', 'end')]),
('found', [('Find Entity', 'found')])
])
with _sm_container_2:
# x:131 y:44
OperatableStateMachine.add('Find Entity',
_sm_find_entity_1,
transitions={'found': 'found'},
autonomy={'found': Autonomy.Inherit},
remapping={'personID': 'personID', 'personEntity': 'personEntity'})
# x:135 y:199
OperatableStateMachine.add('Rotation',
_sm_rotation_0,
transitions={'end': 'not_found'},
autonomy={'end': Autonomy.Inherit},
remapping={'personEntity': 'personEntity'})
with _state_machine:
# x:67 y:42
OperatableStateMachine.add('Look Center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'Container'},
autonomy={'done': Autonomy.Off})
# x:278 y:138
OperatableStateMachine.add('Look Center Not Found',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'not_found'},
autonomy={'done': Autonomy.Off})
# x:63 y:126
OperatableStateMachine.add('Container',
_sm_container_2,
transitions={'found': 'WaitState', 'not_found': 'Look Center Not Found'},
autonomy={'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit},
remapping={'className': 'className', 'personID': 'personID', 'personEntity': 'personEntity'})
# x:67 y:222
OperatableStateMachine.add('WaitState',
WaitState(wait_time=1),
transitions={'done': 'found'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x: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 test_concurrency_container(self):
cc = ConcurrencyContainer(outcomes=['done', 'error'],
conditions=[('error', [('main', 'error')]),
('error', [('side', 'error')]),
('done', [('main', 'done'),
('side', 'done')])])
with cc:
OperatableStateMachine.add('main',
TestSubjectState(),
transitions={
'done': 'done',
'error': 'error'
},
autonomy={
'done': 1,
'error': 2
})
OperatableStateMachine.add('side',
TestSubjectState(),
transitions={
'done': 'done',
'error': 'error'
},
autonomy={
'done': 1,
'error': 2
})
with OperatableStateMachine(outcomes=['done', 'error']):
OperatableStateMachine.add('cc',
cc,
transitions={
'done': 'done',
'error': 'error'
},
autonomy={
'done': 1,
'error': 2
})
class FakeRate(object):
def remaining(self):
return rospy.Duration(0)
def sleep(self):
pass
# all states are called with their correct rate
cc.execute(None)
cc.sleep()
cc.execute(None)
self.assertAlmostEqual(cc.sleep_duration, .1, places=2)
cc.sleep()
cc['main'].set_rate(15)
cc['side'].set_rate(10)
cc['main'].count = 0
cc['side'].count = 0
start = rospy.get_time()
cc_count = 0
while rospy.get_time() - start <= 1.:
cc_count += 1
cc.execute(None)
self.assertLessEqual(cc.sleep_duration, .1)
cc.sleep()
self.assertIn(cc['main'].count, [14, 15, 16])
self.assertIn(cc['side'].count, [9, 10, 11])
self.assertLessEqual(cc_count, 27)
# verify ROS properties and disable sleep
cc._enable_ros_control()
self.assertTrue(cc['main']._is_controlled)
self.assertFalse(cc['side']._is_controlled)
cc['main']._rate = FakeRate()
cc['side']._rate = FakeRate()
# return outcome when all return done or any returns error
outcome = cc.execute(None)
self.assertIsNone(outcome)
cc['main'].result = 'error'
outcome = cc.execute(None)
self.assertEqual(outcome, 'error')
cc['main'].result = None
cc['side'].result = 'error'
outcome = cc.execute(None)
self.assertEqual(outcome, 'error')
cc['side'].result = 'done'
outcome = cc.execute(None)
self.assertIsNone(outcome)
cc['main'].result = 'done'
outcome = cc.execute(None)
self.assertEqual(outcome, 'done')
cc['main'].result = None
cc['side'].result = None
# always call on_exit exactly once when returning an outcome
outcome = cc.execute(None)
self.assertIsNone(outcome)
cc['main'].last_events = []
cc['side'].last_events = []
cc['main'].result = 'error'
outcome = cc.execute(None)
self.assertEqual(outcome, 'error')
self.assertListEqual(cc['main'].last_events, ['on_exit'])
self.assertListEqual(cc['side'].last_events, ['on_exit'])
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:30 y:365, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:54 y:385, x:100 y:523
_sm_safety_monitor_0 = OperatableStateMachine(
outcomes=['finished', 'failed'])
with _sm_safety_monitor_0:
# x:289 y:57
OperatableStateMachine.add('subscribe to safety topic 1',
SubscriberState(topic='/chatter',
blocking=True,
clear=False),
transitions={
'received':
'check if topic publishes warning',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:719 y:64
OperatableStateMachine.add(
'fine continue log',
LogState(text="continue log", severity=Logger.REPORT_HINT),
transitions={'done': 'subscribe to safety topic 1'},
autonomy={'done': Autonomy.Off})
# x:690 y:202
OperatableStateMachine.add(
'check if topic publishes warning',
CheckConditionState(
predicate=lambda message: message.data == "1"),
transitions={
'true': 'fine continue log',
'false': 'stop now log'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'message'})
# x:451 y:481
OperatableStateMachine.add('stop now log',
LogState(
text="stopping - message receieved",
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:30 y:456, x:130 y:456
_sm_normal_activity_1 = OperatableStateMachine(
outcomes=['finished', 'failed'])
with _sm_normal_activity_1:
# x:338 y:227
OperatableStateMachine.add('normalactivity',
LogState(text="normal activity",
severity=Logger.REPORT_HINT),
transitions={'done': 'wait1'},
autonomy={'done': Autonomy.Off})
# x:618 y:312
OperatableStateMachine.add('wait1',
WaitState(wait_time=1),
transitions={'done': 'normalactivity'},
autonomy={'done': Autonomy.Off})
# x:113 y:364, x:465 y:451, x:737 y:425, x:254 y:385, x:221 y:448
_sm_container_2 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('failed', [('normal_activity', 'failed'),
('safety_monitor', 'failed')]),
('finished', [('normal_activity', 'finished')]),
('finished', [('safety_monitor', 'finished')])])
with _sm_container_2:
# x:149 y:54
OperatableStateMachine.add('normal_activity',
_sm_normal_activity_1,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:676 y:54
OperatableStateMachine.add('safety_monitor',
_sm_safety_monitor_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
with _state_machine:
# x:317 y:100
OperatableStateMachine.add('Container',
_sm_container_2,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
return _state_machine
def create(self):
# x:682 y:306, x:452 y:252
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['pos'])
_state_machine.userdata.pos = {"x": 0.8, "y": -0.2, "z": 1}
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458, x:530 y:458, x:630 y:458, x:59 y:533, x:830 y:458
_sm_group_0 = ConcurrencyContainer(
outcomes=['threshold', 'watchdog', 'fail'],
conditions=[('threshold', [('read', 'threshold')]),
('watchdog', [('read', 'watchdog')]),
('fail', [('read', 'fail')]),
('threshold', [('read yaw', 'threshold')]),
('fail', [('read yaw', 'fail')]),
('watchdog', [('read yaw', 'watchdog')])])
with _sm_group_0:
# x:86 y:125
OperatableStateMachine.add('read',
ReadTorque(
watchdog=1,
Joint="right_elbow_pitch_joint",
Threshold=0.7,
min_time=0.4),
transitions={
'threshold': 'threshold',
'watchdog': 'watchdog',
'fail': 'fail'
},
autonomy={
'threshold': Autonomy.Off,
'watchdog': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'torque': 'torque'})
# x:252 y:135
OperatableStateMachine.add('read yaw',
ReadTorque(
watchdog=1,
Joint="right_elbow_pitch_joint",
Threshold=0.5,
min_time=0.4),
transitions={
'threshold': 'threshold',
'watchdog': 'watchdog',
'fail': 'fail'
},
autonomy={
'threshold': Autonomy.Off,
'watchdog': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'torque': 'torque'})
# x:30 y:458
_sm_read_torque_1 = OperatableStateMachine(outcomes=['done'])
with _sm_read_torque_1:
# x:142 y:61
OperatableStateMachine.add('log',
LogState(text="going down",
severity=Logger.REPORT_HINT),
transitions={'done': 'Group'},
autonomy={'done': Autonomy.Off})
# x:131 y:164
OperatableStateMachine.add('Group',
_sm_group_0,
transitions={
'threshold': 'done',
'watchdog': 'log',
'fail': 'done'
},
autonomy={
'threshold': Autonomy.Inherit,
'watchdog': Autonomy.Inherit,
'fail': Autonomy.Inherit
})
# x:30 y:458
_sm_go_down_2 = OperatableStateMachine(outcomes=['done'],
input_keys=['GripPose'])
with _sm_go_down_2:
# x:126 y:194
OperatableStateMachine.add('movePlace',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'done'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'GripPose'})
# x:30 y:324, x:130 y:324
_sm_retreate_arm_3 = OperatableStateMachine(
outcomes=['failed', 'done'],
input_keys=['approach_pose', 'PreGripPose'])
with _sm_retreate_arm_3:
# x:30 y:40
OperatableStateMachine.add('ReturnApproachPose',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'ReturnPreGrip',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'approach_pose'})
# x:202 y:41
OperatableStateMachine.add('ReturnPreGrip',
MoveitMove(move=True,
waitForExecution=False,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PreGripPose'})
# x:30 y:324, x:130 y:324
_sm_releasing_4 = OperatableStateMachine(
outcomes=['object', 'no_object'])
with _sm_releasing_4:
# x:30 y:40
OperatableStateMachine.add('say touchdown',
SaraSay(sentence="Touchdown!",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'open gripper'},
autonomy={'done': Autonomy.Off})
# x:139 y:176
OperatableStateMachine.add(
'open gripper',
SetGripperState(width=0.14, effort=1),
transitions={
'object': 'object',
'no_object': 'no_object'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:30 y:324, x:130 y:324
_sm_moveback_5 = OperatableStateMachine(outcomes=['arrived', 'failed'])
with _sm_moveback_5:
# x:30 y:40
OperatableStateMachine.add('genpose',
GenPoseEuler(x=-0.3,
y=-0.3,
z=0,
roll=0,
pitch=0,
yaw=0),
transitions={'done': 'move back'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'backPose'})
# x:40 y:163
OperatableStateMachine.add('move back',
SaraMoveBase(reference="base_link"),
transitions={
'arrived': 'arrived',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'backPose'})
# x:536 y:72, x:231 y:292
_sm_prepare_grip_6 = OperatableStateMachine(
outcomes=['failed', 'done'],
input_keys=['pos'],
output_keys=['approach_pose', 'grip_pose'])
with _sm_prepare_grip_6:
# x:50 y:40
OperatableStateMachine.add('Gen place_pos',
GenGripperPose(l=0,
z=-0.05,
planar=True),
transitions={
'done': 'Gen approach_pos',
'fail': 'failed'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'pos',
'pose_out': 'grip_pose'
})
# x:30 y:176
OperatableStateMachine.add('MoveIt_isReachable',
MoveitMove(move=False,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'log app',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'grip_pose'})
# x:37 y:108
OperatableStateMachine.add('Gen approach_pos',
GenGripperPose(l=0.0,
z=0.20,
planar=True),
transitions={
'done': 'log place pos',
'fail': 'failed'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'pos',
'pose_out': 'approach_pose'
})
# x:41 y:269
OperatableStateMachine.add('log app',
LogKeyState(
text="{}",
severity=Logger.REPORT_HINT),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'approach_pose'})
# x:360 y:180
OperatableStateMachine.add(
'log place pos',
LogKeyState(text="place pose is {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'MoveIt_isReachable'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'grip_pose'})
# x:30 y:324, x:130 y:324
_sm_pregrippose_7 = OperatableStateMachine(outcomes=['done', 'failed'],
output_keys=['PreGripPose'])
with _sm_pregrippose_7:
# x:30 y:40
OperatableStateMachine.add('setPreGripPose',
SetKey(Value="PrePlacePose"),
transitions={'done': 'gotoPreGrip'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'PreGripPose'})
# x:32 y:106
OperatableStateMachine.add('gotoPreGrip',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PreGripPose'})
# x:30 y:458, x:130 y:458, x:230 y:458
_sm_get_down_8 = ConcurrencyContainer(
outcomes=['done'],
input_keys=['GripPose'],
conditions=[('done', [('Go down', 'done')]),
('done', [('read torque', 'done')])])
with _sm_get_down_8:
# x:178 y:127
OperatableStateMachine.add('Go down',
_sm_go_down_2,
transitions={'done': 'done'},
autonomy={'done': Autonomy.Inherit},
remapping={'GripPose': 'GripPose'})
# x:405 y:150
OperatableStateMachine.add('read torque',
_sm_read_torque_1,
transitions={'done': 'done'},
autonomy={'done': Autonomy.Inherit})
# x:30 y:324, x:130 y:324
_sm_pretraitement_9 = OperatableStateMachine(outcomes=['fail', 'done'],
input_keys=['pos'],
output_keys=['pos'])
with _sm_pretraitement_9:
# x:30 y:40
OperatableStateMachine.add('TF_transformation',
TF_transformation(in_ref="map",
out_ref="base_link"),
transitions={
'done': 'LOG POSE',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'in_pos': 'pos',
'out_pos': 'pos'
})
# x:33 y:107
OperatableStateMachine.add('LOG POSE',
LogKeyState(
text="{}",
severity=Logger.REPORT_HINT),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'pos'})
with _state_machine:
# x:148 y:34
OperatableStateMachine.add('Pretraitement',
_sm_pretraitement_9,
transitions={
'fail': 'failed',
'done': 'PreGripPose'
},
autonomy={
'fail': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={'pos': 'pos'})
# x:634 y:410
OperatableStateMachine.add(
'close gripper',
SetGripperState(width=0, effort=1),
transitions={
'object': 'finished',
'no_object': 'finished'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:141 y:522
OperatableStateMachine.add('Get_down',
_sm_get_down_8,
transitions={'done': 'releasing'},
autonomy={'done': Autonomy.Inherit},
remapping={'GripPose': 'grip_pose'})
# x:159 y:352
OperatableStateMachine.add('look down',
SaraSetHeadAngle(pitch=0.6, yaw=-0.3),
transitions={'done': 'Move_approach'},
autonomy={'done': Autonomy.Off})
# x:160 y:127
OperatableStateMachine.add(
'PreGripPose',
_sm_pregrippose_7,
transitions={
'done': 'Prepare grip',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'PreGripPose': 'PreGripPose'})
# x:156 y:238
OperatableStateMachine.add('Prepare grip',
_sm_prepare_grip_6,
transitions={
'failed': 'failed',
'done': 'look down'
},
autonomy={
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'pos': 'pos',
'approach_pose': 'approach_pose',
'grip_pose': 'grip_pose'
})
# x:139 y:444
OperatableStateMachine.add('Move_approach',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'Get_down',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'approach_pose'})
# x:623 y:525
OperatableStateMachine.add('Moveback',
_sm_moveback_5,
transitions={
'arrived': 'close gripper',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:298 y:520
OperatableStateMachine.add('releasing',
_sm_releasing_4,
transitions={
'object': 'retreate arm',
'no_object': 'retreate arm'
},
autonomy={
'object': Autonomy.Inherit,
'no_object': Autonomy.Inherit
})
# x:459 y:522
OperatableStateMachine.add('retreate arm',
_sm_retreate_arm_3,
transitions={
'failed': 'failed',
'done': 'Moveback'
},
autonomy={
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'approach_pose': 'approach_pose',
'PreGripPose': 'PreGripPose'
})
return _state_machine
def create(self):
# x:192 y:728, x:263 y:340
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.simulation = True
_state_machine.userdata.flying_altitude = 5
_state_machine.userdata.orbit_goal = ""
_state_machine.userdata.window_id = 0
_state_machine.userdata.lidar_flier_speed_orbit = 0
_state_machine.userdata.lidar_flier_speed_goto = 0
_state_machine.userdata.lidar_flier_clockwise = False
_state_machine.userdata.lidar_flier_stick_distance = False
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
param_prefix_namespace = "flexbe_behavior_launcher/"
flying_altitude_list = rospy.get_param(param_prefix_namespace +
'main/flying_altitudes')
uav_name_list = rospy.get_param(param_prefix_namespace +
'main/robot_name_list')
robot_name = rospy.get_param(param_prefix_namespace + 'robot_name')
lidar_flier_speed_goto = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_speed_goto')
lidar_flier_speed_orbit = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_speed_orbit')
lidar_flier_clockwise = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_clockwise')
lidar_flier_stick_distance = rospy.get_param(
param_prefix_namespace + 'lidar_flier/lidar_flier_stick_distance')
window_position_estimation_reset_topic = rospy.get_param(
param_prefix_namespace +
'main/window_position_estimation_reset_topic')
window_position_estimation_reset_topic = "/" + robot_name + "/" + window_position_estimation_reset_topic
Logger.loginfo("window_position_estimation_reset_topic: %s" %
window_position_estimation_reset_topic)
window_position_estimation_closest_window_topic = rospy.get_param(
param_prefix_namespace +
'main/window_position_estimation_closest_window_topic')
window_position_estimation_closest_window_topic = "/" + robot_name + "/" + window_position_estimation_closest_window_topic
Logger.loginfo("window_position_estimation_closest_window_topic: %s" %
window_position_estimation_closest_window_topic)
land_service_topic = rospy.get_param(param_prefix_namespace +
'main/land_service_topic')
land_service_topic = "/" + robot_name + "/" + land_service_topic
Logger.loginfo("land_service_topic: %s" % land_service_topic)
e_land_service_topic = rospy.get_param(param_prefix_namespace +
'main/e_land_service_topic')
e_land_service_topic = "/" + robot_name + "/" + e_land_service_topic
Logger.loginfo("e_land_service_topic: %s" % e_land_service_topic)
fire_detection_topic = rospy.get_param(param_prefix_namespace +
'main/fire_detection_topic')
fire_detection_topic = "/" + robot_name + "/" + fire_detection_topic
Logger.loginfo("fire_detection_topic: %s" % fire_detection_topic)
window_flyer_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/window_flyer_action_server_name')
window_flyer_action_server_name = "/" + robot_name + "/" + window_flyer_action_server_name
Logger.loginfo("window_flyer_action_server_name: %s" %
window_flyer_action_server_name)
lidar_flier_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/lidar_flier_action_server_name')
lidar_flier_action_server_name = "/" + robot_name + "/" + lidar_flier_action_server_name
Logger.loginfo("lidar_flier_action_server_name: %s" %
lidar_flier_action_server_name)
fire_extinguish_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/fire_extinguish_action_server_name')
fire_extinguish_action_server_name = "/" + robot_name + "/" + fire_extinguish_action_server_name
Logger.loginfo("fire_extinguish_action_server_name: %s" %
fire_extinguish_action_server_name)
control_manager_diagnostics_topic = rospy.get_param(
param_prefix_namespace + 'main/control_manager_diagnostics_topic')
control_manager_diagnostics_topic = "/" + robot_name + "/" + control_manager_diagnostics_topic
Logger.loginfo("control_manager_diagnostics_topic: %s" %
control_manager_diagnostics_topic)
control_manager_cmd_topic = rospy.get_param(
param_prefix_namespace + 'main/control_manager_cmd_topic')
control_manager_cmd_topic = "/" + robot_name + "/" + control_manager_cmd_topic
Logger.loginfo("control_manager_cmd_topic: %s" %
control_manager_cmd_topic)
goto_reference_service_topic = rospy.get_param(
param_prefix_namespace + 'main/goto_reference_service_topic')
goto_reference_service_topic = "/" + robot_name + "/" + goto_reference_service_topic
Logger.loginfo("goto_reference_service_topic: %s" %
goto_reference_service_topic)
goto_altitude_service_topic = rospy.get_param(
param_prefix_namespace + 'main/goto_altitude_service_topic')
goto_altitude_service_topic = "/" + robot_name + "/" + goto_altitude_service_topic
Logger.loginfo("goto_altitude_service_topic: %s" %
goto_altitude_service_topic)
wait_for_start_service_topic = rospy.get_param(
param_prefix_namespace + 'main/wait_for_start_service_topic')
wait_for_start_service_topic = "/" + robot_name + "/" + wait_for_start_service_topic
Logger.loginfo("wait_for_start_service_topic: %s" %
wait_for_start_service_topic)
# confirmation_service_topic = rospy.get_param(param_prefix_namespace + 'main/confirmation_service_topic')
# confirmation_service_topic = "/" + robot_name + "/" + confirmation_service_topic
# Logger.loginfo("confirmation_service_topic: %s" % confirmation_service_topic)
hector_reset_service_topic = rospy.get_param(
param_prefix_namespace + 'main/hector_reset_service_topic')
hector_reset_service_topic = "/" + robot_name + "/" + hector_reset_service_topic
Logger.loginfo("hector_reset_service_topic: %s" %
hector_reset_service_topic)
odometry_switch_service_topic = rospy.get_param(
param_prefix_namespace + 'main/odometry_switch_service_topic')
odometry_switch_service_topic = "/" + robot_name + "/" + odometry_switch_service_topic
Logger.loginfo("odometry_switch_service_topic: %s" %
odometry_switch_service_topic)
flying_altitude = 3
for i in range(len(uav_name_list)):
if uav_name_list[i] == robot_name:
flying_altitude = flying_altitude_list[i]
break
orbit_goal = lidar_flier.msg.lfGoal()
orbit_goal.altitude = flying_altitude
orbit_goal.orbit = True
orbit_goal.speed = lidar_flier_speed_orbit
orbit_goal.clockwise = lidar_flier_clockwise
orbit_goal.stick_distance = lidar_flier_stick_distance
_state_machine.userdata.flying_altitude = flying_altitude
_state_machine.userdata.orbit_goal = orbit_goal
_state_machine.userdata.lidar_flier_speed_goto = lidar_flier_speed_goto
_state_machine.userdata.lidar_flier_speed_orbit = lidar_flier_speed_orbit
_state_machine.userdata.lidar_flier_clockwise = lidar_flier_clockwise
_state_machine.userdata.lidar_flier_stick_distance = lidar_flier_stick_distance
Logger.loginfo("fire_ch_sm.flying_altitude: %s" %
_state_machine.userdata.flying_altitude)
Logger.loginfo("fire_ch_sm.orbit_goal: %s" %
_state_machine.userdata.orbit_goal)
Logger.loginfo("fire_ch_sm.lidar_flier_speed_goto: %s" %
_state_machine.userdata.lidar_flier_speed_goto)
Logger.loginfo("fire_ch_sm.lidar_flier_speed_orbit: %s" %
_state_machine.userdata.lidar_flier_speed_orbit)
Logger.loginfo("fire_ch_sm.lidar_flier_stick_distance: %s" %
_state_machine.userdata.lidar_flier_stick_distance)
Logger.loginfo("fire_ch_sm.lidar_flier_clockwise: %s" %
_state_machine.userdata.lidar_flier_clockwise)
# [/MANUAL_CREATE]
# x:30 y:490, x:644 y:487, x:230 y:490, x:330 y:490, x:430 y:490, x:35 y:612, x:97 y:605, x:168 y:600, x:1205 y:397, x:883 y:498, x:1030 y:490, x:1130 y:490, x:1230 y:490
_sm_lookforwindow_0 = ConcurrencyContainer(
outcomes=['error', 'detection', 'no_detection', 'preemted'],
input_keys=['orbit_goal', 'window_id'],
output_keys=['window_id'],
conditions=[
('no_detection', [('OrbitBuilding', 'successed')]),
('error', [('OrbitBuilding', 'no_valid_points_in_scan')]),
('error', [('OrbitBuilding', 'stop_requested')]),
('error', [('OrbitBuilding', 'error')]),
('preemted', [('OrbitBuilding', 'preempted')]),
('error', [('OrbitBuilding', 'server_not_initialized')]),
('error', [('InfiniteWait', 'done')]),
('detection', [('WaitForDetection', 'successed')]),
('error', [('WaitForDetection', 'failed')])
])
with _sm_lookforwindow_0:
# x:434 y:172
OperatableStateMachine.add('InfiniteWait',
WaitState(wait_time=100000),
transitions={'done': 'error'},
autonomy={'done': Autonomy.Off})
# x:155 y:164
OperatableStateMachine.add(
'OrbitBuilding',
LidarFlierActionState(
action_server_name=lidar_flier_action_server_name),
transitions={
'successed': 'no_detection',
'no_valid_points_in_scan': 'error',
'stop_requested': 'error',
'server_not_initialized': 'error',
'error': 'error',
'preempted': 'preemted'
},
autonomy={
'successed': Autonomy.Off,
'no_valid_points_in_scan': Autonomy.Off,
'stop_requested': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={'goal': 'orbit_goal'})
# x:735 y:179
OperatableStateMachine.add(
'WaitForDetection',
WaitForMsgState(
topic=window_position_estimation_closest_window_topic,
wait_time=-1,
function=self.callback_true,
input_keys=["window_id"],
output_keys=["window_id"],
output_function=self.window_detection_cb),
transitions={
'successed': 'detection',
'failed': 'error'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'window_id': 'window_id'})
with _state_machine:
# x:55 y:69
OperatableStateMachine.add(
'WaitForStart',
ServiceWaitForStart(
service_topic=wait_for_start_service_topic),
transitions={'received': 'GotoAltitude'},
autonomy={'received': Autonomy.Off},
remapping={'start_value': 'start_value'})
# x:705 y:70
OperatableStateMachine.add(
'ResetWindowEstimation',
ServiceTriggerState(
service_topic=window_position_estimation_reset_topic,
state_name="WindowEstimation"),
transitions={
'finished': 'LookForWindow',
'failed': 'EmergencyLanding'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:519 y:329
OperatableStateMachine.add('EmergencyLanding',
ServiceTriggerState(
service_topic=e_land_service_topic,
state_name="ControlManager"),
transitions={
'finished': 'failed',
'failed': 'EmergencyLanding'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:251 y:67
OperatableStateMachine.add(
'GotoAltitude',
ServiceGoToAltitudeState(
service_topic=goto_altitude_service_topic,
control_manager_diagnostics_topic=
control_manager_diagnostics_topic),
transitions={
'successed': 'SaveHomePosition',
'failed': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'goal': 'flying_altitude'})
# x:495 y:68
OperatableStateMachine.add(
'SaveHomePosition',
WaitForMsgState(
topic=control_manager_cmd_topic,
wait_time=5,
function=self.callback_true,
input_keys=["lidar_flier_speed_goto"],
output_keys=["home_position"],
output_function=self.home_position_output_function),
transitions={
'successed': 'ResetWindowEstimation',
'failed': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'lidar_flier_speed_goto': 'lidar_flier_speed_goto',
'home_position': 'home_position'
})
# x:1190 y:65
OperatableStateMachine.add(
'FlythroughWindowInsideApproach',
WindowFlyerActionState(
action_server_name=window_flyer_action_server_name,
just_approach=True,
yaw_offset=3.14),
transitions={
'successed': 'ResetHector',
'window_not_found': 'EmergencyLanding',
'already_flythrough': 'EmergencyLanding',
'server_not_initialized': 'EmergencyLanding',
'error': 'EmergencyLanding',
'preempted': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'window_not_found': Autonomy.Off,
'already_flythrough': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'window_id': 'window_id',
'window_position': 'window_position'
})
# x:1009 y:65
OperatableStateMachine.add('LookForWindow',
_sm_lookforwindow_0,
transitions={
'error': 'EmergencyLanding',
'detection':
'FlythroughWindowInsideApproach',
'no_detection': 'EmergencyLanding',
'preemted': 'EmergencyLanding'
},
autonomy={
'error': Autonomy.Inherit,
'detection': Autonomy.Inherit,
'no_detection': Autonomy.Inherit,
'preemted': Autonomy.Inherit
},
remapping={
'orbit_goal': 'orbit_goal',
'window_id': 'window_id'
})
# x:1419 y:387
OperatableStateMachine.add(
'FlythroughWindowOutsideApproach',
WindowFlyerActionState(
action_server_name=window_flyer_action_server_name,
just_approach=True,
yaw_offset=3.14),
transitions={
'successed': 'FlythroughWindowOutside',
'window_not_found': 'EmergencyLanding',
'already_flythrough': 'EmergencyLanding',
'server_not_initialized': 'EmergencyLanding',
'error': 'EmergencyLanding',
'preempted': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'window_not_found': Autonomy.Off,
'already_flythrough': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'window_id': 'window_id',
'window_position': 'window_position'
})
# x:671 y:739
OperatableStateMachine.add(
'GoToHome',
LidarFlierActionState(
action_server_name=lidar_flier_action_server_name),
transitions={
'successed': 'Land',
'no_valid_points_in_scan': 'EmergencyLanding',
'stop_requested': 'EmergencyLanding',
'server_not_initialized': 'EmergencyLanding',
'error': 'EmergencyLanding',
'preempted': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'no_valid_points_in_scan': Autonomy.Off,
'stop_requested': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={'goal': 'home_position'})
# x:390 y:734
OperatableStateMachine.add('Land',
ServiceTriggerState(
service_topic=land_service_topic,
state_name="ControlManager"),
transitions={
'finished': 'finished',
'failed': 'Land'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1467 y:64
OperatableStateMachine.add(
'ResetHector',
ServiceTriggerState(service_topic=hector_reset_service_topic,
state_name="ResetHector"),
transitions={
'finished': 'WaitForTwoSecond',
'failed': 'EmergencyLanding'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1627 y:58
OperatableStateMachine.add(
'WaitForTwoSecond',
WaitState(wait_time=2),
transitions={'done': 'SwitchOdometryToHector'},
autonomy={'done': Autonomy.Off})
# x:1444 y:159
OperatableStateMachine.add(
'SwitchOdometryToHector',
ServiceStringState(service_topic=odometry_switch_service_topic,
msg_text="HECTOR",
state_name="Odometry"),
transitions={
'finished': 'WaitForTwoSecond2',
'failed': 'EmergencyLanding'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1625 y:158
OperatableStateMachine.add(
'WaitForTwoSecond2',
WaitState(wait_time=2),
transitions={'done': 'FlythroughWindowInside'},
autonomy={'done': Autonomy.Off})
# x:999 y:736
OperatableStateMachine.add(
'SwitchOdometryToGPS',
ServiceStringState(service_topic=odometry_switch_service_topic,
msg_text="GPS",
state_name="Odometry"),
transitions={
'finished': 'GoToHome',
'failed': 'EmergencyLanding'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1455 y:250
OperatableStateMachine.add(
'FlythroughWindowInside',
WindowFlyerActionState(
action_server_name=window_flyer_action_server_name,
just_approach=False,
yaw_offset=3.14),
transitions={
'successed': 'FlythroughWindowOutsideApproach',
'window_not_found': 'EmergencyLanding',
'already_flythrough': 'EmergencyLanding',
'server_not_initialized': 'EmergencyLanding',
'error': 'EmergencyLanding',
'preempted': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'window_not_found': Autonomy.Off,
'already_flythrough': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'window_id': 'window_id',
'window_position': 'window_position'
})
# x:1448 y:487
OperatableStateMachine.add(
'FlythroughWindowOutside',
WindowFlyerActionState(
action_server_name=window_flyer_action_server_name,
just_approach=False,
yaw_offset=3.14),
transitions={
'successed': 'SwitchOdometryToGPS',
'window_not_found': 'EmergencyLanding',
'already_flythrough': 'EmergencyLanding',
'server_not_initialized': 'EmergencyLanding',
'error': 'EmergencyLanding',
'preempted': 'EmergencyLanding'
},
autonomy={
'successed': Autonomy.Off,
'window_not_found': Autonomy.Off,
'already_flythrough': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'window_id': 'window_id',
'window_position': 'window_position'
})
return _state_machine
def create(self):
# x:860 y:152, x:755 y:568
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['pose'])
_state_machine.userdata.pose = "crowd"
_state_machine.userdata.name = "destination"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:130 y:365
_sm_look_around_0 = OperatableStateMachine(outcomes=['failed'])
with _sm_look_around_0:
# x:78 y:40
OperatableStateMachine.add('set pitch',
SetKey(Value=0.9),
transitions={'done': 'get angle'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'pitch'})
# x:348 y:210
OperatableStateMachine.add('set head',
SaraSetHeadAngleKey(),
transitions={'done': 'get angle'},
autonomy={'done': Autonomy.Off},
remapping={
'yaw': 'yaw',
'pitch': 'pitch'
})
# x:204 y:248
OperatableStateMachine.add(
'limit yaw',
CalculationState(calculation=lambda x: max(min(x, 1), -1)),
transitions={'done': 'set head'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'yaw',
'output_value': 'yaw'
})
# x:191 y:126
OperatableStateMachine.add('get angle',
GetClosestObstacle(topic="/scan",
maximumDistance=2),
transitions={'done': 'limit yaw'},
autonomy={'done': Autonomy.Off},
remapping={'Angle': 'yaw'})
# x:30 y:365, x:130 y:365
_sm_move_1 = OperatableStateMachine(outcomes=['arrived', 'failed'],
input_keys=['pose'])
with _sm_move_1:
# x:95 y:122
OperatableStateMachine.add('move',
SaraMoveBase(reference="map"),
transitions={
'arrived': 'arrived',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:259 y:573, x:491 y:362, x:491 y:453
_sm_manage_name_2 = OperatableStateMachine(
outcomes=['done', 'too much', 'not found'],
input_keys=['pose'],
output_keys=['pose', 'name'])
with _sm_manage_name_2:
# x:39 y:48
OperatableStateMachine.add(
'check if Pose',
CheckConditionState(predicate=lambda x: type(x) is type([])),
transitions={
'true': 'getname',
'false': 'check if string'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'pose'})
# x:257 y:264
OperatableStateMachine.add(
'getcontainers',
CalculationState(calculation=lambda x: x[1:]),
transitions={'done': 'get wonderland entity'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'pose',
'output_value': 'containers'
})
# x:232 y:354
OperatableStateMachine.add('get wonderland entity',
WonderlandGetEntityVerbal(),
transitions={
'one': 'get waypoint',
'multiple': 'too much',
'none': 'not found',
'error': 'not found'
},
autonomy={
'one': Autonomy.Off,
'multiple': Autonomy.Off,
'none': Autonomy.Off,
'error': Autonomy.Off
},
remapping={
'name': 'name',
'containers': 'containers',
'entities': 'entities',
'firstEntity': 'firstEntity'
})
# x:238 y:442
OperatableStateMachine.add('get waypoint',
GetAttribute(attributes=["waypoint"]),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={
'object': 'firstEntity',
'waypoint': 'pose'
})
# x:256 y:166
OperatableStateMachine.add(
'getname',
CalculationState(calculation=lambda x: x[0]),
transitions={'done': 'getcontainers'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'pose',
'output_value': 'name'
})
# x:42 y:180
OperatableStateMachine.add(
'check if string',
CheckConditionState(predicate=lambda x: type(x) is type("")),
transitions={
'true': 'remap to name',
'false': 'done'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'pose'})
# x:40 y:251
OperatableStateMachine.add(
'remap to name',
CalculationState(calculation=lambda x: x),
transitions={'done': 'set containers empty'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'pose',
'output_value': 'name'
})
# x:30 y:326
OperatableStateMachine.add(
'set containers empty',
SetKey(Value=[]),
transitions={'done': 'get wonderland entity'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'containers'})
# x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365
_sm_move_concurent_3 = ConcurrencyContainer(
outcomes=['arrived', 'failed'],
input_keys=['pose'],
conditions=[('arrived', [('Move', 'arrived')]),
('failed', [('Move', 'failed')]),
('failed', [('Look around', 'failed')])])
with _sm_move_concurent_3:
# x:30 y:40
OperatableStateMachine.add('Move',
_sm_move_1,
transitions={
'arrived': 'arrived',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pose': 'pose'})
# x:268 y:74
OperatableStateMachine.add('Look around',
_sm_look_around_0,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
with _state_machine:
# x:54 y:27
OperatableStateMachine.add('SetCount',
SetKey(Value=2),
transitions={'done': 'manage name'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Count'})
# x:258 y:250
OperatableStateMachine.add('stuck',
SaraSay(sentence="I'm getting stuck.",
input_keys=[],
emotion=2,
block=True),
transitions={'done': 'Count--'},
autonomy={'done': Autonomy.Off})
# x:49 y:251
OperatableStateMachine.add('try again',
SaraSay(sentence="But I'm still going.",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'Move concurent'},
autonomy={'done': Autonomy.Off})
# x:360 y:508
OperatableStateMachine.add(
'sorry',
SaraSay(sentence="Well. It seem's I can't go there.",
input_keys=[],
emotion=2,
block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:672 y:147
OperatableStateMachine.add('set blink',
SetExpression(emotion=6, brightness=-1),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:252 y:146
OperatableStateMachine.add('Move concurent',
_sm_move_concurent_3,
transitions={
'arrived': 'reset head',
'failed': 'stuck'
},
autonomy={
'arrived': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pose': 'pose'})
# x:254 y:353
OperatableStateMachine.add(
'Count--',
CalculationState(calculation=lambda x: x - 1),
transitions={'done': 'check count'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'Count',
'output_value': 'Count'
})
# x:37 y:351
OperatableStateMachine.add(
'check count',
CheckConditionState(predicate=lambda x: x >= 0),
transitions={
'true': 'try again',
'false': 'sorry'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'Count'})
# x:497 y:149
OperatableStateMachine.add('reset head',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'set blink'},
autonomy={'done': Autonomy.Off})
# x:269 y:21
OperatableStateMachine.add('manage name',
_sm_manage_name_2,
transitions={
'done': 'set head',
'too much': 'say too much',
'not found': 'say not known'
},
autonomy={
'done': Autonomy.Inherit,
'too much': Autonomy.Inherit,
'not found': Autonomy.Inherit
},
remapping={
'pose': 'pose',
'name': 'name'
})
# x:46 y:147
OperatableStateMachine.add('set head',
SaraSetHeadAngle(pitch=0.8, yaw=0),
transitions={'done': 'Move concurent'},
autonomy={'done': Autonomy.Off})
# x:477 y:333
OperatableStateMachine.add(
'say too much',
SaraSay(
sentence=lambda x: "There is more than one " + x[0] + ".",
input_keys=["poseName"],
emotion=3,
block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off},
remapping={'poseName': 'name'})
# x:445 y:418
OperatableStateMachine.add(
'say not known',
SaraSay(sentence=lambda x: "I don't know where the " + x[0] +
" is.",
input_keys=["poseName"],
emotion=0,
block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off},
remapping={'poseName': 'name'})
return _state_machine
def create(self):
# x:1023 y:473, x:791 y:291, x:797 y:103
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed', 'critical_fail'],
input_keys=['Action'])
_state_machine.userdata.Action = ["Guide", 'table', 'kitchen']
_state_machine.userdata.relative = False
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:458
_sm_groupwait_0 = OperatableStateMachine(outcomes=['end'])
with _sm_groupwait_0:
# x:30 y:40
OperatableStateMachine.add('waitwait',
WaitState(wait_time=20),
transitions={'done': 'end'},
autonomy={'done': Autonomy.Off})
# x:534 y:319
_sm_move_head_and_base_at_the_end_1 = OperatableStateMachine(
outcomes=['failed'])
with _sm_move_head_and_base_at_the_end_1:
# x:52 y:31
OperatableStateMachine.add('setkeyorientation',
SetKey(Value=1.5),
transitions={'done': 'action_turn'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'rotation'})
# x:51 y:114
OperatableStateMachine.add(
'action_turn',
self.use_behavior(
action_turnSM,
'operator is still there/Move head and base end /move head and base at the end/action_turn'
),
transitions={
'finished': 'turn right head',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'rotation': 'rotation'})
# x:48 y:272
OperatableStateMachine.add('wait while head turn',
WaitState(wait_time=4),
transitions={'done': 'action_turn_2'},
autonomy={'done': Autonomy.Off})
# x:47 y:199
OperatableStateMachine.add(
'turn right head',
SaraSetHeadAngle(pitch=0, yaw=1.57),
transitions={'done': 'wait while head turn'},
autonomy={'done': Autonomy.Off})
# x:45 y:348
OperatableStateMachine.add(
'action_turn_2',
self.use_behavior(
action_turnSM,
'operator is still there/Move head and base end /move head and base at the end/action_turn_2'
),
transitions={
'finished': 'left to rigth',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'rotation': 'rotation'})
# x:43 y:592
OperatableStateMachine.add('left to rigth',
SaraSetHeadAngle(pitch=0, yaw=-1.57),
transitions={'done': 'waitwait1'},
autonomy={'done': Autonomy.Off})
# x:420 y:602
OperatableStateMachine.add('right to left',
SaraSetHeadAngle(pitch=0, yaw=1.57),
transitions={'done': 'waitwait2'},
autonomy={'done': Autonomy.Off})
# x:266 y:541
OperatableStateMachine.add('waitwait1',
WaitState(wait_time=8),
transitions={'done': 'right to left'},
autonomy={'done': Autonomy.Off})
# x:236 y:674
OperatableStateMachine.add('waitwait2',
WaitState(wait_time=8),
transitions={'done': 'left to rigth'},
autonomy={'done': Autonomy.Off})
# x:231 y:538
_sm_find_a_human_2 = OperatableStateMachine(outcomes=['finished'],
input_keys=['ID'])
with _sm_find_a_human_2:
# x:78 y:275
OperatableStateMachine.add('find the human',
GetEntityByID(),
transitions={
'found': 'finished',
'not_found': 'find the human'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'ID',
'Entity': 'Entity'
})
# x:415 y:99, x:318 y:246, x:442 y:295
_sm_move_head_and_base_end__3 = ConcurrencyContainer(
outcomes=['failed'],
conditions=[('failed', [('move head and base at the end', 'failed')
]), ('failed', [('Groupwait', 'end')])])
with _sm_move_head_and_base_end__3:
# x:132 y:57
OperatableStateMachine.add('move head and base at the end',
_sm_move_head_and_base_at_the_end_1,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
# x:121 y:218
OperatableStateMachine.add('Groupwait',
_sm_groupwait_0,
transitions={'end': 'failed'},
autonomy={'end': Autonomy.Inherit})
# x:67 y:227
_sm_export_no_waypoint_4 = OperatableStateMachine(
outcomes=['done'], output_keys=['waipoint', 'area_name'])
with _sm_export_no_waypoint_4:
# x:30 y:40
OperatableStateMachine.add('noWaypoint',
SetKey(Value=None),
transitions={'done': 'NoName'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'waipoint'})
# x:43 y:129
OperatableStateMachine.add('NoName',
SetKey(Value=None),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'area_name'})
# x:30 y:458
_sm_export_waypoint_5 = OperatableStateMachine(
outcomes=['done'],
input_keys=['entity'],
output_keys=['waipoint', 'area_name'])
with _sm_export_waypoint_5:
# x:58 y:107
OperatableStateMachine.add(
'Extract Wayppoint',
CalculationState(calculation=lambda x: x.waypoint),
transitions={'done': 'Extract Name'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'entity',
'output_value': 'waipoint'
})
# x:67 y:257
OperatableStateMachine.add(
'Extract Name',
CalculationState(calculation=lambda x: x.name),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'entity',
'output_value': 'area_name'
})
# x:379 y:221
_sm_wait_to_compte_6 = OperatableStateMachine(outcomes=['finished'])
with _sm_wait_to_compte_6:
# x:77 y:195
OperatableStateMachine.add('one more wait',
WaitState(wait_time=60),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:535 y:314
_sm_turn_around_7 = OperatableStateMachine(outcomes=['failed'])
with _sm_turn_around_7:
# x:47 y:45
OperatableStateMachine.add('set orientation',
SetKey(Value=1.57),
transitions={'done': 'action_turn'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'rotation'})
# x:42 y:213
OperatableStateMachine.add('move head',
SaraSetHeadAngle(pitch=0, yaw=1.57),
transitions={'done': 'waitwait'},
autonomy={'done': Autonomy.Off})
# x:53 y:294
OperatableStateMachine.add('waitwait',
WaitState(wait_time=10),
transitions={'done': 'action_turn_2'},
autonomy={'done': Autonomy.Off})
# x:33 y:122
OperatableStateMachine.add(
'action_turn',
self.use_behavior(
action_turnSM,
'Try to reach/check person behind/move head and base/turn around/action_turn'
),
transitions={
'finished': 'move head',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'rotation': 'rotation'})
# x:31 y:434
OperatableStateMachine.add(
'action_turn_2',
self.use_behavior(
action_turnSM,
'Try to reach/check person behind/move head and base/turn around/action_turn_2'
),
transitions={
'finished': 'head left right',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'rotation': 'rotation'})
# x:35 y:595
OperatableStateMachine.add('head left right',
SaraSetHeadAngle(pitch=0, yaw=1.57),
transitions={'done': 'wait turn head'},
autonomy={'done': Autonomy.Off})
# x:308 y:537
OperatableStateMachine.add('wait turn head',
WaitState(wait_time=10),
transitions={'done': 'head right left'},
autonomy={'done': Autonomy.Off})
# x:495 y:604
OperatableStateMachine.add('head right left',
SaraSetHeadAngle(pitch=0, yaw=-1.57),
transitions={'done': 'wait wait wait'},
autonomy={'done': Autonomy.Off})
# x:271 y:711
OperatableStateMachine.add('wait wait wait',
WaitState(wait_time=10),
transitions={'done': 'head left right'},
autonomy={'done': Autonomy.Off})
# x:845 y:395
_sm_find_human_8 = OperatableStateMachine(outcomes=['finished'],
input_keys=['ID'])
with _sm_find_human_8:
# x:223 y:137
OperatableStateMachine.add('find person',
GetEntityByID(),
transitions={
'found': 'finished',
'not_found': 'find person'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'ID',
'Entity': 'Entity'
})
# x:549 y:135, x:555 y:269, x:230 y:458
_sm_move_head_and_base_9 = ConcurrencyContainer(
outcomes=['failed'],
conditions=[('failed', [('turn around', 'failed')]),
('failed', [('wait to compte', 'finished')])])
with _sm_move_head_and_base_9:
# x:268 y:77
OperatableStateMachine.add('turn around',
_sm_turn_around_7,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
# x:263 y:246
OperatableStateMachine.add('wait to compte',
_sm_wait_to_compte_6,
transitions={'finished': 'failed'},
autonomy={'finished': Autonomy.Inherit})
# x:493 y:206
_sm_container_10 = OperatableStateMachine(outcomes=['check'])
with _sm_container_10:
# x:230 y:160
OperatableStateMachine.add('wait long',
WaitState(wait_time=40),
transitions={'done': 'check'},
autonomy={'done': Autonomy.Off})
# x:30 y:458, x:706 y:447
_sm_navigate_to_the_point_11 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['pose'])
with _sm_navigate_to_the_point_11:
# x:174 y:122
OperatableStateMachine.add('set relative',
SetKey(Value=False),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'relative'})
# x:347 y:191
OperatableStateMachine.add(
'Action_Move',
self.use_behavior(
Action_MoveSM,
'Try to reach/Container/navigate to the point/Action_Move'
),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pose': 'pose'})
# x:728 y:335, x:792 y:103, x:738 y:249, x:724 y:448
_sm_check_person_behind_12 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
input_keys=['ID'],
conditions=[('failed', [('move head and base', 'failed')]),
('finished', [('find human', 'finished')])])
with _sm_check_person_behind_12:
# x:250 y:72
OperatableStateMachine.add('move head and base',
_sm_move_head_and_base_9,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
# x:261 y:238
OperatableStateMachine.add('find human',
_sm_find_human_8,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:635 y:61, x:634 y:159, x:597 y:300, x:330 y:458, x:430 y:458, x:530 y:458
_sm_container_13 = ConcurrencyContainer(
outcomes=['finished', 'failed', 'check'],
input_keys=['waypoint'],
conditions=[('check', [('Container', 'check')]),
('finished', [('navigate to the point', 'finished')]),
('failed', [('navigate to the point', 'failed')])])
with _sm_container_13:
# x:315 y:51
OperatableStateMachine.add('navigate to the point',
_sm_navigate_to_the_point_11,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pose': 'waypoint'})
# x:322 y:257
OperatableStateMachine.add('Container',
_sm_container_10,
transitions={'check': 'check'},
autonomy={'check': Autonomy.Inherit})
# x:98 y:451
_sm_get_area_containers_14 = OperatableStateMachine(
outcomes=['done'],
input_keys=['command'],
output_keys=['containers'])
with _sm_get_area_containers_14:
# x:46 y:31
OperatableStateMachine.add(
'Set Empty Array',
SetKey(Value=[]),
transitions={'done': 'Set initial index'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'containers'})
# x:50 y:110
OperatableStateMachine.add('Set initial index',
SetKey(Value=2),
transitions={'done': 'Set Loop Max'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'iLoop'})
# x:49 y:192
OperatableStateMachine.add(
'Set Loop Max',
CalculationState(calculation=lambda x: len(x)),
transitions={'done': 'Check Loop End'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'command',
'output_value': 'iLoopMax'
})
# x:514 y:278
OperatableStateMachine.add(
'Increment',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'Print container array'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'iLoop',
'output_value': 'iLoop'
})
# x:271 y:154
OperatableStateMachine.add(
'Add Container to Array',
FlexibleCalculationState(
calculation=lambda x: x[2] + [x[0][x[1]]],
input_keys=['command', 'iLoop', 'containers']),
transitions={'done': 'Increment'},
autonomy={'done': Autonomy.Off},
remapping={
'command': 'command',
'iLoop': 'iLoop',
'containers': 'containers',
'output_value': 'containers'
})
# x:42 y:287
OperatableStateMachine.add('Check Loop End',
FlexibleCheckConditionState(
predicate=lambda x: x[0] < x[1],
input_keys=['iLoop', 'iLoopMax']),
transitions={
'true': 'Add Container to Array',
'false': 'done'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'iLoop': 'iLoop',
'iLoopMax': 'iLoopMax'
})
# x:279 y:401
OperatableStateMachine.add('Print container array',
LogKeyState(
text="Containers: \n {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'Check Loop End'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'containers'})
# x:626 y:228, x:607 y:71, x:230 y:458, x:330 y:458
_sm_operator_is_still_there_15 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
input_keys=['ID'],
conditions=[('finished', [('find a human', 'finished')]),
('failed', [('Move head and base end ', 'failed')])])
with _sm_operator_is_still_there_15:
# x:207 y:54
OperatableStateMachine.add('Move head and base end ',
_sm_move_head_and_base_end__3,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
# x:222 y:190
OperatableStateMachine.add('find a human',
_sm_find_a_human_2,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:1620 y:109, x:1648 y:375
_sm_try_to_find_area_16 = OperatableStateMachine(
outcomes=['found', 'not_found'],
input_keys=['area_to_search', 'containers'],
output_keys=['area_name', 'waypoint'])
with _sm_try_to_find_area_16:
# x:517 y:67
OperatableStateMachine.add(
'WonderlandUniqueEnity',
self.use_behavior(WonderlandUniqueEnitySM,
'Try to find area/WonderlandUniqueEnity'),
transitions={
'found': 'Export Waypoint',
'not_found': 'Export No Waypoint'
},
autonomy={
'found': Autonomy.Inherit,
'not_found': Autonomy.Inherit
},
remapping={
'name': 'area_to_search',
'containers': 'containers',
'entity': 'entity'
})
# x:966 y:71
OperatableStateMachine.add('Export Waypoint',
_sm_export_waypoint_5,
transitions={'done': 'say_going'},
autonomy={'done': Autonomy.Inherit},
remapping={
'entity': 'entity',
'waipoint': 'waypoint',
'area_name': 'area_name'
})
# x:1247 y:295
OperatableStateMachine.add('Export No Waypoint',
_sm_export_no_waypoint_4,
transitions={'done': 'not_found'},
autonomy={'done': Autonomy.Inherit},
remapping={
'waipoint': 'waypoint',
'area_name': 'area_name'
})
# x:1273 y:106
OperatableStateMachine.add(
'say_going',
SaraSay(sentence=lambda x: "I'm going to the " + x,
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'found'},
autonomy={'done': Autonomy.Off})
# x:323 y:632, x:638 y:631
_sm_try_to_reach_17 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['waypoint', 'relative', 'areaName', 'ID'])
with _sm_try_to_reach_17:
# x:252 y:161
OperatableStateMachine.add('Container',
_sm_container_13,
transitions={
'finished': 'say_reached',
'failed': 'Say_not_reached',
'check': 'say check'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'check': Autonomy.Inherit
},
remapping={'waypoint': 'waypoint'})
# x:581 y:163
OperatableStateMachine.add('check person behind',
_sm_check_person_behind_12,
transitions={
'finished': 'say found',
'failed': 'say lost'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'ID': 'ID'})
# x:706 y:484
OperatableStateMachine.add(
'say lost',
SaraSay(sentence="Oh no! I lost my operator!",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:445 y:69
OperatableStateMachine.add(
'say check',
SaraSay(sentence="I check if my operator is still there",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'check person behind'},
autonomy={'done': Autonomy.Off})
# x:443 y:187
OperatableStateMachine.add(
'say found',
SaraSay(sentence="Great. You are still there.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'Container'},
autonomy={'done': Autonomy.Off})
# x:463 y:398
OperatableStateMachine.add(
'Say_not_reached',
SaraSay(sentence=lambda x: "I have not reach the " + x + "!",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:61 y:366
OperatableStateMachine.add(
'say_reached',
SaraSay(sentence=lambda x: "I have reach the " + x + "!",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:871 y:59
_sm_decompose_command_18 = OperatableStateMachine(
outcomes=['done'],
input_keys=['command'],
output_keys=['containers', 'area'])
with _sm_decompose_command_18:
# x:163 y:34
OperatableStateMachine.add('Set State Command',
Set_a_step(step=0),
transitions={'done': 'Get area name'},
autonomy={'done': Autonomy.Off})
# x:387 y:58
OperatableStateMachine.add(
'Get area name',
CalculationState(calculation=lambda x: x[1]),
transitions={'done': 'Get area containers'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'command',
'output_value': 'area'
})
# x:597 y:94
OperatableStateMachine.add('Get area containers',
_sm_get_area_containers_14,
transitions={'done': 'done'},
autonomy={'done': Autonomy.Inherit},
remapping={
'command': 'command',
'containers': 'containers'
})
with _state_machine:
# x:54 y:24
OperatableStateMachine.add(
'Get Person Id',
GetRosParam(ParamName="behavior/FoundPerson/Id"),
transitions={
'done': 'GetPerson',
'failed': 'Cant Find Person'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'Value': 'ID'})
# x:19 y:152
OperatableStateMachine.add(
'Decompose Command',
_sm_decompose_command_18,
transitions={'done': 'Try to find area'},
autonomy={'done': Autonomy.Inherit},
remapping={
'command': 'Action',
'containers': 'containers',
'area': 'area'
})
# x:43 y:358
OperatableStateMachine.add('Try to reach',
_sm_try_to_reach_17,
transitions={
'finished':
'operator is still there',
'failed': 'cause2'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'waypoint': 'waypoint',
'relative': 'relative',
'areaName': 'area_name',
'ID': 'ID'
})
# x:360 y:109
OperatableStateMachine.add('Cant Find Person',
SaraSay(
sentence="I can't find a person.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'cause1'},
autonomy={'done': Autonomy.Off})
# x:52 y:82
OperatableStateMachine.add('GetPerson',
GetEntityByID(),
transitions={
'found': 'Decompose Command',
'not_found': 'Cant Find Person'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'ID',
'Entity': 'Entity'
})
# x:36 y:219
OperatableStateMachine.add('Try to find area',
_sm_try_to_find_area_16,
transitions={
'found': 'sayfollowme',
'not_found': 'Cant Find Person'
},
autonomy={
'found': Autonomy.Inherit,
'not_found': Autonomy.Inherit
},
remapping={
'area_to_search': 'area',
'containers': 'containers',
'area_name': 'area_name',
'waypoint': 'waypoint'
})
# x:27 y:438
OperatableStateMachine.add('operator is still there',
_sm_operator_is_still_there_15,
transitions={
'finished': 'getentitybyID',
'failed': 'say lost operator'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'ID': 'ID'})
# x:347 y:370
OperatableStateMachine.add(
'say lost operator',
SaraSay(
sentence=
"I have reach my goal but I lost the person I was guiding.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'cause3'},
autonomy={'done': Autonomy.Off})
# x:49 y:291
OperatableStateMachine.add('sayfollowme',
SaraSay(sentence="Follow me please.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'Try to reach'},
autonomy={'done': Autonomy.Off})
# x:808 y:451
OperatableStateMachine.add('head to middle',
SaraSetHeadAngle(pitch=0, yaw=0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:48 y:525
OperatableStateMachine.add('getentitybyID',
GetEntityByID(),
transitions={
'found': 'get entity to point',
'not_found': 'say_reach_the_entity'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'ID',
'Entity': 'Entity'
})
# x:249 y:562
OperatableStateMachine.add('get entity to point',
WonderlandGetEntityVerbal(),
transitions={
'one': 'find the point',
'multiple': 'say_reach_the_entity',
'none': 'say_reach_the_entity',
'error': 'say_reach_the_entity'
},
autonomy={
'one': Autonomy.Off,
'multiple': Autonomy.Off,
'none': Autonomy.Off,
'error': Autonomy.Off
},
remapping={
'name': 'area_name',
'containers': 'containers',
'entities': 'entities',
'firstEntity': 'firstEntity'
})
# x:708 y:529
OperatableStateMachine.add(
'Action_point_at',
self.use_behavior(Action_point_atSM, 'Action_point_at'),
transitions={
'finished': 'say_reach_the_entity',
'failed': 'say_reach_the_entity'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'targetPoint': 'targetPoint'})
# x:539 y:558
OperatableStateMachine.add(
'find the point',
CalculationState(calculation=lambda x: x.position),
transitions={'done': 'Action_point_at'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'entities',
'output_value': 'targetPoint'
})
# x:547 y:121
OperatableStateMachine.add(
'cause1',
SetKey(Value="I didn't find any persone"),
transitions={'done': 'setrosparamfail'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Key'})
# x:507 y:220
OperatableStateMachine.add(
'cause2',
SetKey(Value="I did not reach the area"),
transitions={'done': 'setrosparamfail'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Key'})
# x:559 y:337
OperatableStateMachine.add(
'cause3',
SetKey(Value="I lost the person I was guiding."),
transitions={'done': 'setrosparamfail'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Key'})
# x:656 y:250
OperatableStateMachine.add(
'setrosparamfail',
SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off},
remapping={'Value': 'Key'})
# x:523 y:462
OperatableStateMachine.add(
'say_reach_the_entity',
SaraSay(sentence=lambda x: "Here is the " + x,
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'head to middle'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:486 y:179, x:68 y:193
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:417 y:37, x:130 y:365
_sm_movetogoal_0 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['goal'])
with _sm_movetogoal_0:
# x:88 y:28
OperatableStateMachine.add('MoveToHololens',
MoveBaseState(head_angle=25),
transitions={
'arrived': 'finished',
'failed': 'SayBlocked'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'goal'})
# x:347 y:142
OperatableStateMachine.add(
'SayBlocked',
TalkState(
message=
'The way is blocked. Please clear the way and tell me when it is clear.',
blocking=True),
transitions={
'done': 'WaitForClear',
'failed': 'WaitForClear'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
})
# x:80 y:247
OperatableStateMachine.add(
'WaitForClear',
WaitForNaoQiSpeechState(
strings_to_rec=['The way is clear now'],
outcomes=['clear'],
topic='/pepper_robot/speechrec/context'),
transitions={'clear': 'MoveToHololens'},
autonomy={'clear': Autonomy.Off})
# x:533 y:33, x:115 y:357, x:540 y:161, x:537 y:83, x:39 y:358, x:187 y:354
_sm_interruptiblemove_1 = ConcurrencyContainer(
outcomes=['finished', 'failed', 'interrupted'],
input_keys=['goal'],
output_keys=['hololens_gen'],
conditions=[('finished', [('MoveToGoal', 'finished')]),
('finished', [('MoveToGoal', 'failed')]),
('interrupted', [('WaitForHololensGoal', 'done')])])
with _sm_interruptiblemove_1:
# x:151 y:34
OperatableStateMachine.add('MoveToGoal',
_sm_movetogoal_0,
transitions={
'finished': 'finished',
'failed': 'finished'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'goal': 'goal'})
# x:140 y:144
OperatableStateMachine.add(
'WaitForHololensGoal',
WaitForRosNavgoalState(topic='/hololens/navgoal'),
transitions={'done': 'interrupted'},
autonomy={'done': Autonomy.Off},
remapping={'navgoal': 'hololens_gen'})
# x:471 y:22, x:130 y:365, x:447 y:167, x:470 y:102, x:206 y:368, x:276 y:363
_sm_interruptibledemo_2 = ConcurrencyContainer(
outcomes=['finished', 'failed', 'interrupted'],
output_keys=['hololens_gen'],
conditions=[('interrupted', [('WaitForHololensGoal', 'done')]),
('finished', [('Pepper AR Demo', 'finished')]),
('finished', [('Pepper AR Demo', 'failed')])])
with _sm_interruptibledemo_2:
# x:119 y:28
OperatableStateMachine.add('Pepper AR Demo',
self.use_behavior(
PepperARDemoSM,
'InterruptibleDemo/Pepper AR Demo'),
transitions={
'finished': 'finished',
'failed': 'finished'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:121 y:156
OperatableStateMachine.add(
'WaitForHololensGoal',
WaitForRosNavgoalState(topic='/hololens/navgoal'),
transitions={'done': 'interrupted'},
autonomy={'done': Autonomy.Off},
remapping={'navgoal': 'hololens_gen'})
with _state_machine:
# x:262 y:23
OperatableStateMachine.add(
'InterruptibleDemo',
_sm_interruptibledemo_2,
transitions={
'finished': 'InterruptibleDemo',
'failed': 'failed',
'interrupted': 'InterruptibleMove'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'interrupted': Autonomy.Inherit
},
remapping={'hololens_gen': 'hololens_gen'})
# x:260 y:281
OperatableStateMachine.add('InterruptibleMove',
_sm_interruptiblemove_1,
transitions={
'finished': 'InterruptibleDemo',
'failed': 'failed',
'interrupted': 'InterruptibleMove'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'interrupted': Autonomy.Inherit
},
remapping={
'goal': 'hololens_gen',
'hololens_gen': 'hololens_gen'
})
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):
wait_time_after_flying = 1
wall_following_desired_yaw = -math.pi / 2
# x:139 y:774, x:281 y:333
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.simulation = True
_state_machine.userdata.flying_altitude = 5
_state_machine.userdata.waiting_spot = mrs_msgs.srv.ReferenceStampedSrvRequest(
)
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
param_prefix_namespace = "flexbe_behavior_launcher/"
flying_altitude_list = rospy.get_param(param_prefix_namespace +
'main/flying_altitudes')
uav_name_list = rospy.get_param(param_prefix_namespace +
'main/robot_name_list')
robot_name = rospy.get_param(param_prefix_namespace + 'robot_name')
waiting_spots = rospy.get_param(param_prefix_namespace +
'main/waiting_spots')
wall_following_desired_speed = rospy.get_param(
param_prefix_namespace + 'main/wall_following_desired_speed')
wall_following_desired_wall_distance = rospy.get_param(
param_prefix_namespace +
'main/wall_following_desired_wall_distance')
_state_machine.userdata.robot_name = robot_name
Logger.loginfo("wall_following_desired_speed: %s" %
wall_following_desired_speed)
Logger.loginfo("wall_following_desired_wall_distance: %s" %
wall_following_desired_wall_distance)
window_position_estimation_reset_topic = rospy.get_param(
param_prefix_namespace +
'main/window_position_estimation_reset_topic')
window_position_estimation_reset_topic = "/" + robot_name + "/" + window_position_estimation_reset_topic
Logger.loginfo("window_position_estimation_reset_topic: %s" %
window_position_estimation_reset_topic)
window_position_estimation_closest_window_topic = rospy.get_param(
param_prefix_namespace +
'main/window_position_estimation_closest_window_topic')
window_position_estimation_closest_window_topic = "/" + robot_name + "/" + window_position_estimation_closest_window_topic
Logger.loginfo("window_position_estimation_closest_window_topic: %s" %
window_position_estimation_closest_window_topic)
land_home_service_topic = rospy.get_param(
param_prefix_namespace + 'main/land_home_service_topic')
land_home_service_topic = "/" + robot_name + "/" + land_home_service_topic
Logger.loginfo("land_home_service_topic: %s" % land_home_service_topic)
e_land_topic = rospy.get_param(param_prefix_namespace +
'main/e_land_topic')
e_land_topic = "/" + robot_name + "/" + e_land_topic
Logger.loginfo("e_land_topic: %s" % e_land_topic)
fire_detection_topic = rospy.get_param(param_prefix_namespace +
'main/fire_detection_topic')
fire_detection_topic = "/" + robot_name + "/" + fire_detection_topic
Logger.loginfo("fire_detection_topic: %s" % fire_detection_topic)
window_flyer_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/window_flyer_action_server_name')
window_flyer_action_server_name = "/" + robot_name + "/" + window_flyer_action_server_name
Logger.loginfo("window_flyer_action_server_name: %s" %
window_flyer_action_server_name)
wall_following_action_server_name = rospy.get_param(
param_prefix_namespace + 'main/wall_following_action_server_name')
wall_following_action_server_name = "/" + robot_name + "/" + wall_following_action_server_name
Logger.loginfo("wall_following_action_server_name: %s" %
wall_following_action_server_name)
Logger.loginfo("fire_ch_sm.robot_name: %s" % robot_name)
flying_altitude = 3
waiting_spot = mrs_msgs.srv.ReferenceStampedSrvRequest()
waiting_spot.header.frame_id = robot_name + "/rtk_origin"
for i in range(len(uav_name_list)):
if uav_name_list[i] == robot_name:
flying_altitude = flying_altitude_list[i]
waiting_spot.reference.position.x = waiting_spots[i * 3]
waiting_spot.reference.position.y = waiting_spots[i * 3 + 1]
waiting_spot.reference.position.z = flying_altitude
waiting_spot.reference.yaw = waiting_spots[i * 3 + 2]
break
_state_machine.userdata.flying_altitude = flying_altitude
_state_machine.userdata.waiting_spot = waiting_spot
Logger.loginfo("fire_ch_sm.flying_altitude: %s" %
_state_machine.userdata.flying_altitude)
Logger.loginfo("fire_ch_sm.waiting_spot: %s" %
_state_machine.userdata.waiting_spot)
# [/MANUAL_CREATE]
# x:30 y:480, x:130 y:480, x:230 y:480, x:734 y:480, x:430 y:480, x:530 y:483, x:630 y:480, x:335 y:480, x:830 y:480
_sm_detect_window_inside_0 = ConcurrencyContainer(
outcomes=['preempted', 'no_detection', 'error', 'window_detected'],
input_keys=['window_id'],
output_keys=['window_id'],
conditions=[
('preempted', [('Wall_Follow_From_Inside', 'preempted')]),
('no_detection', [('Wall_Follow_From_Inside', 'successed')]),
('error', [('Wall_Follow_From_Inside', 'aborted')]),
('window_detected', [('Wait_For_Window_Estimation',
'successed')]),
('error', [('Wait_For_Window_Estimation', 'failed')])
])
with _sm_detect_window_inside_0:
# x:156 y:151
OperatableStateMachine.add(
'Wall_Follow_From_Inside',
WallFollowingActionState(
action_server_name=wall_following_action_server_name,
desired_yaw=wall_following_desired_yaw,
desired_speed=wall_following_desired_speed,
desired_wall_distance=wall_following_desired_wall_distance
),
transitions={
'successed': 'no_detection',
'aborted': 'error',
'preempted': 'preempted'
},
autonomy={
'successed': Autonomy.Off,
'aborted': Autonomy.Off,
'preempted': Autonomy.Off
})
# x:537 y:145
OperatableStateMachine.add(
'Wait_For_Window_Estimation',
WaitForMsgState(
topic=window_position_estimation_closest_window_topic,
wait_time=-1,
function=lambda x: True,
output_keys=['window_id'],
output_function=self.window_detection_cb),
transitions={
'successed': 'window_detected',
'failed': 'error'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'window_id': 'window_id'})
# x:133 y:390, x:683 y:390, x:233 y:390, x:333 y:390, x:533 y:390, x:530 y:550, x:630 y:550, x:730 y:550, x:830 y:550
_sm_detect_fire_inside_1 = ConcurrencyContainer(
outcomes=['preempted', 'fire_detected', 'no_detection', 'error'],
conditions=[
('error', [('Wall_Follow_From_Inside', 'aborted')]),
('no_detection', [('Wall_Follow_From_Inside', 'successed')]),
('preempted', [('Wall_Follow_From_Inside', 'preempted')]),
('error', [('Wait_For_Fire_Detection', 'failed')]),
('fire_detected', [('Wait_For_Fire_Detection', 'successed')])
])
with _sm_detect_fire_inside_1:
# x:198 y:174
OperatableStateMachine.add(
'Wall_Follow_From_Inside',
WallFollowingActionState(
action_server_name=wall_following_action_server_name,
desired_yaw=wall_following_desired_yaw,
desired_speed=wall_following_desired_speed,
desired_wall_distance=wall_following_desired_wall_distance
),
transitions={
'successed': 'no_detection',
'aborted': 'error',
'preempted': 'preempted'
},
autonomy={
'successed': Autonomy.Off,
'aborted': Autonomy.Off,
'preempted': Autonomy.Off
})
# x:579 y:174
OperatableStateMachine.add('Wait_For_Fire_Detection',
WaitForMsgState(
topic=fire_detection_topic,
wait_time=-1,
function=lambda x: True,
output_keys=[],
output_function=None),
transitions={
'successed': 'fire_detected',
'failed': 'error'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
})
with _state_machine:
# x:76 y:71
OperatableStateMachine.add('Prepare_UAV',
self.use_behavior(
Prepare_UAVSM, 'Prepare_UAV'),
transitions={
'finished': 'GotoAltitude',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'simulation': 'simulation'})
# x:326 y:71
OperatableStateMachine.add('GotoAltitude',
self.use_behavior(
GotoAltitudeSM, 'GotoAltitude'),
transitions={
'reached': 'Goto_Waiting_Spot',
'failed': 'Emergency_Landing'
},
autonomy={
'reached': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'goal': 'flying_altitude'})
# x:971 y:268
OperatableStateMachine.add(
'Flythrough_Window_Inside',
WindowFlyerActionState(
action_server_name=window_flyer_action_server_name,
yaw_offset=3.14),
transitions={
'successed': 'Detect_Fire_Inside',
'window_not_found': 'Emergency_Landing',
'already_flythrough': 'Emergency_Landing',
'server_not_initialized': 'Emergency_Landing',
'error': 'Emergency_Landing',
'preempted': 'Emergency_Landing'
},
autonomy={
'successed': Autonomy.Off,
'window_not_found': Autonomy.Off,
'already_flythrough': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'window_id': 'window_id',
'window_position': 'window_position'
})
# x:774 y:74
OperatableStateMachine.add(
'Reset_Window_Estimation',
ServiceTriggerState(
service_topic=window_position_estimation_reset_topic,
state_name="WindowEstimation"),
transitions={
'finished': 'Wait',
'failed': 'Emergency_Landing'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:965 y:178
OperatableStateMachine.add(
'Wait_For_Window_Estimation',
WaitForMsgState(
topic=window_position_estimation_closest_window_topic,
wait_time=5,
function=lambda x: True,
output_keys=['window_id'],
output_function=self.window_detection_cb),
transitions={
'successed': 'Flythrough_Window_Inside',
'failed': 'Call_Land_Home'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'window_id': 'window_id'})
# x:1008 y:74
OperatableStateMachine.add(
'Wait',
WaitState(wait_time=wait_time_after_flying),
transitions={'done': 'Wait_For_Window_Estimation'},
autonomy={'done': Autonomy.Off})
# x:954 y:536
OperatableStateMachine.add(
'Wait_For_Window_Estimation_2',
WaitForMsgState(
topic=window_position_estimation_closest_window_topic,
wait_time=5,
function=lambda x: True,
output_keys=['window_id'],
output_function=self.window_detection_cb),
transitions={
'successed': 'Flythrough_Window_Outside',
'failed': 'Reset_Window_Estimation_2'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'window_id': 'window_id'})
# x:663 y:755
OperatableStateMachine.add(
'Flythrough_Window_Outside',
WindowFlyerActionState(
action_server_name=window_flyer_action_server_name,
yaw_offset=0),
transitions={
'successed': 'Call_Land_Home',
'window_not_found': 'Emergency_Landing',
'already_flythrough': 'Emergency_Landing',
'server_not_initialized': 'Emergency_Landing',
'error': 'Emergency_Landing',
'preempted': 'Emergency_Landing'
},
autonomy={
'successed': Autonomy.Off,
'window_not_found': Autonomy.Off,
'already_flythrough': Autonomy.Off,
'server_not_initialized': Autonomy.Off,
'error': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={
'window_id': 'window_id',
'window_position': 'window_position'
})
# x:386 y:754
OperatableStateMachine.add(
'Call_Land_Home',
ServiceTriggerState(service_topic=land_home_service_topic,
state_name="ControlManager"),
transitions={
'finished': 'finished',
'failed': 'Call_Land_Home'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:519 y:329
OperatableStateMachine.add('Emergency_Landing',
ServiceTriggerState(
service_topic=e_land_topic,
state_name="ControlManager"),
transitions={
'finished': 'failed',
'failed': 'Emergency_Landing'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:977 y:343
OperatableStateMachine.add('Detect_Fire_Inside',
_sm_detect_fire_inside_1,
transitions={
'preempted': 'Emergency_Landing',
'fire_detected':
'Goto_ClosestWindow2',
'no_detection': 'Emergency_Landing',
'error': 'Emergency_Landing'
},
autonomy={
'preempted': Autonomy.Inherit,
'fire_detected': Autonomy.Inherit,
'no_detection': Autonomy.Inherit,
'error': Autonomy.Inherit
})
# x:555 y:69
OperatableStateMachine.add(
'Goto_Waiting_Spot',
self.use_behavior(goto_referenceSM, 'Goto_Waiting_Spot'),
transitions={
'reached': 'Reset_Window_Estimation',
'failed': 'Emergency_Landing'
},
autonomy={
'reached': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'goal': 'waiting_spot'})
# x:967 y:443
OperatableStateMachine.add(
'Goto_ClosestWindow2',
self.use_behavior(goto_referenceSM, 'Goto_ClosestWindow2'),
transitions={
'reached': 'Wait_For_Window_Estimation_2',
'failed': 'Emergency_Landing'
},
autonomy={
'reached': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'goal': 'window_position'})
# x:962 y:625
OperatableStateMachine.add(
'Reset_Window_Estimation_2',
ServiceTriggerState(
service_topic=window_position_estimation_reset_topic,
state_name="WindowEstimation"),
transitions={
'finished': 'Detect_Window_Inside',
'failed': 'Emergency_Landing'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
})
# x:965 y:718
OperatableStateMachine.add('Detect_Window_Inside',
_sm_detect_window_inside_0,
transitions={
'preempted':
'Emergency_Landing',
'no_detection':
'Emergency_Landing',
'error':
'Emergency_Landing',
'window_detected':
'Flythrough_Window_Outside'
},
autonomy={
'preempted': Autonomy.Inherit,
'no_detection': Autonomy.Inherit,
'error': Autonomy.Inherit,
'window_detected': Autonomy.Inherit
},
remapping={'window_id': 'window_id'})
return _state_machine
def create(self):
# x:392 y:613, x:739 y:627, x:59 y:602, x:34 y:328
_state_machine = OperatableStateMachine(
outcomes=['timeout', 'failed', 'invalid_pose', 'unknown_keys'],
input_keys=['key_pressed_msg'],
output_keys=['key_pressed_msg'])
_state_machine.userdata.key_pressed_msg = KeyPressed()
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:353, x:130 y:353, x:626 y:359, x:330 y:353, x:430 y:353, x:530 y:353
_sm_waitkey_0 = ConcurrencyContainer(
outcomes=['received', 'timeout', 'failed'],
output_keys=['key_pressed_msg'],
conditions=[('received', [('WaitKeyPressed', 'received')]),
('timeout', [('WaitTimeout', 'done')]),
('failed', [('WaitKeyPressed', 'unavailable')])])
with _sm_waitkey_0:
# x:281 y:146
OperatableStateMachine.add(
'WaitKeyPressed',
WaitForMessageState(topic='/hmi/joy_decoder/keys_pressed',
condition=self.trigger,
buffered=False,
clear=True),
transitions={
'received': 'received',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'key_pressed_msg'})
# x:509 y:151
OperatableStateMachine.add('WaitTimeout',
WaitState(wait_time=self.timeout),
transitions={'done': 'timeout'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:51 y:31
OperatableStateMachine.add(
'CheckExitEnter',
DecisionState(
outcomes=['animation', 'exit', 'walk', 'unknown'],
conditions=self.decision),
transitions={
'animation': 'unknown_keys',
'exit': 'unknown_keys',
'walk': 'PrepareForWalk',
'unknown': 'unknown_keys'
},
autonomy={
'animation': Autonomy.Off,
'exit': Autonomy.Off,
'walk': Autonomy.Off,
'unknown': Autonomy.Off
},
remapping={'input_value': 'key_pressed_msg'})
# x:326 y:259
OperatableStateMachine.add(
'CheckExit',
DecisionState(
outcomes=['animation', 'walk', 'exit', 'unknown'],
conditions=self.decision),
transitions={
'animation': 'EndWalkUnknownKeys',
'walk': 'ProcessKeyMsg',
'exit': 'EndWalkUnknownKeys',
'unknown': 'WaitKey'
},
autonomy={
'animation': Autonomy.Off,
'walk': Autonomy.Off,
'exit': Autonomy.Off,
'unknown': Autonomy.Off
},
remapping={'input_value': 'key_pressed_msg'})
# x:703 y:410
OperatableStateMachine.add(
'ExecuteStepSeq',
ExecuteStepSequenceKey(
controller='motion/controller/step_sequence',
trajectory_ns='saved_msgs/step_sequence'),
transitions={
'success': 'WaitKey',
'unavailable': 'WaitKey',
'partial_movement': 'invalid_pose',
'invalid_pose': 'invalid_pose',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'unavailable': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'trajectory_param': 'motion_param'})
# x:431 y:381
OperatableStateMachine.add(
'WaitKey',
_sm_waitkey_0,
transitions={
'received': 'CheckExit',
'timeout': 'EndWalkTimeout',
'failed': 'failed'
},
autonomy={
'received': Autonomy.Inherit,
'timeout': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'key_pressed_msg': 'key_pressed_msg'})
# x:719 y:246
OperatableStateMachine.add(
'CheckNone',
CheckConditionState(predicate=lambda x: x == None),
transitions={
'true': 'WaitKey',
'false': 'ExecuteStepSeq'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'motion_param'})
# x:108 y:289
OperatableStateMachine.add(
'EndWalkUnknownKeys',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='crouch_end',
trajectory_ns='saved_msgs/joint_trajectory'),
transitions={
'success': 'unknown_keys',
'partial_movement': 'invalid_pose',
'invalid_pose': 'invalid_pose',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
})
# x:379 y:491
OperatableStateMachine.add(
'EndWalkTimeout',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='crouch_end',
trajectory_ns='saved_msgs/joint_trajectory'),
transitions={
'success': 'timeout',
'partial_movement': 'invalid_pose',
'invalid_pose': 'invalid_pose',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
})
# x:408 y:158
OperatableStateMachine.add(
'ProcessKeyMsg',
CalculationState(calculation=self.process_walk),
transitions={'done': 'CheckNone'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'key_pressed_msg',
'output_value': 'motion_param'
})
# x:211 y:72
OperatableStateMachine.add(
'PrepareForWalk',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='crouch_begin',
trajectory_ns='saved_msgs/joint_trajectory'),
transitions={
'success': 'ProcessKeyMsg',
'partial_movement': 'invalid_pose',
'invalid_pose': 'invalid_pose',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
})
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):
joy_topic = '/hmi/joystick'
# x:33 y:423, x:683 y:568
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['be_evil'])
_state_machine.userdata.cycle_counter = 0
_state_machine.userdata.be_evil = self.be_evil
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:356, x:644 y:383
_sm_detectjoyevent_0 = OperatableStateMachine(
outcomes=['received', 'unavailable'])
with _sm_detectjoyevent_0:
# x:229 y:154
OperatableStateMachine.add('JoyEvent',
SubscriberState(topic=joy_topic,
blocking=True,
clear=False),
transitions={
'received': 'CheckEvent',
'unavailable': 'unavailable'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:229 y:296
OperatableStateMachine.add(
'CheckEvent',
DecisionState(
outcomes=['yes', 'no'],
conditions=lambda msg: 'yes'
if any(msg.buttons) or any(msg.axes[0:5]) else 'no'),
transitions={
'yes': 'received',
'no': 'JoyEvent'
},
autonomy={
'yes': Autonomy.Off,
'no': Autonomy.Off
},
remapping={'input_value': 'message'})
# x:778 y:316, x:75 y:354, x:294 y:358, x:776 y:358, x:777 y:406, x:530 y:356
_sm_checkjoy_1 = ConcurrencyContainer(
outcomes=['no_activity', 'activity_detected', 'failed'],
conditions=[('no_activity', [('WaitForJoy', 'done')]),
('failed', [('DetectJoyEvent', 'unavailable')]),
('activity_detected', [('DetectJoyEvent', 'received')])
])
with _sm_checkjoy_1:
# x:219 y:132
OperatableStateMachine.add('DetectJoyEvent',
_sm_detectjoyevent_0,
transitions={
'received': 'activity_detected',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Inherit,
'unavailable': Autonomy.Inherit
})
# x:624 y:152
OperatableStateMachine.add('WaitForJoy',
WaitState(wait_time=self.wait_time),
transitions={'done': 'no_activity'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:646 y:31
OperatableStateMachine.add(
'RandomChoose',
DecisionState(outcomes=['greeting', 'play', 'cheer', 'bad'],
conditions=self.select_behavior),
transitions={
'greeting': 'Greeting',
'play': 'Play',
'cheer': 'Cheer',
'bad': 'Bad'
},
autonomy={
'greeting': Autonomy.Low,
'play': Autonomy.Low,
'cheer': Autonomy.Low,
'bad': Autonomy.Low
},
remapping={'input_value': 'cycle_counter'})
# x:383 y:460
OperatableStateMachine.add('CheckJoy',
_sm_checkjoy_1,
transitions={
'no_activity': 'ResetCounter',
'activity_detected':
'AddCycleCounter',
'failed': 'failed'
},
autonomy={
'no_activity': Autonomy.Inherit,
'activity_detected':
Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:221 y:134
OperatableStateMachine.add(
'AddCycleCounter',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'RandomChoose'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'cycle_counter',
'output_value': 'cycle_counter'
})
# x:483 y:171
OperatableStateMachine.add('Greeting',
self.use_behavior(
GreetingSM, 'Greeting'),
transitions={
'finished': 'CheckJoy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:849 y:197
OperatableStateMachine.add('Cheer',
self.use_behavior(CheerSM, 'Cheer'),
transitions={
'finished': 'CheckJoy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:687 y:197
OperatableStateMachine.add('Play',
self.use_behavior(PlaySM, 'Play'),
transitions={
'finished': 'CheckJoy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:991 y:203
OperatableStateMachine.add('Bad',
self.use_behavior(BadSM, 'Bad'),
transitions={
'finished': 'CheckJoy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:115 y:364
OperatableStateMachine.add(
'ResetCounter',
CalculationState(calculation=lambda x: 0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'cycle_counter',
'output_value': 'cycle_counter'
})
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:30 y:365, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:585 y:71, x:130 y:365
_sm_recovery_steps_0 = OperatableStateMachine(
outcomes=['finished', 'failed'])
with _sm_recovery_steps_0:
# x:94 y:63
OperatableStateMachine.add('Backup',
MoveDistanceState(
target_time=3,
distance=-0.33,
cmd_topic='/create_node/cmd_vel',
odometry_topic='/create_node/odom'),
transitions={
'done': 'Clear Costmaps',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
})
# x:241 y:70
OperatableStateMachine.add('Clear Costmaps',
ClearCostmapsState(costmap_topics=[
'/high_level_planner/clear_costmap',
'/mid_level_planner/clear_costmap',
'/low_level_planner/clear_costmap'
]),
transitions={
'done': 'Full Rotation',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
})
# x:398 y:63
OperatableStateMachine.add('Full Rotation',
RotateAngleState(
target_time=5,
cmd_topic='/create_node/cmd_vel',
odometry_topic='/create_node/odom'),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:792 y:148, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365
_sm_recovery_statemachine_1 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('failed', [('Bumper State', 'failed')]),
('failed', [('Recovery steps', 'failed')]),
('finished', [('Recovery steps', 'finished')])])
with _sm_recovery_statemachine_1:
# x:294 y:72
OperatableStateMachine.add('Recovery steps',
_sm_recovery_steps_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:73 y:193
OperatableStateMachine.add('Bumper State',
CreateStatusState(),
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Off})
with _state_machine:
# x:92 y:51
OperatableStateMachine.add('Recovery Statemachine',
_sm_recovery_statemachine_1,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
return _state_machine
def create(self):
joy_topic = '/hmi/joystick'
joint_trajectory_action = '/motion/controller/joint_trajectory'
# x:30 y:365, x:1031 y:242
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.rand_moves_config = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:397 y:287, x:130 y:297, x:230 y:297, x:330 y:297, x:459 y:297, x:530 y:297
_sm_joystickmovements_0 = ConcurrencyContainer(
outcomes=['failed', 'button1234', 'timeout'],
output_keys=['joy_msg'],
conditions=[('timeout', [('JoystickControl', 'done')]),
('failed', [('WaitButton1234Pressed', 'unavailable')]),
('button1234', [('WaitButton1234Pressed', 'received')])
])
with _sm_joystickmovements_0:
# x:385 y:129
OperatableStateMachine.add(
'JoystickControl',
JoystickJointControl(
joints_topic='/joint_states',
goal_joints_topic=
'/motion/controller/joint_state/out_joints_src_reset',
joy_topic=joy_topic,
buttons=[(4, 0.6, 'ear_l_joint', -3.0, 1.0),
(6, -0.6, 'ear_l_joint', -3.0, 1.0),
(5, 0.6, 'ear_r_joint', -3.0, 1.0),
(7, -0.6, 'ear_r_joint', -3.0, 1.0)],
axes=[(0, 0, 'eyes_yaw', -1.5, 1.5),
(1, 0, 'eyes_pitch', -1.5, 1.5),
(4, -0.4, 'head_joint4', -1.5, 1.5),
(5, 0.4, 'head_joint2', -1.5, 1.5),
(3, 0.5, 'mouth_joint', -0.6, 0)],
timeout=5.0),
transitions={'done': 'timeout'},
autonomy={'done': Autonomy.Off})
# x:130 y:136
OperatableStateMachine.add(
'WaitButton1234Pressed',
WaitForMessageState(
topic=joy_topic,
condition=lambda msg: any(msg.buttons[0:4]),
buffered=False,
clear=True),
transitions={
'received': 'button1234',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'joy_msg'})
# x:625 y:339, x:47 y:263, x:323 y:268, x:703 y:252, x:630 y:296, x:443 y:267, x:227 y:263
_sm_randmovements_1 = ConcurrencyContainer(
outcomes=['failed', 'timeout', 'joy_msg'],
input_keys=['rand_moves_config'],
output_keys=['joy_msg'],
conditions=[('failed', [('WaitJoystick', 'unavailable')]),
('failed', [('RandHeadMoves', 'failed')]),
('joy_msg', [('WaitJoystick', 'received')]),
('timeout', [('RandHeadMoves', 'done')])])
with _sm_randmovements_1:
# x:88 y:74
OperatableStateMachine.add(
'WaitJoystick',
WaitForMessageState(
topic=joy_topic,
condition=lambda msg: any(msg.buttons) or any(msg.axes),
buffered=False,
clear=False),
transitions={
'received': 'joy_msg',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'joy_msg'})
# x:332 y:60
OperatableStateMachine.add(
'RandHeadMoves',
RandJointsMovements(controller='joint_state_head',
duration=2000,
interval=[2.0, 5.0],
joints=['head_joint2', 'head_joint4'],
minimal=[-0.2, -0.3],
maximal=[0.3, 0.3]),
transitions={
'done': 'timeout',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'rand_moves_config'})
with _state_machine:
# x:57 y:26
OperatableStateMachine.add('RandMovements',
_sm_randmovements_1,
transitions={
'failed': 'failed',
'timeout': 'RandMovements',
'joy_msg': 'CheckButtonPressed'
},
autonomy={
'failed': Autonomy.Inherit,
'timeout': Autonomy.Inherit,
'joy_msg': Autonomy.Inherit
},
remapping={
'rand_moves_config':
'rand_moves_config',
'joy_msg': 'joy_msg'
})
# x:413 y:17
OperatableStateMachine.add('PlaceHead',
SrdfStateToMoveit(
config_name='head_group_basic',
move_group='head_group',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'SelectMovement',
'planning_failed': 'Wait',
'control_failed': 'Wait',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:373 y:236
OperatableStateMachine.add(
'Greeting',
ExecuteJointTrajectory(
action_topic=joint_trajectory_action,
trajectory_param='greeting',
trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={
'success': 'JoystickMovements',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:85 y:283
OperatableStateMachine.add('JoystickMovements',
_sm_joystickmovements_0,
transitions={
'failed': 'failed',
'button1234': 'PlaceHead',
'timeout': 'RandMovements'
},
autonomy={
'failed': Autonomy.Inherit,
'button1234': Autonomy.Inherit,
'timeout': Autonomy.Inherit
},
remapping={'joy_msg': 'joy_msg'})
# x:612 y:186
OperatableStateMachine.add(
'SelectMovement',
DecisionState(outcomes=[
'none', 'button1', 'button2', 'button3', 'button4'
],
conditions=self.move_selector),
transitions={
'none': 'RandMovements',
'button1': 'Greeting',
'button2': 'HeadShake',
'button3': 'GrownUp',
'button4': 'Greeting'
},
autonomy={
'none': Autonomy.Off,
'button1': Autonomy.Off,
'button2': Autonomy.Off,
'button3': Autonomy.Off,
'button4': Autonomy.Off
},
remapping={'input_value': 'joy_msg'})
# x:412 y:317
OperatableStateMachine.add(
'HeadShake',
ExecuteJointTrajectory(
action_topic=joint_trajectory_action,
trajectory_param='head_shake',
trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={
'success': 'JoystickMovements',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:472 y:404
OperatableStateMachine.add(
'GrownUp',
ExecuteJointTrajectory(
action_topic=joint_trajectory_action,
trajectory_param='very_grown_up',
trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={
'success': 'JoystickMovements',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:167 y:134
OperatableStateMachine.add(
'CheckButtonPressed',
DecisionState(outcomes=['button1234', 'other'],
conditions=lambda msg: 'button1234'
if any(msg.buttons[0:4]) else 'other'),
transitions={
'button1234': 'PlaceHead',
'other': 'JoystickMovements'
},
autonomy={
'button1234': Autonomy.Off,
'other': Autonomy.Off
},
remapping={'input_value': 'joy_msg'})
# x:686 y:104
OperatableStateMachine.add('PlaceHead_2',
SrdfStateToMoveit(
config_name='head_group_basic',
move_group='head_group',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'SelectMovement',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:703 y:18
OperatableStateMachine.add('Wait',
WaitState(wait_time=0.5),
transitions={'done': 'PlaceHead_2'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:660 y:509, x:880 y:203, x:715 y:440
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed', 'critical_fail'],
input_keys=['Action'])
_state_machine.userdata.Action = ["Pick", "bottle"]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365
_sm_group_0 = ConcurrencyContainer(outcomes=['done'],
input_keys=['pose_out'],
conditions=[
('done', [('move', 'arrived')]),
('done', [('move', 'failed')]),
('done', [('3', 'done')])
])
with _sm_group_0:
# x:30 y:40
OperatableStateMachine.add('move',
SaraMoveBase(reference="map"),
transitions={
'arrived': 'done',
'failed': 'done'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose_out'})
# x:179 y:91
OperatableStateMachine.add('3',
WaitState(wait_time=2),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:40 y:700
_sm_get_closer_1 = OperatableStateMachine(outcomes=['done'],
input_keys=['Object'])
with _sm_get_closer_1:
# x:59 y:36
OperatableStateMachine.add('set targetpose',
SetKey(Value="PreGripPose"),
transitions={'done': 'say closer'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'target'})
# x:88 y:374
OperatableStateMachine.add('set dist',
SetKey(Value=0.8),
transitions={'done': 'get close pos'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distance'})
# x:26 y:448
OperatableStateMachine.add('get close pos',
Get_Reacheable_Waypoint(),
transitions={'done': 'Group'},
autonomy={'done': Autonomy.Off},
remapping={
'pose_in': 'Pos',
'distance': 'distance',
'pose_out': 'pose_out'
})
# x:47 y:213
OperatableStateMachine.add(
'get pos',
CalculationState(calculation=lambda x: x.position),
transitions={'done': 'move head'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'Object',
'output_value': 'Pos'
})
# x:88 y:290
OperatableStateMachine.add('move head',
SaraSetHeadAngle(pitch=0.7, yaw=0),
transitions={'done': 'set dist'},
autonomy={'done': Autonomy.Off})
# x:201 y:156
OperatableStateMachine.add('move arm',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'get pos',
'failed': 'get pos'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'target'})
# x:60 y:106
OperatableStateMachine.add(
'say closer',
SaraSay(sentence="I need to get a bit closer.",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'move arm'},
autonomy={'done': Autonomy.Off})
# x:26 y:541
OperatableStateMachine.add('Group',
_sm_group_0,
transitions={'done': 'wait'},
autonomy={'done': Autonomy.Inherit},
remapping={'pose_out': 'pose_out'})
# x:33 y:625
OperatableStateMachine.add('wait',
WaitState(wait_time=2),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:59 y:308, x:447 y:59, x:384 y:162
_sm_check_form_2 = OperatableStateMachine(
outcomes=['done', 'fail_full', 'full_no_object'],
input_keys=['Action'])
with _sm_check_form_2:
# x:31 y:40
OperatableStateMachine.add(
'check if gripper full',
GetRosParam(ParamName="behavior/Gripper_Content"),
transitions={
'done': 'Say_Full',
'failed': 'cond'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'Value': 'ObjectInGripper'})
# x:30 y:121
OperatableStateMachine.add(
'cond',
CheckConditionState(predicate=lambda x: x[1] == ''),
transitions={
'true': 'not told',
'false': 'done'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'Action'})
# x:222 y:119
OperatableStateMachine.add(
'not told',
SaraSay(sentence="Hum! They didn't told me what to pick",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'full_no_object'},
autonomy={'done': Autonomy.Off})
# x:242 y:31
OperatableStateMachine.add(
'Say_Full',
SaraSay(sentence=lambda x: "Wait. There is already a " + x +
"in my gripper.",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'fail_full'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:84 y:30
OperatableStateMachine.add('Check Form',
_sm_check_form_2,
transitions={
'done': 'get name',
'fail_full': 'cause1',
'full_no_object': 'cause2'
},
autonomy={
'done': Autonomy.Inherit,
'fail_full': Autonomy.Inherit,
'full_no_object': Autonomy.Inherit
},
remapping={'Action': 'Action'})
# x:28 y:452
OperatableStateMachine.add('Action_pick',
self.use_behavior(
Action_pickSM, 'Action_pick'),
transitions={
'success': 'Got_It',
'unreachable': 'for 1',
'not found': 'Say_lost',
'dropped': 'say missed'
},
autonomy={
'success': Autonomy.Inherit,
'unreachable': Autonomy.Inherit,
'not found': Autonomy.Inherit,
'dropped': Autonomy.Inherit
},
remapping={'objectID': 'ID'})
# x:261 y:239
OperatableStateMachine.add('Get closer',
_sm_get_closer_1,
transitions={'done': 'Action_find'},
autonomy={'done': Autonomy.Inherit},
remapping={'Object': 'Object'})
# x:275 y:333
OperatableStateMachine.add('for 1',
ForLoop(repeat=1),
transitions={
'do': 'Get closer',
'end': 'say giveup'
},
autonomy={
'do': Autonomy.Off,
'end': Autonomy.Off
},
remapping={'index': 'index'})
# x:416 y:264
OperatableStateMachine.add('say giveup',
SaraSay(sentence="I give up",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'cause4'},
autonomy={'done': Autonomy.Off})
# x:284 y:496
OperatableStateMachine.add('say missed',
SaraSay(sentence="Oops! I missed.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'cause4'},
autonomy={'done': Autonomy.Off})
# x:469 y:495
OperatableStateMachine.add(
'set param',
SetRosParam(ParamName="behavior/GripperContent"),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'Value': 'ObjectName'})
# x:82 y:115
OperatableStateMachine.add(
'get name',
CalculationState(calculation=lambda x: x[1]),
transitions={'done': 'Action_find'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'Action',
'output_value': 'ObjectName'
})
# x:511 y:20
OperatableStateMachine.add(
'cause1',
SetKey(Value="My gripper was already full."),
transitions={'done': 'setrosparam'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Key'})
# x:512 y:81
OperatableStateMachine.add(
'cause2',
SetKey(Value="I didn't know what to pick."),
transitions={'done': 'setrosparam'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Key'})
# x:511 y:143
OperatableStateMachine.add(
'cause3',
SetKey(Value="I didn't found the object."),
transitions={'done': 'setrosparam'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Key'})
# x:690 y:197
OperatableStateMachine.add(
'setrosparam',
SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off},
remapping={'Value': 'Key'})
# x:605 y:312
OperatableStateMachine.add(
'cause4',
SetKey(Value="I was unable to pick the object."),
transitions={'done': 'setrosparam'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Key'})
# x:30 y:188
OperatableStateMachine.add('Action_find',
self.use_behavior(
Action_findSM, 'Action_find'),
transitions={
'done': 'getID',
'failed': 'cause3'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'className': 'ObjectName',
'entity': 'Object'
})
# x:49 y:322
OperatableStateMachine.add(
'getID',
CalculationState(calculation=lambda x: x.ID),
transitions={'done': 'Action_pick'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'Object',
'output_value': 'ID'
})
# x:284 y:422
OperatableStateMachine.add(
'Say_lost',
SaraSay(sentence=lambda x: "Hum! I lost sight of the " + x,
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'cause4'},
autonomy={'done': Autonomy.Off})
# x:281 y:572
OperatableStateMachine.add(
'Got_It',
SaraSay(sentence=lambda x: "I have the " + x,
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'set param'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:666 y:331, x:502 y:199, x:797 y:103
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Position', 'ID'])
_state_machine.userdata.relative = False
_state_machine.userdata.Position = 0
_state_machine.userdata.ID = 0
_state_machine.userdata.className = "person"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:379 y:221
_sm_wait_to_compte_0 = OperatableStateMachine(outcomes=['finished'])
with _sm_wait_to_compte_0:
# x:77 y:195
OperatableStateMachine.add('one more wait',
WaitState(wait_time=60),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:535 y:314
_sm_turn_around_1 = OperatableStateMachine(outcomes=['failed'])
with _sm_turn_around_1:
# x:47 y:45
OperatableStateMachine.add('set orientation',
SetKey(Value=1.57),
transitions={'done': 'action_turn'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'rotation'})
# x:42 y:213
OperatableStateMachine.add('move head',
SaraSetHeadAngle(pitch=0, yaw=1.57),
transitions={'done': 'waitwait'},
autonomy={'done': Autonomy.Off})
# x:53 y:294
OperatableStateMachine.add('waitwait',
WaitState(wait_time=10),
transitions={'done': 'action_turn_2'},
autonomy={'done': Autonomy.Off})
# x:33 y:122
OperatableStateMachine.add('action_turn',
self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'Try to reach/check person behind/move head and base/turn around/action_turn'),
transitions={'finished': 'move head', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'rotation': 'rotation'})
# x:31 y:434
OperatableStateMachine.add('action_turn_2',
self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'Try to reach/check person behind/move head and base/turn around/action_turn_2'),
transitions={'finished': 'head left right', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'rotation': 'rotation'})
# x:35 y:595
OperatableStateMachine.add('head left right',
SaraSetHeadAngle(pitch=0, yaw=1.57),
transitions={'done': 'wait turn head'},
autonomy={'done': Autonomy.Off})
# x:308 y:537
OperatableStateMachine.add('wait turn head',
WaitState(wait_time=10),
transitions={'done': 'head right left'},
autonomy={'done': Autonomy.Off})
# x:495 y:604
OperatableStateMachine.add('head right left',
SaraSetHeadAngle(pitch=0, yaw=-1.57),
transitions={'done': 'wait wait wait'},
autonomy={'done': Autonomy.Off})
# x:262 y:652
OperatableStateMachine.add('wait wait wait',
WaitState(wait_time=10),
transitions={'done': 'head left right'},
autonomy={'done': Autonomy.Off})
# x:845 y:395
_sm_find_human_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID'])
with _sm_find_human_2:
# x:143 y:40
OperatableStateMachine.add('set name',
SetKey(Value="person"),
transitions={'done': 'list'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'name'})
# x:361 y:53
OperatableStateMachine.add('list',
list_entities_by_name(frontality_level=0.5, distance_max=3),
transitions={'found': 'finished', 'none_found': 'list'},
autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off},
remapping={'name': 'name', 'entity_list': 'entity_list', 'number': 'number'})
# x:549 y:135, x:555 y:269, x:230 y:458
_sm_move_head_and_base_3 = ConcurrencyContainer(outcomes=['failed'], conditions=[
('failed', [('turn around', 'failed')]),
('failed', [('wait to compte', 'finished')])
])
with _sm_move_head_and_base_3:
# x:268 y:77
OperatableStateMachine.add('turn around',
_sm_turn_around_1,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
# x:263 y:246
OperatableStateMachine.add('wait to compte',
_sm_wait_to_compte_0,
transitions={'finished': 'failed'},
autonomy={'finished': Autonomy.Inherit})
# x:493 y:206
_sm_container_4 = OperatableStateMachine(outcomes=['check'])
with _sm_container_4:
# x:230 y:160
OperatableStateMachine.add('wait long',
WaitState(wait_time=40),
transitions={'done': 'check'},
autonomy={'done': Autonomy.Off})
# x:30 y:458, x:706 y:447
_sm_navigate_to_the_point_5 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['pose'])
with _sm_navigate_to_the_point_5:
# x:174 y:122
OperatableStateMachine.add('set relative',
SetKey(Value=False),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'relative'})
# x:347 y:191
OperatableStateMachine.add('Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'Try to reach/Container/navigate to the point/Action_Move'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'pose', 'relative': 'relative'})
# x:728 y:335, x:792 y:103, x:738 y:249, x:724 y:448
_sm_check_person_behind_6 = ConcurrencyContainer(outcomes=['finished', 'failed'], input_keys=['ID'], conditions=[
('failed', [('move head and base', 'failed')]),
('finished', [('find human', 'finished')])
])
with _sm_check_person_behind_6:
# x:250 y:72
OperatableStateMachine.add('move head and base',
_sm_move_head_and_base_3,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
# x:261 y:238
OperatableStateMachine.add('find human',
_sm_find_human_2,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:635 y:61, x:634 y:159, x:597 y:300, x:330 y:458, x:430 y:458, x:530 y:458
_sm_container_7 = ConcurrencyContainer(outcomes=['finished', 'failed', 'check'], input_keys=['waypoint'], conditions=[
('check', [('Container', 'check')]),
('finished', [('navigate to the point', 'finished')]),
('failed', [('navigate to the point', 'failed')])
])
with _sm_container_7:
# x:315 y:51
OperatableStateMachine.add('navigate to the point',
_sm_navigate_to_the_point_5,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'waypoint'})
# x:322 y:257
OperatableStateMachine.add('Container',
_sm_container_4,
transitions={'check': 'check'},
autonomy={'check': Autonomy.Inherit})
# x:30 y:458
_sm_groupwait_8 = OperatableStateMachine(outcomes=['end'])
with _sm_groupwait_8:
# x:30 y:40
OperatableStateMachine.add('waitwait',
WaitState(wait_time=20),
transitions={'done': 'end'},
autonomy={'done': Autonomy.Off})
# x:534 y:319
_sm_move_head_and_base_at_the_end_9 = OperatableStateMachine(outcomes=['failed'])
with _sm_move_head_and_base_at_the_end_9:
# x:52 y:31
OperatableStateMachine.add('setkeyorientation',
SetKey(Value=1.5),
transitions={'done': 'action_turn'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'rotation'})
# x:51 y:114
OperatableStateMachine.add('action_turn',
self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'operator is still there/Move head and base end /move head and base at the end/action_turn'),
transitions={'finished': 'turn right head', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'rotation': 'rotation'})
# x:48 y:272
OperatableStateMachine.add('wait while head turn',
WaitState(wait_time=4),
transitions={'done': 'action_turn_2'},
autonomy={'done': Autonomy.Off})
# x:47 y:199
OperatableStateMachine.add('turn right head',
SaraSetHeadAngle(pitch=0, yaw=1.57),
transitions={'done': 'wait while head turn'},
autonomy={'done': Autonomy.Off})
# x:45 y:348
OperatableStateMachine.add('action_turn_2',
self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'operator is still there/Move head and base end /move head and base at the end/action_turn_2'),
transitions={'finished': 'left to rigth', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'rotation': 'rotation'})
# x:43 y:592
OperatableStateMachine.add('left to rigth',
SaraSetHeadAngle(pitch=0, yaw=-1.57),
transitions={'done': 'waitwait1'},
autonomy={'done': Autonomy.Off})
# x:420 y:602
OperatableStateMachine.add('right to left',
SaraSetHeadAngle(pitch=0, yaw=1.57),
transitions={'done': 'waitwait2'},
autonomy={'done': Autonomy.Off})
# x:266 y:541
OperatableStateMachine.add('waitwait1',
WaitState(wait_time=8),
transitions={'done': 'right to left'},
autonomy={'done': Autonomy.Off})
# x:236 y:674
OperatableStateMachine.add('waitwait2',
WaitState(wait_time=8),
transitions={'done': 'left to rigth'},
autonomy={'done': Autonomy.Off})
# x:231 y:538
_sm_find_a_human_10 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID'])
with _sm_find_a_human_10:
# x:168 y:102
OperatableStateMachine.add('set name',
SetKey(Value="person"),
transitions={'done': 'list'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'name'})
# x:275 y:203
OperatableStateMachine.add('list',
list_entities_by_name(frontality_level=0.5, distance_max=3),
transitions={'found': 'finished', 'none_found': 'list'},
autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off},
remapping={'name': 'name', 'entity_list': 'entity_list', 'number': 'number'})
# x:415 y:99, x:318 y:246, x:442 y:295
_sm_move_head_and_base_end__11 = ConcurrencyContainer(outcomes=['failed'], conditions=[
('failed', [('move head and base at the end', 'failed')]),
('failed', [('Groupwait', 'end')])
])
with _sm_move_head_and_base_end__11:
# x:132 y:57
OperatableStateMachine.add('move head and base at the end',
_sm_move_head_and_base_at_the_end_9,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
# x:121 y:218
OperatableStateMachine.add('Groupwait',
_sm_groupwait_8,
transitions={'end': 'failed'},
autonomy={'end': Autonomy.Inherit})
# x:323 y:632, x:638 y:631
_sm_try_to_reach_12 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['waypoint', 'relative', 'ID'])
with _sm_try_to_reach_12:
# x:186 y:130
OperatableStateMachine.add('Container',
_sm_container_7,
transitions={'finished': 'Say_Reached', 'failed': 'say_not_reached', 'check': 'say check'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'check': Autonomy.Inherit},
remapping={'waypoint': 'waypoint'})
# x:581 y:163
OperatableStateMachine.add('check person behind',
_sm_check_person_behind_6,
transitions={'finished': 'say found', 'failed': 'say lost'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:706 y:484
OperatableStateMachine.add('say lost',
SaraSay(sentence="Oh no! I lost my operator!", input_keys=[], emotion=1, block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:445 y:69
OperatableStateMachine.add('say check',
SaraSay(sentence="I check if my operator is still there", input_keys=[], emotion=1, block=True),
transitions={'done': 'check person behind'},
autonomy={'done': Autonomy.Off})
# x:443 y:187
OperatableStateMachine.add('say found',
SaraSay(sentence="Great. You are still there.", input_keys=[], emotion=1, block=True),
transitions={'done': 'Container'},
autonomy={'done': Autonomy.Off})
# x:520 y:409
OperatableStateMachine.add('say_not_reached',
SaraSay(sentence=lambda x: "I have not reach the destination", input_keys=[], emotion=0, block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:139 y:339
OperatableStateMachine.add('Say_Reached',
SaraSay(sentence=lambda x: "I have reach the destination", input_keys=[], emotion=0, block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:626 y:228, x:607 y:71, x:230 y:458, x:330 y:458
_sm_operator_is_still_there_13 = ConcurrencyContainer(outcomes=['finished', 'failed'], input_keys=['ID'], conditions=[
('finished', [('find a human', 'finished')]),
('failed', [('Move head and base end ', 'failed')])
])
with _sm_operator_is_still_there_13:
# x:207 y:54
OperatableStateMachine.add('Move head and base end ',
_sm_move_head_and_base_end__11,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit})
# x:222 y:190
OperatableStateMachine.add('find a human',
_sm_find_a_human_10,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit},
remapping={'ID': 'ID'})
with _state_machine:
# x:52 y:98
OperatableStateMachine.add('GetPerson',
GetEntityByID(),
transitions={'found': 'sayfollowme', 'not_found': 'Action_findPerson'},
autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ID': 'ID', 'Entity': 'Entity'})
# x:29 y:416
OperatableStateMachine.add('operator is still there',
_sm_operator_is_still_there_13,
transitions={'finished': 'head to middle', 'failed': 'say lost operator'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:290 y:324
OperatableStateMachine.add('say lost operator',
SaraSay(sentence="I have reach my goal but I lost the person I was guiding.", input_keys=[], emotion=1, block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:43 y:214
OperatableStateMachine.add('sayfollowme',
SaraSay(sentence="Follow me please.", input_keys=[], emotion=1, block=True),
transitions={'done': 'Try to reach'},
autonomy={'done': Autonomy.Off})
# x:413 y:441
OperatableStateMachine.add('head to middle',
SaraSetHeadAngle(pitch=0, yaw=0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:39 y:330
OperatableStateMachine.add('Try to reach',
_sm_try_to_reach_12,
transitions={'finished': 'operator is still there', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'waypoint': 'Position', 'relative': 'relative', 'ID': 'ID'})
# x:443 y:95
OperatableStateMachine.add('Cant Find Person',
SaraSay(sentence="I can't find a person.", input_keys=[], emotion=1, block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:201 y:57
OperatableStateMachine.add('Action_findPerson',
self.use_behavior(sara_flexbe_behaviors__Action_findPersonSM, 'Action_findPerson'),
transitions={'done': 'sayfollowme', 'pas_done': 'Cant Find Person'},
autonomy={'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit},
remapping={'className': 'className', 'entity': 'Entity'})
return _state_machine
def create(self):
# x: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):
joy_topic = '/hmi/joystick'
# x:30 y:365, x:846 y:376
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.unused = None
_state_machine.userdata.joy_msg = Joy()
_state_machine.userdata.text_msg = TextCommand()
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:353
_sm_waituntiltextcmd_0 = OperatableStateMachine(
outcomes=['received'], output_keys=['text_msg'])
with _sm_waituntiltextcmd_0:
# x:87 y:101
OperatableStateMachine.add(
'WaitTextMsg',
WaitForMessageState(
topic='/control',
condition=lambda x: x.type == 'flexbe/action',
buffered=False,
clear=True),
transitions={
'received': 'received',
'unavailable': 'Wait1s'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'text_msg'})
# x:314 y:97
OperatableStateMachine.add('Wait1s',
WaitState(wait_time=1),
transitions={'done': 'WaitTextMsg'},
autonomy={'done': Autonomy.Off})
# x:397 y:287, x:85 y:291, x:504 y:228, x:330 y:297, x:459 y:297, x:530 y:297
_sm_joystickmovements_1 = ConcurrencyContainer(
outcomes=['failed', 'button1234', 'timeout'],
input_keys=['joy_msg'],
output_keys=['joy_msg'],
conditions=[('timeout', [('JoystickControl', 'done')]),
('failed', [('WaitButton1234Pressed', 'unavailable')]),
('button1234', [('WaitButton1234Pressed', 'received')])
])
with _sm_joystickmovements_1:
# x:385 y:129
OperatableStateMachine.add(
'JoystickControl',
JoystickJointControl(
joints_topic='/joint_states',
goal_joints_topic=
'/motion/controller/joint_state/out_joints_src_reset',
joy_topic=joy_topic,
buttons=[(4, 0.6, 'ear_l_joint', -3.0, 1.0),
(6, -0.6, 'ear_l_joint', -3.0, 1.0),
(5, 0.6, 'ear_r_joint', -3.0, 1.0),
(7, -0.6, 'ear_r_joint', -3.0, 1.0)],
axes=[(0, 0, 'eyes_yaw', -1.5, 1.5),
(1, 0, 'eyes_pitch', -1.5, 1.5),
(4, -0.4, 'head_joint4', -1.5, 1.5),
(5, 0.4, 'head_joint2', -1.5, 1.5),
(3, 0.5, 'mouth_joint', -0.6, 0)],
timeout=5.0),
transitions={'done': 'timeout'},
autonomy={'done': Autonomy.Off})
# x:130 y:136
OperatableStateMachine.add(
'WaitButton1234Pressed',
WaitForMessageState(
topic=joy_topic,
condition=lambda msg: any(msg.buttons[0:4]),
buffered=False,
clear=True),
transitions={
'received': 'button1234',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'joy_msg'})
# x:205 y:274, x:300 y:274, x:68 y:275, x:449 y:266, x:595 y:272, x:833 y:323, x:638 y:392, x:730 y:353, x:949 y:274
_sm_randmovements_2 = ConcurrencyContainer(
outcomes=['failed', 'timeout', 'joy_msg', 'text_msg'],
input_keys=['unused', 'joy_msg', 'text_msg'],
output_keys=['joy_msg', 'text_msg', 'unused'],
conditions=[('failed', [('WaitJoystick', 'unavailable')]),
('failed', [('RandHeadMoves', 'failed')]),
('joy_msg', [('WaitJoystick', 'received')]),
('timeout', [('RandHeadMoves', 'done')]),
('text_msg', [('WaitUntilTextCmd', 'received')])])
with _sm_randmovements_2:
# x:88 y:74
OperatableStateMachine.add(
'WaitJoystick',
WaitForMessageState(
topic=joy_topic,
condition=lambda msg: any(msg.buttons) or any(msg.axes),
buffered=False,
clear=True),
transitions={
'received': 'joy_msg',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'joy_msg'})
# x:332 y:60
OperatableStateMachine.add(
'RandHeadMoves',
RandJointsMovements(controller='joint_state_head',
duration=10,
interval=[2.0, 5.0],
joints=['head_joint2', 'head_joint4'],
minimal=[-0.2, -0.3],
maximal=[0.3, 0.3]),
transitions={
'done': 'timeout',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'unused'})
# x:647 y:50
OperatableStateMachine.add('WaitUntilTextCmd',
_sm_waituntiltextcmd_0,
transitions={'received': 'text_msg'},
autonomy={'received': Autonomy.Inherit},
remapping={'text_msg': 'text_msg'})
with _state_machine:
# x:12 y:169
OperatableStateMachine.add(
'SetNominalPose',
SetJointState(controller='motion/controller/joint_state_head',
pose_param='nominal',
pose_ns='saved_msgs/joint_state',
tolerance=0.017,
timeout=10.0,
joint_topic="joint_states"),
transitions={
'done': 'RandMovements',
'failed': 'failed',
'timeout': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off,
'timeout': Autonomy.Off
})
# x:127 y:370
OperatableStateMachine.add('JoystickMovements',
_sm_joystickmovements_1,
transitions={
'failed': 'failed',
'button1234': 'ProcessButton',
'timeout': 'RandMovements'
},
autonomy={
'failed': Autonomy.Inherit,
'button1234': Autonomy.Inherit,
'timeout': Autonomy.Inherit
},
remapping={'joy_msg': 'joy_msg'})
# x:248 y:186
OperatableStateMachine.add(
'CheckButtonPressed',
DecisionState(outcomes=['button1234', 'other'],
conditions=lambda msg: 'button1234'
if any(msg.buttons[0:4]) else 'other'),
transitions={
'button1234': 'ProcessButton',
'other': 'JoystickMovements'
},
autonomy={
'button1234': Autonomy.Off,
'other': Autonomy.Off
},
remapping={'input_value': 'joy_msg'})
# x:690 y:123
OperatableStateMachine.add(
'SetHeadNominalPose',
SetJointState(controller='motion/controller/joint_state_head',
pose_param='head_nominal',
pose_ns='saved_msgs/joint_state',
tolerance=0.017,
timeout=10.0,
joint_topic="joint_states"),
transitions={
'done': 'ExecuteAction',
'failed': 'failed',
'timeout': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off,
'timeout': Autonomy.Off
})
# x:425 y:181
OperatableStateMachine.add(
'ProcessButton',
CalculationState(calculation=self.process_joy_msg),
transitions={'done': 'SetHeadNominalPose'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'joy_msg',
'output_value': 'action_name'
})
# x:272 y:100
OperatableStateMachine.add(
'ProcessTextMsg',
CalculationState(calculation=self.process_text_msg),
transitions={'done': 'SetHeadNominalPose'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'text_msg',
'output_value': 'action_name'
})
# x:457 y:12
OperatableStateMachine.add(
'RandomAction',
CalculationState(calculation=self.random_action),
transitions={'done': 'SetHeadNominalPose'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'unused',
'output_value': 'action_name'
})
# x:383 y:325
OperatableStateMachine.add(
'ExecuteAction',
ExecuteJointTrajectoryKey(
controller='motion/controller/joint_trajectory',
trajectory_ns='saved_msgs/joint_trajectory'),
transitions={
'success': 'JoystickMovements',
'unavalible': 'JoystickMovements',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'unavalible': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'trajectory_param': 'action_name'})
# x:308 y:7
OperatableStateMachine.add(
'RandomChoice',
DecisionState(outcomes=['move', 'continue'],
conditions=lambda x: 'continue'
if random.random() < 0.4 else 'move'),
transitions={
'move': 'RandomAction',
'continue': 'RandMovements'
},
autonomy={
'move': Autonomy.Off,
'continue': Autonomy.Off
},
remapping={'input_value': 'unused'})
# x:117 y:39
OperatableStateMachine.add('RandMovements',
_sm_randmovements_2,
transitions={
'failed': 'failed',
'timeout': 'RandomChoice',
'joy_msg': 'CheckButtonPressed',
'text_msg': 'ProcessTextMsg'
},
autonomy={
'failed': Autonomy.Inherit,
'timeout': Autonomy.Inherit,
'joy_msg': Autonomy.Inherit,
'text_msg': Autonomy.Inherit
},
remapping={
'unused': 'unused',
'joy_msg': 'joy_msg',
'text_msg': 'text_msg'
})
return _state_machine
def create(self):
# x:712 y:612, x:820 y:97
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.name = "person"
_state_machine.userdata.distance = 1.05
_state_machine.userdata.taxi = "taxi"
_state_machine.userdata.umbrella = "umbrella"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:130 y:365, x:230 y:365
_sm_get_closer_0 = ConcurrencyContainer(
outcomes=['finished'],
input_keys=['ID'],
conditions=[('finished', [('follow', 'failed')]),
('finished', [('wait 3', 'done')])])
with _sm_get_closer_0:
# x:46 y:131
OperatableStateMachine.add('follow',
SaraFollow(distance=1.15,
ReplanPeriod=0.5),
transitions={'failed': 'finished'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:223 y:134
OperatableStateMachine.add('wait 3',
WaitState(wait_time=4),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:571 y:592
_sm_scan_1 = OperatableStateMachine(outcomes=['finished'])
with _sm_scan_1:
# x:42 y:66
OperatableStateMachine.add(
'Looking',
SaraSay(
sentence=
"I am trying to find who wants to leave. Please raise your hand.",
input_keys=[],
emotion=0,
block=False),
transitions={'done': 'center'},
autonomy={'done': Autonomy.Off})
# x:394 y:262
OperatableStateMachine.add('right',
SaraSetHeadAngle(pitch=0.1, yaw=-0.3),
transitions={'done': 'w3'},
autonomy={'done': Autonomy.Off})
# x:206 y:372
OperatableStateMachine.add('center2',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w4'},
autonomy={'done': Autonomy.Off})
# x:34 y:589
OperatableStateMachine.add('w1',
WaitState(wait_time=3),
transitions={'done': 'center3'},
autonomy={'done': Autonomy.Off})
# x:413 y:159
OperatableStateMachine.add('w2',
WaitState(wait_time=3),
transitions={'done': 'right'},
autonomy={'done': Autonomy.Off})
# x:415 y:370
OperatableStateMachine.add('w3',
WaitState(wait_time=3),
transitions={'done': 'center2'},
autonomy={'done': Autonomy.Off})
# x:39 y:371
OperatableStateMachine.add('w4',
WaitState(wait_time=3),
transitions={'done': 'left'},
autonomy={'done': Autonomy.Off})
# x:16 y:475
OperatableStateMachine.add('left',
SaraSetHeadAngle(pitch=0.1, yaw=0.3),
transitions={'done': 'w1'},
autonomy={'done': Autonomy.Off})
# x:215 y:156
OperatableStateMachine.add('center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w2'},
autonomy={'done': Autonomy.Off})
# x:213 y:579
OperatableStateMachine.add('center3',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'wait'},
autonomy={'done': Autonomy.Off})
# x:393 y:576
OperatableStateMachine.add('wait',
WaitState(wait_time=4),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:32 y:494, x:627 y:411
_sm_filtregender_2 = OperatableStateMachine(
outcomes=['none_found', 'found person'],
input_keys=['name'],
output_keys=['pronoun', 'person'])
with _sm_filtregender_2:
# x:109 y:51
OperatableStateMachine.add('List',
list_entities_by_name(
frontality_level=0.5,
distance_max=10),
transitions={
'found': 'FiltreWave',
'none_found': 'none_found'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'name',
'entity_list': 'entity_list',
'number': 'number'
})
# x:180 y:301
OperatableStateMachine.add(
'FiltreExitingwomen',
Filter(filter=lambda x: x.face.gender == "female"),
transitions={'done': 'no female?'},
autonomy={'done': Autonomy.Off},
remapping={
'input_list': 'persons',
'output_list': 'female'
})
# x:174 y:386
OperatableStateMachine.add(
'no female?',
CheckConditionState(predicate=lambda x: len(x) > 0),
transitions={
'true': 'set pronoun female',
'false': 'set pronoun male'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'female'})
# x:374 y:312
OperatableStateMachine.add(
'set pronoun female',
SetKey(Value="miss"),
transitions={'done': 'get first female'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'pronoun'})
# x:181 y:476
OperatableStateMachine.add('set pronoun male',
SetKey(Value=""),
transitions={'done': 'get first male'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'pronoun'})
# x:599 y:290
OperatableStateMachine.add(
'get first female',
CalculationState(calculation=lambda x: x[0]),
transitions={'done': 'found person'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'female',
'output_value': 'person'
})
# x:381 y:408
OperatableStateMachine.add(
'get first male',
CalculationState(calculation=lambda x: x[0]),
transitions={'done': 'found person'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'persons',
'output_value': 'person'
})
# x:151 y:135
OperatableStateMachine.add(
'FiltreWave',
Filter(filter=lambda x: x.pose.right_arm_up or x.pose.
left_arm_up),
transitions={'done': 'if more than 0'},
autonomy={'done': Autonomy.Off},
remapping={
'input_list': 'entity_list',
'output_list': 'persons'
})
# x:157 y:217
OperatableStateMachine.add(
'if more than 0',
CheckConditionState(predicate=lambda x: len(x) > 0),
transitions={
'true': 'FiltreExitingwomen',
'false': 'none_found'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'persons'})
# x:772 y:208, x:360 y:516
_sm_confirm_3 = OperatableStateMachine(
outcomes=['false', 'done'], input_keys=['Person', 'pronoun'])
with _sm_confirm_3:
# x:66 y:83
OperatableStateMachine.add(
'Confirm',
SaraSay(sentence=lambda x: "would you like to leave, " + x[0] +
"?",
input_keys=["pronoun"],
emotion=0,
block=True),
transitions={'done': 'GetSpeech'},
autonomy={'done': Autonomy.Off},
remapping={'pronoun': 'pronoun'})
# x:82 y:504
OperatableStateMachine.add('GetID',
SetRosParam(ParamName="OpeID"),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={'Value': 'ID'})
# x:67 y:298
OperatableStateMachine.add('if yes',
RegexTester(regex=".*((yes)|(I do)).*"),
transitions={
'true': 'get id',
'false': 'test no'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'text': 'text',
'result': 'result'
})
# x:84 y:409
OperatableStateMachine.add('get id',
GetAttribute(attributes=["ID"]),
transitions={'done': 'GetID'},
autonomy={'done': Autonomy.Off},
remapping={
'object': 'Person',
'ID': 'ID'
})
# x:87 y:188
OperatableStateMachine.add('GetSpeech',
GetSpeech(watchdog=10),
transitions={
'done': 'if yes',
'nothing': 'sayno',
'fail': 'if yes'
},
autonomy={
'done': Autonomy.Off,
'nothing': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'words': 'text'})
# x:259 y:193
OperatableStateMachine.add(
'sayno',
SaraSay(
sentence="Sorry, did you say that you wanted to leave?",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'getno'},
autonomy={'done': Autonomy.Off})
# x:439 y:179
OperatableStateMachine.add('getno',
GetSpeech(watchdog=10),
transitions={
'done': 'getNO',
'nothing': 'false',
'fail': 'false'
},
autonomy={
'done': Autonomy.Off,
'nothing': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'words': 'text'})
# x:603 y:337
OperatableStateMachine.add('getNO',
RegexTester(regex=".*((yes)|(I do)).*"),
transitions={
'true': 'get id',
'false': 'false'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'text': 'text',
'result': 'result'
})
# x:246 y:298
OperatableStateMachine.add('test no',
RegexTester(regex=".*((no)|(not)).*"),
transitions={
'true': 'false',
'false': 'sayno'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'text': 'text',
'result': 'result'
})
# x:228 y:419
_sm_find_raising_arm_4 = OperatableStateMachine(
outcomes=['finished'], output_keys=['umbrella'])
with _sm_find_raising_arm_4:
# x:33 y:48
OperatableStateMachine.add('setperson',
SetKey(Value="person"),
transitions={'done': 'gettaxiperson'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'person'})
# x:183 y:120
OperatableStateMachine.add('gettaxiperson',
list_entities_by_name(
frontality_level=0.5,
distance_max=10),
transitions={
'found': 'taxi',
'none_found': 'gettaxiperson'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'person',
'entity_list': 'entity_list',
'number': 'number'
})
# x:408 y:270
OperatableStateMachine.add(
'if',
CheckConditionState(predicate=lambda x: len(x) > 0),
transitions={
'true': 'get first',
'false': 'gettaxiperson'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'output_list'})
# x:344 y:184
OperatableStateMachine.add(
'taxi',
Filter(filter=lambda x: x.pose.right_arm_up or x.pose.
left_arm_up),
transitions={'done': 'if'},
autonomy={'done': Autonomy.Off},
remapping={
'input_list': 'entity_list',
'output_list': 'output_list'
})
# x:405 y:359
OperatableStateMachine.add(
'get first',
CalculationState(calculation=lambda x: x[0]),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'output_list',
'output_value': 'umbrella'
})
# x:30 y:365
_sm_rotation_5 = OperatableStateMachine(outcomes=['end'])
with _sm_rotation_5:
# x:51 y:38
OperatableStateMachine.add('Set 180 degres',
SetKey(Value=3.1416),
transitions={'done': 'Look Center'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'rotation'})
# x:613 y:470
OperatableStateMachine.add(
'action_turn',
self.use_behavior(
sara_flexbe_behaviors__action_turnSM,
'GetTaxi/Find umbrella/gettaxihuman/Rotation/action_turn'),
transitions={
'finished': 'Look Right',
'failed': 'Look Right'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'rotation': 'rotation'})
# x:421 y:54
OperatableStateMachine.add('Look Right',
SaraSetHeadAngle(pitch=0, yaw=-1.5),
transitions={'done': 'w2'},
autonomy={'done': Autonomy.Off})
# x:265 y:56
OperatableStateMachine.add('w1',
WaitState(wait_time=4),
transitions={'done': 'Look Right'},
autonomy={'done': Autonomy.Off})
# x:630 y:56
OperatableStateMachine.add('w2',
WaitState(wait_time=4),
transitions={'done': 'center'},
autonomy={'done': Autonomy.Off})
# x:250 y:177
OperatableStateMachine.add('Look Center',
SaraSetHeadAngle(pitch=0, yaw=0),
transitions={'done': 'w1'},
autonomy={'done': Autonomy.Off})
# x:618 y:304
OperatableStateMachine.add('Look Left 2',
SaraSetHeadAngle(pitch=0, yaw=1.5),
transitions={'done': 'w4'},
autonomy={'done': Autonomy.Off})
# x:612 y:138
OperatableStateMachine.add('center',
SaraSetHeadAngle(pitch=0, yaw=0),
transitions={'done': 'w3'},
autonomy={'done': Autonomy.Off})
# x:635 y:214
OperatableStateMachine.add('w3',
WaitState(wait_time=4),
transitions={'done': 'Look Left 2'},
autonomy={'done': Autonomy.Off})
# x:636 y:394
OperatableStateMachine.add('w4',
WaitState(wait_time=4),
transitions={'done': 'action_turn'},
autonomy={'done': Autonomy.Off})
# x:30 y:365
_sm_find_entity_6 = OperatableStateMachine(outcomes=['found'],
input_keys=['className'],
output_keys=['entity'])
with _sm_find_entity_6:
# x:181 y:178
OperatableStateMachine.add('find_entity',
list_entities_by_name(
frontality_level=0.5,
distance_max=4),
transitions={
'found': 'Get Entity',
'none_found': 'find_entity'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'className',
'entity_list': 'entity_list',
'number': 'number'
})
# x:454 y:178
OperatableStateMachine.add(
'Get Entity',
CalculationState(calculation=lambda x: x[0]),
transitions={'done': 'found'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'entity_list',
'output_value': 'entity'
})
# x:194 y:40
OperatableStateMachine.add('WaitState',
WaitState(wait_time=1),
transitions={'done': 'find_entity'},
autonomy={'done': Autonomy.Off})
# x:34 y:496, x:130 y:365, x:475 y:291, x:330 y:365, x:430 y:365
_sm_gettaxihuman_7 = ConcurrencyContainer(
outcomes=['found', 'not_found'],
input_keys=['umbrella'],
output_keys=['umbrella'],
conditions=[('not_found', [('Rotation', 'end')]),
('found', [('Find Entity', 'found')]),
('found', [('FInd raising arm', 'finished')])])
with _sm_gettaxihuman_7:
# x:127 y:67
OperatableStateMachine.add('Find Entity',
_sm_find_entity_6,
transitions={'found': 'found'},
autonomy={'found': Autonomy.Inherit},
remapping={
'className': 'umbrella',
'entity': 'umbrella'
})
# x:129 y:180
OperatableStateMachine.add('Rotation',
_sm_rotation_5,
transitions={'end': 'not_found'},
autonomy={'end': Autonomy.Inherit})
# x:377 y:127
OperatableStateMachine.add('FInd raising arm',
_sm_find_raising_arm_4,
transitions={'finished': 'found'},
autonomy={'finished': Autonomy.Inherit},
remapping={'umbrella': 'umbrella'})
# x:73 y:441, x:586 y:51
_sm_find_umbrella_8 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['umbrella'],
output_keys=['umbrella'])
with _sm_find_umbrella_8:
# x:67 y:42
OperatableStateMachine.add('Look Center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'gettaxihuman'},
autonomy={'done': Autonomy.Off})
# x:278 y:138
OperatableStateMachine.add('Look Center Not Found',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:58 y:326
OperatableStateMachine.add('Log Entity',
LogKeyState(
text="Found entity: {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'umbrella'})
# x:63 y:126
OperatableStateMachine.add('gettaxihuman',
_sm_gettaxihuman_7,
transitions={
'found': 'WaitState',
'not_found': 'Look Center Not Found'
},
autonomy={
'found': Autonomy.Inherit,
'not_found': Autonomy.Inherit
},
remapping={'umbrella': 'umbrella'})
# x:67 y:222
OperatableStateMachine.add('WaitState',
WaitState(wait_time=1),
transitions={'done': 'Log Entity'},
autonomy={'done': Autonomy.Off})
# x:83 y:284
_sm_lift_head_9 = OperatableStateMachine(outcomes=['finished'])
with _sm_lift_head_9:
# x:53 y:42
OperatableStateMachine.add('lift head',
SaraSetHeadAngle(pitch=0, yaw=0),
transitions={'done': 'wait 2'},
autonomy={'done': Autonomy.Off})
# x:57 y:152
OperatableStateMachine.add('wait 2',
WaitState(wait_time=3),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:30 y:365
_sm_get_closer_10 = OperatableStateMachine(outcomes=['finished'],
input_keys=['person'])
with _sm_get_closer_10:
# x:151 y:63
OperatableStateMachine.add('get ID',
GetAttribute(attributes=["ID"]),
transitions={'done': 'Get closer'},
autonomy={'done': Autonomy.Off},
remapping={
'object': 'person',
'ID': 'ID'
})
# x:135 y:194
OperatableStateMachine.add('Get closer',
_sm_get_closer_0,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:607 y:98, x:558 y:330
_sm_get_gender_11 = OperatableStateMachine(
outcomes=['none_found', 'done'],
input_keys=['name', 'distance'],
output_keys=['person', 'pronoun'])
with _sm_get_gender_11:
# x:30 y:40
OperatableStateMachine.add('set head',
SaraSetHeadAngle(pitch=0, yaw=0),
transitions={'done': 'Scan'},
autonomy={'done': Autonomy.Off})
# x:441 y:109
OperatableStateMachine.add('FiltreGender',
_sm_filtregender_2,
transitions={
'none_found': 'none_found',
'found person': 'done'
},
autonomy={
'none_found': Autonomy.Inherit,
'found person': Autonomy.Inherit
},
remapping={
'name': 'name',
'pronoun': 'pronoun',
'person': 'person'
})
# x:211 y:93
OperatableStateMachine.add(
'Scan',
_sm_scan_1,
transitions={'finished': 'FiltreGender'},
autonomy={'finished': Autonomy.Inherit})
# x:30 y:373, x:130 y:373
_sm_nevermind_12 = OperatableStateMachine(outcomes=['done', 'failed'],
input_keys=['poseOrigin'])
with _sm_nevermind_12:
# x:30 y:40
OperatableStateMachine.add('say ok',
SaraSay(sentence="I will try again",
input_keys=[],
emotion=0,
block=False),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:26 y:192
OperatableStateMachine.add(
'Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM,
'nevermind/Action_Move'),
transitions={
'finished': 'done',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pose': 'poseOrigin'})
# x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458
_sm_confirm_13 = ConcurrencyContainer(
outcomes=['false', 'done'],
input_keys=['person', 'pronoun'],
conditions=[('false', [('Confirm', 'false')]),
('done', [('Confirm', 'done')]),
('false', [('LookAtClosest', 'failed')])])
with _sm_confirm_13:
# x:95 y:163
OperatableStateMachine.add('Confirm',
_sm_confirm_3,
transitions={
'false': 'false',
'done': 'done'
},
autonomy={
'false': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'Person': 'person',
'pronoun': 'pronoun'
})
# x:309 y:162
OperatableStateMachine.add(
'LookAtClosest',
self.use_behavior(sara_flexbe_behaviors__LookAtClosestSM,
'confirm/LookAtClosest'),
transitions={'failed': 'false'},
autonomy={'failed': Autonomy.Inherit})
# x:65 y:581, x:688 y:449
_sm_gettaxi_14 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['distance', 'taxi', 'umbrella'])
with _sm_gettaxi_14:
# x:103 y:28
OperatableStateMachine.add(
'Action_Move_to taxi',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM,
'GetTaxi/Action_Move_to taxi'),
transitions={
'finished': 'Lift head',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pose': 'taxi'})
# x:220 y:546
OperatableStateMachine.add(
'Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM,
'GetTaxi/Action_Move'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pose': 'pose_out'})
# x:80 y:475
OperatableStateMachine.add('NotTooClose',
Get_Reacheable_Waypoint(),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off},
remapping={
'pose_in': 'position',
'distance': 'distance',
'pose_out': 'pose_out'
})
# x:81 y:400
OperatableStateMachine.add('GEtUmbrellaPosition',
GetAttribute(attributes=["position"]),
transitions={'done': 'NotTooClose'},
autonomy={'done': Autonomy.Off},
remapping={
'object': 'umbrella',
'position': 'position'
})
# x:108 y:131
OperatableStateMachine.add(
'Lift head',
_sm_lift_head_9,
transitions={'finished': 'Find umbrella'},
autonomy={'finished': Autonomy.Inherit})
# x:150 y:315
OperatableStateMachine.add(
'log',
LogKeyState(text="found umbrella: {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'GEtUmbrellaPosition'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'umbrella'})
# x:267 y:312
OperatableStateMachine.add(
'say see taxy',
SaraSay(
sentence=
"I see the taxi. Over there. The person with the umbrella.",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'log'},
autonomy={'done': Autonomy.Off})
# x:111 y:204
OperatableStateMachine.add('Find umbrella',
_sm_find_umbrella_8,
transitions={
'finished': 'say see taxy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'umbrella': 'umbrella'})
with _state_machine:
# x:46 y:16
OperatableStateMachine.add('GetOrigin',
Get_Robot_Pose(),
transitions={'done': 'Init_Sequence'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'poseOrigin'})
# x:63 y:574
OperatableStateMachine.add('GetTaxi',
_sm_gettaxi_14,
transitions={
'finished': 'say succeed',
'failed': 'say fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'distance': 'distance',
'taxi': 'taxi',
'umbrella': 'umbrella'
})
# x:103 y:369
OperatableStateMachine.add('confirm',
_sm_confirm_13,
transitions={
'false': 'nevermind',
'done': 'say taxi'
},
autonomy={
'false': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'person': 'person',
'pronoun': 'pronoun'
})
# x:289 y:142
OperatableStateMachine.add(
'say nobody',
SaraSay(sentence="I don't see any raised hand.",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'Get Gender'},
autonomy={'done': Autonomy.Off})
# x:571 y:104
OperatableStateMachine.add(
'say fail',
SaraSay(sentence="I failed this scenario. Sorry.",
input_keys=[],
emotion=3,
block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:245 y:595
OperatableStateMachine.add('say succeed',
SaraSay(
sentence="here is the taxy driver.",
input_keys=[],
emotion=5,
block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:103 y:483
OperatableStateMachine.add(
'say taxi',
SaraSay(sentence="Ok, follow me to the taxi then.",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'GetTaxi'},
autonomy={'done': Autonomy.Off})
# x:49 y:101
OperatableStateMachine.add(
'Init_Sequence',
self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM,
'Init_Sequence'),
transitions={
'finished': 'Get Gender',
'failed': 'say fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:17 y:288
OperatableStateMachine.add('nevermind',
_sm_nevermind_12,
transitions={
'done': 'Get Gender',
'failed': 'say fail'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'poseOrigin': 'poseOrigin'})
# x:30 y:192
OperatableStateMachine.add('Get Gender',
_sm_get_gender_11,
transitions={
'none_found': 'say nobody',
'done': 'get closer'
},
autonomy={
'none_found': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'name': 'name',
'distance': 'distance',
'person': 'person',
'pronoun': 'pronoun'
})
# x:259 y:310
OperatableStateMachine.add('get closer',
_sm_get_closer_10,
transitions={'finished': 'confirm'},
autonomy={'finished': Autonomy.Inherit},
remapping={'person': 'person'})
return _state_machine
def create(self):
# x:72 y:443, x:514 y:143
_state_machine = OperatableStateMachine(outcomes=['done', 'pas_done'], input_keys=['className'], output_keys=['entity'])
_state_machine.userdata.className = "person"
_state_machine.userdata.entity = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:706 y:647
_sm_rotation_0 = OperatableStateMachine(outcomes=['end'])
with _sm_rotation_0:
# x:51 y:38
OperatableStateMachine.add('Set 180 degres',
SetKey(Value=3.1416),
transitions={'done': 'Look Center'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'rotation'})
# x:613 y:470
OperatableStateMachine.add('action_turn',
self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'Container/Rotation/action_turn'),
transitions={'finished': 'Look Right', 'failed': 'Look Right'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'rotation': 'rotation'})
# x:421 y:54
OperatableStateMachine.add('Look Right',
SaraSetHeadAngle(pitch=0.1, yaw=-1.5),
transitions={'done': 'w2'},
autonomy={'done': Autonomy.Off})
# x:265 y:56
OperatableStateMachine.add('w1',
WaitState(wait_time=4),
transitions={'done': 'Look Right'},
autonomy={'done': Autonomy.Off})
# x:630 y:56
OperatableStateMachine.add('w2',
WaitState(wait_time=4),
transitions={'done': 'center'},
autonomy={'done': Autonomy.Off})
# x:250 y:177
OperatableStateMachine.add('Look Center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w1'},
autonomy={'done': Autonomy.Off})
# x:618 y:304
OperatableStateMachine.add('Look Left 2',
SaraSetHeadAngle(pitch=0.1, yaw=1.5),
transitions={'done': 'w4'},
autonomy={'done': Autonomy.Off})
# x:612 y:138
OperatableStateMachine.add('center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w3'},
autonomy={'done': Autonomy.Off})
# x:635 y:214
OperatableStateMachine.add('w3',
WaitState(wait_time=4),
transitions={'done': 'Look Left 2'},
autonomy={'done': Autonomy.Off})
# x:636 y:394
OperatableStateMachine.add('w4',
WaitState(wait_time=4),
transitions={'done': 'action_turn'},
autonomy={'done': Autonomy.Off})
# x:683 y:188
_sm_find_entity_1 = OperatableStateMachine(outcomes=['found'], input_keys=['className'], output_keys=['entity'])
with _sm_find_entity_1:
# x:181 y:178
OperatableStateMachine.add('find_entity',
list_entities_by_name(frontality_level=0.5, distance_max=4),
transitions={'found': 'Get Entity', 'none_found': 'find_entity'},
autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off},
remapping={'name': 'className', 'entity_list': 'entity_list', 'number': 'number'})
# x:454 y:178
OperatableStateMachine.add('Get Entity',
CalculationState(calculation=lambda x: x[0]),
transitions={'done': 'found'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'entity_list', 'output_value': 'entity'})
# x:194 y:40
OperatableStateMachine.add('WaitState',
WaitState(wait_time=1),
transitions={'done': 'find_entity'},
autonomy={'done': Autonomy.Off})
# x:372 y:27, x:370 y:220, x:368 y:100, x:330 y:458
_sm_container_2 = ConcurrencyContainer(outcomes=['found', 'not_found'], input_keys=['className'], output_keys=['entity'], conditions=[
('not_found', [('Rotation', 'end')]),
('found', [('Find Entity', 'found')])
])
with _sm_container_2:
# x:131 y:44
OperatableStateMachine.add('Find Entity',
_sm_find_entity_1,
transitions={'found': 'found'},
autonomy={'found': Autonomy.Inherit},
remapping={'className': 'className', 'entity': 'entity'})
# x:129 y:197
OperatableStateMachine.add('Rotation',
_sm_rotation_0,
transitions={'end': 'not_found'},
autonomy={'end': Autonomy.Inherit})
with _state_machine:
# x:67 y:42
OperatableStateMachine.add('Look Center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'Container'},
autonomy={'done': Autonomy.Off})
# x:278 y:138
OperatableStateMachine.add('Look Center Not Found',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'pas_done'},
autonomy={'done': Autonomy.Off})
# x:58 y:326
OperatableStateMachine.add('Log Entity',
LogKeyState(text="Found entity: {}", severity=Logger.REPORT_HINT),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'entity'})
# x:63 y:126
OperatableStateMachine.add('Container',
_sm_container_2,
transitions={'found': 'WaitState', 'not_found': 'Look Center Not Found'},
autonomy={'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit},
remapping={'className': 'className', 'entity': 'entity'})
# x:67 y:222
OperatableStateMachine.add('WaitState',
WaitState(wait_time=1),
transitions={'done': 'Log Entity'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x: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:1103 y:529, x:321 y:227
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.Pose_Init = "IdlePose"
_state_machine.userdata.Closed_Gripper_Width = 1
_state_machine.userdata.Open_Gripper_Width = 255
_state_machine.userdata.ID = 0
_state_machine.userdata.PoseStart = "PoseStart"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365
_sm_look_in_the_eyes_0 = OperatableStateMachine(outcomes=['finished'])
with _sm_look_in_the_eyes_0:
# x:189 y:48
OperatableStateMachine.add('wait 3',
WaitState(wait_time=3),
transitions={'done': 'say ready'},
autonomy={'done': Autonomy.Off})
# x:171 y:288
OperatableStateMachine.add('reset head',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:164 y:163
OperatableStateMachine.add(
'say ready',
SaraSay(
sentence=
"Good, I see you want this bag. But, could you hand it to me please?",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'reset head'},
autonomy={'done': Autonomy.Off})
# x:30 y:365
_sm_look_at_it_1 = OperatableStateMachine(outcomes=['fail'],
input_keys=['position'])
with _sm_look_at_it_1:
# x:126 y:99
OperatableStateMachine.add('look',
LookAtPos(),
transitions={
'failed': 'look',
'done': 'look'
},
autonomy={
'failed': Autonomy.Off,
'done': Autonomy.Off
},
remapping={'pos': 'position'})
# x:30 y:365
_sm_point_at_it_2 = OperatableStateMachine(outcomes=['finished'],
input_keys=['position'])
with _sm_point_at_it_2:
# x:112 y:202
OperatableStateMachine.add(
'Action_point_at',
self.use_behavior(
Action_point_atSM,
'recevoir sac/Recevoir sac/look at bag/designate bag/point at it/Action_point_at'
),
transitions={
'finished': 'Action_point_at',
'failed': 'Action_point_at'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'targetPoint': 'position'})
# x:30 y:365
_sm_move_to_it_3 = OperatableStateMachine(outcomes=['arrived'],
input_keys=['position'])
with _sm_move_to_it_3:
# x:204 y:52
OperatableStateMachine.add('set distance',
SetKey(Value=1),
transitions={'done': 'get pose'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distance'})
# x:211 y:185
OperatableStateMachine.add('get pose',
Get_Reacheable_Waypoint(),
transitions={'done': 'move to bag'},
autonomy={'done': Autonomy.Off},
remapping={
'pose_in': 'position',
'distance': 'distance',
'pose_out': 'pose'
})
# x:188 y:279
OperatableStateMachine.add('move to bag',
SaraMoveBase(reference="map"),
transitions={
'arrived': 'arrived',
'failed': 'arrived'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365
_sm_designate_bag_4 = ConcurrencyContainer(
outcomes=['finished'],
input_keys=['position'],
conditions=[('finished', [('look at it', 'fail')]),
('finished', [('point at it', 'finished')]),
('finished', [('move to it', 'arrived')])])
with _sm_designate_bag_4:
# x:499 y:118
OperatableStateMachine.add('move to it',
_sm_move_to_it_3,
transitions={'arrived': 'finished'},
autonomy={'arrived': Autonomy.Inherit},
remapping={'position': 'position'})
# x:239 y:126
OperatableStateMachine.add('point at it',
_sm_point_at_it_2,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit},
remapping={'position': 'position'})
# x:45 y:121
OperatableStateMachine.add('look at it',
_sm_look_at_it_1,
transitions={'fail': 'finished'},
autonomy={'fail': Autonomy.Inherit},
remapping={'position': 'position'})
# x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365
_sm_receive_it_5 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
input_keys=['Closed_Gripper_Width', 'Open_Gripper_Width'],
conditions=[('finished', [('Action_Receive_Bag', 'finished'),
('look in the eyes', 'finished')]),
('failed', [('Action_Receive_Bag', 'failed')])])
with _sm_receive_it_5:
# x:30 y:149
OperatableStateMachine.add(
'Action_Receive_Bag',
self.use_behavior(
Action_Receive_BagSM,
'recevoir sac/Recevoir sac/receive it/Action_Receive_Bag'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'Closed_Gripper_Width': 'Closed_Gripper_Width',
'Open_Gripper_Width': 'Open_Gripper_Width',
'Closed_Gripper_Width': 'Closed_Gripper_Width'
})
# x:259 y:150
OperatableStateMachine.add('look in the eyes',
_sm_look_in_the_eyes_0,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit})
# x:30 y:322
_sm_look_at_bag_6 = OperatableStateMachine(outcomes=['finished'])
with _sm_look_at_bag_6:
# x:35 y:70
OperatableStateMachine.add('setName',
SetKey(Value="person"),
transitions={'done': 'say 1'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'personName'})
# x:233 y:109
OperatableStateMachine.add(
'say 1',
SaraSay(sentence="Please, point at the bag you want.",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'list'},
autonomy={'done': Autonomy.Off})
# x:28 y:193
OperatableStateMachine.add('list',
list_entities_by_name(
frontality_level=0.5,
distance_max=10),
transitions={
'found': 'calc',
'none_found': 'list'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'personName',
'entity_list': 'entity_list',
'number': 'number'
})
# x:230 y:363
OperatableStateMachine.add(
'get bag position from pointing',
GetPointedPositionOnPlane(planeHeight=0.2),
transitions={
'done': 'designate bag',
'not_pointing': 'say 1',
'pointing_up': 'say 1',
'failed': 'list'
},
autonomy={
'done': Autonomy.Off,
'not_pointing': Autonomy.Off,
'pointing_up': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'entity': 'entity',
'position': 'position'
})
# x:34 y:362
OperatableStateMachine.add(
'calc',
CalculationState(calculation=lambda x: x[0]),
transitions={'done': 'get bag position from pointing'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'entity_list',
'output_value': 'entity'
})
# x:241 y:554
OperatableStateMachine.add('designate bag',
_sm_designate_bag_4,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit},
remapping={'position': 'position'})
# x:30 y:365, x:182 y:454
_sm_recevoir_sac_7 = OperatableStateMachine(
outcomes=['failed', 'done'],
input_keys=['Closed_Gripper_Width', 'Open_Gripper_Width'])
with _sm_recevoir_sac_7:
# x:66 y:44
OperatableStateMachine.add('look at bag',
_sm_look_at_bag_6,
transitions={'finished': 'close'},
autonomy={'finished': Autonomy.Inherit})
# x:41 y:267
OperatableStateMachine.add('receive it',
_sm_receive_it_5,
transitions={
'finished': 'sayCAr',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'Closed_Gripper_Width':
'Closed_Gripper_Width',
'Open_Gripper_Width':
'Open_Gripper_Width'
})
# x:81 y:157
OperatableStateMachine.add(
'close',
SetGripperState(width=0, effort=0),
transitions={
'object': 'receive it',
'no_object': 'receive it'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:139 y:334
OperatableStateMachine.add(
'sayCAr',
SaraSay(sentence="PLease tell me when we get to the car.",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:30 y:365
_sm_listen_8 = OperatableStateMachine(outcomes=['done'])
with _sm_listen_8:
# x:50 y:76
OperatableStateMachine.add(
'SayFollow',
SaraSay(
sentence=
"I will follow you to the car now. Tell me when we get to the car.",
input_keys=[],
emotion=0,
block=False),
transitions={'done': 'LIsten'},
autonomy={'done': Autonomy.Off})
# x:161 y:148
OperatableStateMachine.add('LIsten',
GetSpeech(watchdog=10),
transitions={
'done': 'Listen2',
'nothing': 'LIsten',
'fail': 'LIsten'
},
autonomy={
'done': Autonomy.Off,
'nothing': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'words': 'words'})
# x:201 y:256
OperatableStateMachine.add(
'Listen2',
RegexTester(regex=".*((car)|(here it is)|(now)).*"),
transitions={
'true': 'done',
'false': 'Listen2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'text': 'words',
'result': 'result'
})
# x:520 y:327
_sm_follow_9 = OperatableStateMachine(outcomes=['failed'],
input_keys=['ID'])
with _sm_follow_9:
# x:128 y:127
OperatableStateMachine.add(
'Action_follow',
self.use_behavior(Action_followSM,
'Follow and listen/follow/Action_follow'),
transitions={'failed': 'set name'},
autonomy={'failed': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:288 y:346
OperatableStateMachine.add('list people',
list_entities_by_name(
frontality_level=0.5,
distance_max=10),
transitions={
'found': 'get first ID',
'none_found': 'failed'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'name',
'entity_list': 'entity_list',
'number': 'number'
})
# x:109 y:334
OperatableStateMachine.add(
'get first ID',
CalculationState(calculation=lambda x: x[0].ID),
transitions={'done': 'Action_follow'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'entity_list',
'output_value': 'ID'
})
# x:349 y:116
OperatableStateMachine.add('set name',
SetKey(Value="person"),
transitions={'done': 'list people'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'name'})
# x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365
_sm_recevoir_sac_10 = ConcurrencyContainer(
outcomes=['failed', 'done'],
input_keys=['Closed_Gripper_Width', 'Open_Gripper_Width'],
conditions=[('failed', [('Recevoir sac', 'failed')]),
('done', [('Recevoir sac', 'done')]),
('done', [('LookAtClosest', 'failed')])])
with _sm_recevoir_sac_10:
# x:30 y:40
OperatableStateMachine.add('Recevoir sac',
_sm_recevoir_sac_7,
transitions={
'failed': 'failed',
'done': 'done'
},
autonomy={
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'Closed_Gripper_Width':
'Closed_Gripper_Width',
'Open_Gripper_Width':
'Open_Gripper_Width'
})
# x:234 y:114
OperatableStateMachine.add('LookAtClosest',
self.use_behavior(
LookAtClosestSM,
'recevoir sac/LookAtClosest'),
transitions={'failed': 'done'},
autonomy={'failed': Autonomy.Inherit})
# x:390 y:296, x:111 y:269, x:230 y:365, x:542 y:339, x:465 y:331, x:530 y:380
_sm_follow_and_listen_11 = ConcurrencyContainer(
outcomes=['done', 'failed'],
input_keys=['ID'],
conditions=[('done', [('Listen', 'done')]),
('failed', [('follow', 'failed')]),
('done', [('continue', 'true')]),
('done', [('continue', 'false')])])
with _sm_follow_and_listen_11:
# x:132 y:90
OperatableStateMachine.add('follow',
_sm_follow_9,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:356 y:105
OperatableStateMachine.add('Listen',
_sm_listen_8,
transitions={'done': 'done'},
autonomy={'done': Autonomy.Inherit})
# x:485 y:162
OperatableStateMachine.add('continue',
ContinueButton(),
transitions={
'true': 'done',
'false': 'done'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
})
# x:76 y:535
_sm_getidope_12 = OperatableStateMachine(outcomes=['done'],
output_keys=['ID'])
with _sm_getidope_12:
# x:55 y:63
OperatableStateMachine.add('Person',
SetKey(Value="person"),
transitions={'done': 'GEtId'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'name'})
# x:43 y:180
OperatableStateMachine.add('GEtId',
list_entities_by_name(
frontality_level=0.5,
distance_max=10),
transitions={
'found': 'ID',
'none_found': 'GEtId'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'name',
'entity_list': 'Entities_list',
'number': 'number'
})
# x:52 y:290
OperatableStateMachine.add(
'ID',
CalculationState(calculation=lambda x: x[0].ID),
transitions={'done': 'setID'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'Entities_list',
'output_value': 'ID'
})
# x:37 y:399
OperatableStateMachine.add(
'setID',
SetRosParam(ParamName="behavior/Operator/Id"),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={'Value': 'ID'})
with _state_machine:
# x:38 y:33
OperatableStateMachine.add('continue',
ContinueButton(),
transitions={
'true': 'GEtPose',
'false': 'GEtPose'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
})
# x:35 y:351
OperatableStateMachine.add(
'ImHere',
SaraSay(sentence="I am ready to carry your luggage!",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'recevoir sac'},
autonomy={'done': Autonomy.Off})
# x:28 y:539
OperatableStateMachine.add(
'GetIDOpe',
_sm_getidope_12,
transitions={'done': 'Follow and listen'},
autonomy={'done': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:220 y:536
OperatableStateMachine.add('Follow and listen',
_sm_follow_and_listen_11,
transitions={
'done': 'Action_Give_Back_Bag',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'ID': 'ID'})
# x:411 y:525
OperatableStateMachine.add('Action_Give_Back_Bag',
self.use_behavior(
Action_Give_Back_BagSM,
'Action_Give_Back_Bag'),
transitions={
'finished': 'GoBackHome',
'failed': 'Action_Give_Back_Bag'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:16 y:206
OperatableStateMachine.add('Init_Sequence',
self.use_behavior(
Init_SequenceSM, 'Init_Sequence'),
transitions={
'finished': 'move head up',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:634 y:522
OperatableStateMachine.add(
'GoBackHome',
SaraSay(sentence="I will go back home now. Have a good day!",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'setarm'},
autonomy={'done': Autonomy.Off})
# x:922 y:519
OperatableStateMachine.add('Action_Move',
self.use_behavior(
Action_MoveSM, 'Action_Move'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pose': 'Origin'})
# x:35 y:281
OperatableStateMachine.add('move head up',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'ImHere'},
autonomy={'done': Autonomy.Off})
# x:35 y:427
OperatableStateMachine.add('recevoir sac',
_sm_recevoir_sac_10,
transitions={
'failed': 'failed',
'done': 'GetIDOpe'
},
autonomy={
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'Closed_Gripper_Width':
'Closed_Gripper_Width',
'Open_Gripper_Width':
'Open_Gripper_Width'
})
# x:770 y:526
OperatableStateMachine.add('setarm',
RunTrajectory(file="sac_transport",
duration=0),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:24 y:134
OperatableStateMachine.add('GEtPose',
Get_Robot_Pose(),
transitions={'done': 'Init_Sequence'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'Origin'})
return _state_machine
