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: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:960 y:715, x:912 y:318, x:904 y:185, x:941 y:610
_state_machine = OperatableStateMachine(
outcomes=['success', 'unreachable', 'not found', 'dropped'],
input_keys=['objectID'])
_state_machine.userdata.objectID = 539
_state_machine.userdata.PreGripPose = "pre_grip_pose"
_state_machine.userdata.entity = 0
_state_machine.userdata.grasp_pose = 0
_state_machine.userdata.approach_pose = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:130 y:365, x:230 y:365
_sm_look_at_it_for_3s_0 = ConcurrencyContainer(
outcomes=['done'],
input_keys=['ID'],
conditions=[('done', [('look', 'failed')]),
('done', [('wait', 'done')])])
with _sm_look_at_it_for_3s_0:
# x:30 y:54
OperatableStateMachine.add('look',
KeepLookingAt(),
transitions={'failed': 'done'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:187 y:111
OperatableStateMachine.add('wait',
WaitState(wait_time=6),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:334 y:336, x:96 y:627
_sm_get_object_1 = OperatableStateMachine(
outcomes=['not_found', 'finished'],
input_keys=['objectID'],
output_keys=['entity'])
with _sm_get_object_1:
# x:67 y:27
OperatableStateMachine.add(
'start segmentation',
SetSegmentationRosParam(ValueTableSegmentation=True,
ValueObjectSegmentation=True),
transitions={'done': 'Look at it for 3s'},
autonomy={'done': Autonomy.Off})
# x:61 y:410
OperatableStateMachine.add(
'Say_See_It',
SaraSay(sentence=lambda x: "I see the " + x[0].name,
input_keys=["Entity"],
emotion=0,
block=True),
transitions={'done': 'pregrip'},
autonomy={'done': Autonomy.Off},
remapping={'Entity': 'entity'})
# x:72 y:322
OperatableStateMachine.add('getobject',
GetEntityByID(),
transitions={
'found': 'Say_See_It',
'not_found': 'not_found'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'objectID',
'Entity': 'entity'
})
# x:62 y:224
OperatableStateMachine.add('stop segmentation',
SetSegmentationRosParam(
ValueTableSegmentation=False,
ValueObjectSegmentation=False),
transitions={'done': 'getobject'},
autonomy={'done': Autonomy.Off})
# x:70 y:122
OperatableStateMachine.add(
'Look at it for 3s',
_sm_look_at_it_for_3s_0,
transitions={'done': 'stop segmentation'},
autonomy={'done': Autonomy.Inherit},
remapping={'ID': 'objectID'})
# x:57 y:513
OperatableStateMachine.add('pregrip',
RunTrajectory(file="pre_grip_pose",
duration=5),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:153 y:506, x:350 y:396
_sm_get_closer_2 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['entity'])
with _sm_get_closer_2:
# x:120 y:61
OperatableStateMachine.add('set distance',
SetKey(Value=0.6),
transitions={'done': 'get att'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distance'})
# x:113 y:172
OperatableStateMachine.add('get att',
GetAttribute(attributes=["position"]),
transitions={'done': 'get waypoint'},
autonomy={'done': Autonomy.Off},
remapping={
'object': 'entity',
'position': 'position'
})
# x:116 y:390
OperatableStateMachine.add('move to waypoint',
SaraMoveBase(reference="map"),
transitions={
'arrived': 'finished',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:107 y:272
OperatableStateMachine.add(
'get waypoint',
Get_Reacheable_Waypoint(),
transitions={'done': 'move to waypoint'},
autonomy={'done': Autonomy.Off},
remapping={
'pose_in': 'position',
'distance': 'distance',
'pose_out': 'pose'
})
# x:280 y:183, x:92 y:289
_sm_get_on_it_3 = OperatableStateMachine(outcomes=['failed', 'done'],
input_keys=['grasp_pose'])
with _sm_get_on_it_3:
# x:76 y:26
OperatableStateMachine.add(
'open gripper',
SetGripperState(width=0.1, effort=0),
transitions={
'object': 'move forward',
'no_object': 'move forward'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:64 y:158
OperatableStateMachine.add('move forward',
MoveitMoveCartesian(
move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'targetPose': 'grasp_pose'})
# x:30 y:324, x:130 y:324
_sm_get_away_from_failure_4 = OperatableStateMachine(
outcomes=['done', 'failed'])
with _sm_get_away_from_failure_4:
# x:30 y:40
OperatableStateMachine.add(
'open 2',
SetGripperState(width=0.1, effort=1),
transitions={
'object': 'go back',
'no_object': 'go back'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:44 y:167
OperatableStateMachine.add('go back',
RunTrajectory(file="pre_grip_pose",
duration=0),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:68 y:579, x:349 y:211
_sm_lift_object_5 = OperatableStateMachine(
outcomes=['done', 'failed'],
input_keys=['approach_pose', 'up_pose'])
with _sm_lift_object_5:
# x:45 y:34
OperatableStateMachine.add('move up',
MoveitMoveCartesian(
move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'genpose',
'failed': 'genpose'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'targetPose': 'up_pose'})
# x:39 y:480
OperatableStateMachine.add('place arm',
RunTrajectory(file="transport",
duration=0),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:40 y:144
OperatableStateMachine.add('genpose',
GenPoseEuler(x=-0.2,
y=0,
z=0,
roll=0,
pitch=0,
yaw=0),
transitions={'done': 'move_back'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'backPose'})
# x:42 y:382
OperatableStateMachine.add('move_back',
SaraMoveBase(reference="base_link"),
transitions={
'arrived': 'place arm',
'failed': 'place arm'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'backPose'})
# x:263 y:214, x:271 y:492
_sm_validation_and_approach_6 = OperatableStateMachine(
outcomes=['failed', 'done'],
input_keys=['grasp_pose', 'approach_pose'])
with _sm_validation_and_approach_6:
# x:85 y:30
OperatableStateMachine.add('checkifposeaccess',
MoveitMove(move=False,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'say can reach',
'failed': 'sayapp'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'grasp_pose'})
# x:48 y:236
OperatableStateMachine.add('move approach',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'approach_pose'})
# x:71 y:115
OperatableStateMachine.add('say can reach',
SaraSay(sentence="I will grab it",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'move approach'},
autonomy={'done': Autonomy.Off})
# x:342 y:93
OperatableStateMachine.add('sayapp',
SaraSay(sentence="unreachable",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:40 y:26
OperatableStateMachine.add('start loop',
SetKey(Value=0),
transitions={'done': 'Get object'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'i'})
# x:256 y:616
OperatableStateMachine.add(
'gripclose',
SetGripperState(width=0, effort=250),
transitions={
'object': 'say picked',
'no_object': 'get away from failure'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:255 y:507
OperatableStateMachine.add('cant reach',
SaraSay(
sentence="Hum! I can't reach it.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'for 2 try'},
autonomy={'done': Autonomy.Off})
# x:261 y:732
OperatableStateMachine.add('say picked',
SaraSay(sentence="I think I got it",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'Lift object'},
autonomy={'done': Autonomy.Off})
# x:751 y:715
OperatableStateMachine.add('welcome',
SaraSay(sentence="you are welcome",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'success'},
autonomy={'done': Autonomy.Off})
# x:9 y:403
OperatableStateMachine.add('validation and approach',
_sm_validation_and_approach_6,
transitions={
'failed': 'say plan',
'done': 'gen up pose'
},
autonomy={
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'grasp_pose': 'grasp_pose',
'approach_pose': 'approach_pose'
})
# x:516 y:717
OperatableStateMachine.add('Lift object',
_sm_lift_object_5,
transitions={
'done': 'welcome',
'failed': 'welcome'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'approach_pose': 'approach_pose',
'up_pose': 'up_pose'
})
# x:480 y:609
OperatableStateMachine.add('get away from failure',
_sm_get_away_from_failure_4,
transitions={
'done': 'say missed',
'failed': 'say missed'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:25 y:311
OperatableStateMachine.add('get grasps',
GetGraspFromEntity(
approachDistance=0.2,
distanceScoringMultiplier=1,
orientationScoringMultiplier=2,
graspScoringMultiplier=1),
transitions={
'done': 'validation and approach',
'failed': 'say cant handle'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'Entity': 'entity',
'ApproachPose': 'approach_pose',
'GraspingPose': 'grasp_pose'
})
# x:738 y:602
OperatableStateMachine.add('say missed',
SaraSay(sentence="Oops! I missed.",
input_keys=[],
emotion=2,
block=True),
transitions={'done': 'dropped'},
autonomy={'done': Autonomy.Off})
# x:257 y:279
OperatableStateMachine.add(
'say cant handle',
SaraSay(sentence="I don't understand how to grab it.",
input_keys=[],
emotion=3,
block=True),
transitions={'done': 'for 2 try'},
autonomy={'done': Autonomy.Off})
# x:18 y:605
OperatableStateMachine.add('get on it',
_sm_get_on_it_3,
transitions={
'failed': 'cant reach',
'done': 'gripclose'
},
autonomy={
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={'grasp_pose': 'grasp_pose'})
# x:261 y:387
OperatableStateMachine.add('say plan',
SaraSay(sentence="Planing failed",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'for 2 try'},
autonomy={'done': Autonomy.Off})
# x:20 y:496
OperatableStateMachine.add('gen up pose',
GenGripperPose(l=0, z=0.1,
planar=False),
transitions={
'done': 'get on it',
'fail': 'cant reach'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'grasp_pose',
'pose_out': 'up_pose'
})
# x:558 y:231
OperatableStateMachine.add('get closer',
_sm_get_closer_2,
transitions={
'finished': 'Get object',
'failed': 'unreachable'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'entity': 'entity'})
# x:553 y:390
OperatableStateMachine.add('for 2 try',
ForLoopWithInput(repeat=1),
transitions={
'do': 'get closer',
'end': 'unreachable'
},
autonomy={
'do': Autonomy.Off,
'end': Autonomy.Off
},
remapping={
'index_in': 'i',
'index_out': 'i'
})
# x:28 y:217
OperatableStateMachine.add('Get object',
_sm_get_object_1,
transitions={
'not_found': 'not found',
'finished': 'get grasps'
},
autonomy={
'not_found': Autonomy.Inherit,
'finished': Autonomy.Inherit
},
remapping={
'objectID': 'objectID',
'entity': 'entity'
})
return _state_machine
def create(self):
# x: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
def create(self):
# x:746 y:544, x:65 y:361
_state_machine = OperatableStateMachine(
outcomes=['finished', 'not found'], input_keys=['Position'])
_state_machine.userdata.Position = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365
_sm_verifie_presence_0 = OperatableStateMachine(outcomes=['not found'])
with _sm_verifie_presence_0:
# x:90 y:107
OperatableStateMachine.add(
'getID',
GetRosParam(ParamName="behavior/Operator/Id"),
transitions={
'done': 'getOperator',
'failed': 'not found'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'Value': 'ID'})
# x:297 y:176
OperatableStateMachine.add('getOperator',
GetEntityByID(),
transitions={
'found': 'getID',
'not_found': 'not found'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'ID',
'Entity': 'Operator'
})
# x:208 y:123, x:289 y:206, x:75 y:376, x:330 y:365, x:430 y:365
_sm_deplacement_et_verification_de_presence_1 = ConcurrencyContainer(
outcomes=['arrived', 'failed'],
input_keys=['Position'],
conditions=[('failed', [('verifie presence', 'not found')]),
('arrived', [('saramove', 'arrived')]),
('failed', [('saramove', 'failed')])])
with _sm_deplacement_et_verification_de_presence_1:
# x:70 y:106
OperatableStateMachine.add('saramove',
SaraMoveBase(reference="map"),
transitions={
'arrived': 'arrived',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'Position'})
# x:255 y:90
OperatableStateMachine.add(
'verifie presence',
_sm_verifie_presence_0,
transitions={'not found': 'failed'},
autonomy={'not found': Autonomy.Inherit})
with _state_machine:
# x:55 y:57
OperatableStateMachine.add(
'Get_operator',
self.use_behavior(sara_flexbe_behaviors__Get_operatorSM,
'Get_operator'),
transitions={
'Found': 'foundyou!',
'NotFound': 'not found'
},
autonomy={
'Found': Autonomy.Inherit,
'NotFound': Autonomy.Inherit
},
remapping={'Operator': 'Operator'})
# x:162 y:210
OperatableStateMachine.add(
'foundyou!',
SaraSay(sentence="Operator, please follow me",
emotion=1,
block=True),
transitions={
'done': 'deplacement et verification de presence'
},
autonomy={'done': Autonomy.Off})
# x:398 y:500
OperatableStateMachine.add(
'destinationreached',
SaraSay(sentence="We have reached our destination",
emotion=1,
block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:247 y:319
OperatableStateMachine.add(
'deplacement et verification de presence',
_sm_deplacement_et_verification_de_presence_1,
transitions={
'arrived': 'destinationreached',
'failed': 'lostyou'
},
autonomy={
'arrived': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'Position': 'Position'})
# x:599 y:180
OperatableStateMachine.add(
'lostyou',
SaraSay(sentence="I lost you, please stay where you are",
emotion=1,
block=True),
transitions={'done': 'Get_operator'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x: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:738 y:371, x:736 y:220
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.bin1Waypoint = "trash bin 2"
_state_machine.userdata.bin2Waypoint = "trash bin"
_state_machine.userdata.bin1Height = "1"
_state_machine.userdata.bin2Height = "1"
_state_machine.userdata.dropzone1Waypoint = "drop_zone1"
_state_machine.userdata.dropzone2Waypoint = "drop_zone2"
_state_machine.userdata.DoorName = "door_1_entry"
_state_machine.userdata.exit_door = "door_2_exit"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:810 y:644, x:808 y:125
_sm_go_to_drop_the_bag_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['dropzoneWaypoint', 'exit_door'])
with _sm_go_to_drop_the_bag_0:
# x:84 y:36
OperatableStateMachine.add('say drop',
SaraSay(sentence="I will go and drop this bag in the drop zone.", input_keys=[], emotion=0, block=False),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:66 y:598
OperatableStateMachine.add('open the gripper',
SetGripperState(width=0.1, effort=1),
transitions={'object': 'go to pose transport', 'no_object': 'go to pose transport'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:341 y:469
OperatableStateMachine.add('say cant go to drop zone',
SaraSay(sentence="I am not able to go to the drop zone. I will put the bag here and go to the second bin.", input_keys=[], emotion=0, block=True),
transitions={'done': 'run depose sac'},
autonomy={'done': Autonomy.Off})
# x:74 y:490
OperatableStateMachine.add('run depose sac',
RunTrajectory(file="poubelle_depose", duration=10),
transitions={'done': 'open the gripper'},
autonomy={'done': Autonomy.Off})
# x:69 y:726
OperatableStateMachine.add('go to pose transport',
RunTrajectory(file="transport", duration=0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:76 y:386
OperatableStateMachine.add('Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'First bin/go to drop the bag/Action_Move'),
transitions={'finished': 'run depose sac', 'failed': 'say cant go to drop zone'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'dropzoneWaypoint'})
# x:570 y:519, x:575 y:330
_sm_find_the_bin_1 = OperatableStateMachine(outcomes=['finished', 'failed'])
with _sm_find_the_bin_1:
# x:89 y:62
OperatableStateMachine.add('place arm',
RunTrajectory(file="poubelle_init", duration=0),
transitions={'done': 'leve la tete'},
autonomy={'done': Autonomy.Off})
# x:270 y:378
OperatableStateMachine.add('get waypoint',
Get_Reacheable_Waypoint(),
transitions={'done': 'move'},
autonomy={'done': Autonomy.Off},
remapping={'pose_in': 'detectedBin', 'distance': 'distanceToBin', 'pose_out': 'poseToBin'})
# x:296 y:284
OperatableStateMachine.add('set distance to bin',
SetKey(Value=0.5),
transitions={'done': 'get waypoint'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distanceToBin'})
# x:278 y:201
OperatableStateMachine.add('pose form lidar to map',
TF_transformation(in_ref="front_hokuyo_link", out_ref="map"),
transitions={'done': 'set distance to bin', 'fail': 'failed'},
autonomy={'done': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'in_pos': 'detectedObstacle', 'out_pos': 'detectedBin'})
# x:80 y:514
OperatableStateMachine.add('redo ajustements de la position',
ForLoop(repeat=2),
transitions={'do': 'find closest obstacle point', 'end': 'finished'},
autonomy={'do': Autonomy.Off, 'end': Autonomy.Off},
remapping={'index': 'index'})
# x:87 y:129
OperatableStateMachine.add('find closest obstacle point',
GetClosestObstacle(topic="/scan", maximumDistance=2),
transitions={'done': 'pose form lidar to map'},
autonomy={'done': Autonomy.Off},
remapping={'Angle': 'Angle', 'distance': 'distance', 'position': 'detectedObstacle'})
# x:288 y:77
OperatableStateMachine.add('leve la tete',
SaraSetHeadAngle(pitch=0, yaw=0),
transitions={'done': 'find closest obstacle point'},
autonomy={'done': Autonomy.Off})
# x:287 y:464
OperatableStateMachine.add('move',
SaraMoveBase(reference="map"),
transitions={'arrived': 'redo ajustements de la position', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'pose': 'poseToBin'})
# x:573 y:546, x:622 y:92
_sm_go_to_bin_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin1Waypoint'])
with _sm_go_to_bin_2:
# x:86 y:104
OperatableStateMachine.add('bras sur le cote',
RunTrajectory(file="poubelle_transport", duration=0),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:254 y:199
OperatableStateMachine.add('Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'First bin/go to bin/Action_Move'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'bin1Waypoint'})
# x:600 y:617, x:666 y:442
_sm_find_the_bin_3 = OperatableStateMachine(outcomes=['finished', 'failed'])
with _sm_find_the_bin_3:
# x:89 y:62
OperatableStateMachine.add('place arm',
RunTrajectory(file="poubelle_init", duration=0),
transitions={'done': 'leve la tete'},
autonomy={'done': Autonomy.Off})
# x:268 y:410
OperatableStateMachine.add('get waypoint',
Get_Reacheable_Waypoint(),
transitions={'done': 'move'},
autonomy={'done': Autonomy.Off},
remapping={'pose_in': 'detectedBin', 'distance': 'distanceToBin', 'pose_out': 'poseToBin'})
# x:293 y:326
OperatableStateMachine.add('set distance to bin',
SetKey(Value=0.5),
transitions={'done': 'get waypoint'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distanceToBin'})
# x:278 y:238
OperatableStateMachine.add('pose form lidar to map',
TF_transformation(in_ref="front_hokuyo_link", out_ref="map"),
transitions={'done': 'set distance to bin', 'fail': 'failed'},
autonomy={'done': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'in_pos': 'detectedObstacle', 'out_pos': 'detectedBin'})
# x:80 y:598
OperatableStateMachine.add('redo ajustements de la position',
ForLoop(repeat=2),
transitions={'do': 'find closest obstacle point', 'end': 'finished'},
autonomy={'do': Autonomy.Off, 'end': Autonomy.Off},
remapping={'index': 'index'})
# x:89 y:168
OperatableStateMachine.add('find closest obstacle point',
GetClosestObstacle(topic="/scan", maximumDistance=2),
transitions={'done': 'pose form lidar to map'},
autonomy={'done': Autonomy.Off},
remapping={'Angle': 'Angle', 'distance': 'distance', 'position': 'detectedObstacle'})
# x:288 y:104
OperatableStateMachine.add('leve la tete',
SaraSetHeadAngle(pitch=0, yaw=0),
transitions={'done': 'find closest obstacle point'},
autonomy={'done': Autonomy.Off})
# x:285 y:504
OperatableStateMachine.add('move',
SaraMoveBase(reference="map"),
transitions={'arrived': 'redo ajustements de la position', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'pose': 'poseToBin'})
# x:30 y:365, x:130 y:365
_sm_go_to_bin_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin2Waypoint'])
with _sm_go_to_bin_4:
# x:59 y:100
OperatableStateMachine.add('bras sur le cote',
RunTrajectory(file="poubelle_transport", duration=0),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:254 y:199
OperatableStateMachine.add('Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'second bin/go to bin/Action_Move'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'bin2Waypoint'})
# x:418 y:580, x:694 y:339
_sm_go_to_drop_the_bag_5 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['dropzoneWaypoint', 'exit_door'])
with _sm_go_to_drop_the_bag_5:
# x:65 y:215
OperatableStateMachine.add('Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'second bin/go to drop the bag/Action_Move'),
transitions={'finished': 'run depose sac', 'failed': 'say cant go to drop zone'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'dropzoneWaypoint'})
# x:385 y:324
OperatableStateMachine.add('say cant go to drop zone',
SaraSay(sentence="I am not able to go to the drop zone. I will put the bag here.", input_keys=[], emotion=0, block=True),
transitions={'done': 'run depose sac'},
autonomy={'done': Autonomy.Off})
# x:76 y:340
OperatableStateMachine.add('run depose sac',
RunTrajectory(file="poubelle_depose", duration=10),
transitions={'done': 'open the gripper'},
autonomy={'done': Autonomy.Off})
# x:74 y:505
OperatableStateMachine.add('go to pose transport',
RunTrajectory(file="transport", duration=0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:74 y:426
OperatableStateMachine.add('open the gripper',
SetGripperState(width=0.1, effort=1),
transitions={'object': 'go to pose transport', 'no_object': 'go to pose transport'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:704 y:599, x:717 y:53
_sm_first_bin_6 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin1Waypoint', 'bin1Height', 'dropzoneWaypoint', 'exit_door'])
with _sm_first_bin_6:
# x:57 y:26
OperatableStateMachine.add('Init_Sequence',
self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'First bin/Init_Sequence'),
transitions={'finished': 'go to bin', 'failed': 'go to bin'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:257 y:329
OperatableStateMachine.add('Action_TakeBag',
self.use_behavior(sara_flexbe_behaviors__Action_TakeBagSM, 'First bin/Action_TakeBag'),
transitions={'finished': 'go to drop the bag', 'failed': 'Missed It once'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:275 y:35
OperatableStateMachine.add('go to bin',
_sm_go_to_bin_2,
transitions={'finished': 'find the bin', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'bin1Waypoint': 'bin1Waypoint'})
# x:286 y:164
OperatableStateMachine.add('find the bin',
_sm_find_the_bin_1,
transitions={'finished': 'Try twice', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:292 y:234
OperatableStateMachine.add('Try twice',
ForLoop(repeat=1),
transitions={'do': 'Action_TakeBag', 'end': 'failed'},
autonomy={'do': Autonomy.Off, 'end': Autonomy.Off},
remapping={'index': 'index'})
# x:102 y:262
OperatableStateMachine.add('Missed It once',
SaraSay(sentence="I will try one more time.", input_keys=[], emotion=0, block=True),
transitions={'done': 'Try twice'},
autonomy={'done': Autonomy.Off})
# x:267 y:432
OperatableStateMachine.add('go to drop the bag',
_sm_go_to_drop_the_bag_0,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'dropzoneWaypoint': 'dropzoneWaypoint', 'exit_door': 'exit_door'})
# x:946 y:467, x:907 y:75
_sm_second_bin_7 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin2Waypoint', 'bin2Height', 'dropzoneWaypoint', 'exit_door'])
with _sm_second_bin_7:
# x:57 y:26
OperatableStateMachine.add('Init_Sequence',
self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'second bin/Init_Sequence'),
transitions={'finished': 'go to bin', 'failed': 'go to bin'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:267 y:432
OperatableStateMachine.add('go to drop the bag',
_sm_go_to_drop_the_bag_5,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'dropzoneWaypoint': 'dropzoneWaypoint', 'exit_door': 'exit_door'})
# x:257 y:346
OperatableStateMachine.add('Action_TakeBag',
self.use_behavior(sara_flexbe_behaviors__Action_TakeBagSM, 'second bin/Action_TakeBag'),
transitions={'finished': 'go to drop the bag', 'failed': 'Missed It once'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:275 y:35
OperatableStateMachine.add('go to bin',
_sm_go_to_bin_4,
transitions={'finished': 'find the bin', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'bin2Waypoint': 'bin2Waypoint'})
# x:286 y:164
OperatableStateMachine.add('find the bin',
_sm_find_the_bin_3,
transitions={'finished': 'Try twice', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:298 y:254
OperatableStateMachine.add('Try twice',
ForLoop(repeat=1),
transitions={'do': 'Action_TakeBag', 'end': 'failed'},
autonomy={'do': Autonomy.Off, 'end': Autonomy.Off},
remapping={'index': 'index'})
# x:102 y:262
OperatableStateMachine.add('Missed It once',
SaraSay(sentence="I will try one more time.", input_keys=[], emotion=0, block=True),
transitions={'done': 'Try twice'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:15 y:242
OperatableStateMachine.add('button',
ContinueButton(),
transitions={'true': 'Action_Pass_Door', 'false': 'Action_Pass_Door'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off})
# x:485 y:201
OperatableStateMachine.add('try second bin',
SaraSay(sentence="I failed to take out the garbage from the first bin but I will try the second bin.", input_keys=[], emotion=0, block=True),
transitions={'done': 'second bin'},
autonomy={'done': Autonomy.Off})
# x:267 y:199
OperatableStateMachine.add('say take second bag',
SaraSay(sentence="Good! I will now get rid of the second bag.", input_keys=[], emotion=0, block=True),
transitions={'done': 'second bin'},
autonomy={'done': Autonomy.Off})
# x:267 y:376
OperatableStateMachine.add('second bin',
_sm_second_bin_7,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'bin2Waypoint': 'bin2Waypoint', 'bin2Height': 'bin2Height', 'dropzoneWaypoint': 'dropzone2Waypoint', 'exit_door': 'exit_door'})
# x:94 y:153
OperatableStateMachine.add('IllManageSomehow',
SaraSay(sentence="Hmmm. This is a hard door to pass...", input_keys=[], emotion=0, block=True),
transitions={'done': 'First bin'},
autonomy={'done': Autonomy.Off})
# x:277 y:63
OperatableStateMachine.add('First bin',
_sm_first_bin_6,
transitions={'finished': 'say take second bag', 'failed': 'try second bin'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'bin1Waypoint': 'bin1Waypoint', 'bin1Height': 'bin1Height', 'dropzoneWaypoint': 'dropzone1Waypoint', 'exit_door': 'exit_door'})
# x:65 y:53
OperatableStateMachine.add('Action_Pass_Door',
self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'Action_Pass_Door'),
transitions={'Done': 'First bin', 'Fail': 'IllManageSomehow'},
autonomy={'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit},
remapping={'DoorName': 'DoorName'})
return _state_machine
def create(self):
# x:902 y:757, x:882 y:161
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:1232 y:907, x:1267 y:613, x:1271 y:566
_sm_find_table_and_place_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'no_table'])
with _sm_find_table_and_place_0:
# x:877 y:673
OperatableStateMachine.add('run traj',
RunTrajectory(file="receive_object", duration=0),
transitions={'done': 'say release'},
autonomy={'done': Autonomy.Off})
# x:532 y:572
OperatableStateMachine.add('Action_find_2',
self.use_behavior(Action_findSM, 'take objects and bring the order to customer/find table and place/Action_find_2'),
transitions={'done': 'get table position', 'failed': 'say do not find table'},
autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'className': 'tableKey', 'entity': 'tableEntity'})
# x:576 y:514
OperatableStateMachine.add('set tableKey',
SetKey(Value="table"),
transitions={'done': 'Action_find_2'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'tableKey'})
# x:538 y:638
OperatableStateMachine.add('get table position',
CalculationState(calculation=lambda x: x.position),
transitions={'done': 'Action_place'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'tableEntity', 'output_value': 'pose'})
# x:736 y:677
OperatableStateMachine.add('say do not find table',
SaraSay(sentence="I can not find the table. Please be ready to grab the object.", input_keys=[], emotion=0, block=True),
transitions={'done': 'run traj'},
autonomy={'done': Autonomy.Off})
# x:41 y:231
OperatableStateMachine.add('find a table and a free spot',
GetPositionToPlaceOnTable(),
transitions={'done': 'Action_place', 'not_found': 'look down_left'},
autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'})
# x:291 y:805
OperatableStateMachine.add('Action_place',
self.use_behavior(Action_placeSM, 'take objects and bring the order to customer/find table and place/Action_place'),
transitions={'finished': 'finished', 'failed': 'say do not find table'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pos': 'pose'})
# x:50 y:102
OperatableStateMachine.add('look down',
SaraSetHeadAngle(pitch=0, yaw=0.7),
transitions={'done': 'wait'},
autonomy={'done': Autonomy.Off})
# x:214 y:378
OperatableStateMachine.add('find a table and a free spot_2',
GetPositionToPlaceOnTable(),
transitions={'done': 'Action_place', 'not_found': 'look down_right'},
autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'})
# x:240 y:232
OperatableStateMachine.add('look down_left',
SaraSetHeadAngle(pitch=0.7, yaw=0.6),
transitions={'done': 'wait_2'},
autonomy={'done': Autonomy.Off})
# x:387 y:513
OperatableStateMachine.add('find a table and a free spot_2_2',
GetPositionToPlaceOnTable(),
transitions={'done': 'Action_place', 'not_found': 'set tableKey'},
autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'})
# x:402 y:378
OperatableStateMachine.add('look down_right',
SaraSetHeadAngle(pitch=-0.7, yaw=0.6),
transitions={'done': 'wait_3'},
autonomy={'done': Autonomy.Off})
# x:66 y:169
OperatableStateMachine.add('wait',
WaitState(wait_time=3),
transitions={'done': 'find a table and a free spot'},
autonomy={'done': Autonomy.Off})
# x:242 y:294
OperatableStateMachine.add('wait_2',
WaitState(wait_time=3),
transitions={'done': 'find a table and a free spot_2'},
autonomy={'done': Autonomy.Off})
# x:411 y:444
OperatableStateMachine.add('wait_3',
WaitState(wait_time=3),
transitions={'done': 'find a table and a free spot_2_2'},
autonomy={'done': Autonomy.Off})
# x:1000 y:670
OperatableStateMachine.add('say release',
SaraSay(sentence="Here is your order. I will open my gripper in 3, 2, 1, now.", input_keys=[], emotion=0, block=True),
transitions={'done': 'open gripper'},
autonomy={'done': Autonomy.Off})
# x:1122 y:666
OperatableStateMachine.add('open gripper',
SetGripperState(width=0.1, effort=1),
transitions={'object': 'say good day', 'no_object': 'say good day'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:1170 y:771
OperatableStateMachine.add('say good day',
SaraSay(sentence="Thank you. Have a good day.", input_keys=[], emotion=6, block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:62 y:29
OperatableStateMachine.add('set distance',
SetKey(Value=0.3),
transitions={'done': 'look down'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distanceFromEdge'})
# x:30 y:458, x:130 y:458
_sm_keep_looking_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['personID'])
with _sm_keep_looking_1:
# x:30 y:40
OperatableStateMachine.add('keep',
KeepLookingAt(),
transitions={'failed': 'keep'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'personID'})
# x:30 y:458, x:130 y:458
_sm_ask_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['question'], output_keys=['answer'])
with _sm_ask_2:
# x:30 y:40
OperatableStateMachine.add('Action_Ask',
self.use_behavior(Action_AskSM, 'ask and save order/ask AND look person/ask/Action_Ask'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'question': 'question', 'answer': 'answer'})
# x:30 y:458, x:130 y:458
_sm_confirm_order_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['orderList'])
with _sm_confirm_order_3:
# x:114 y:67
OperatableStateMachine.add('produce question',
CalculationState(calculation=lambda x: "Did you order :" + str(x[0].data)),
transitions={'done': 'Action_Ask'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'orderList', 'output_value': 'question'})
# x:423 y:171
OperatableStateMachine.add('Action_Ask',
self.use_behavior(Action_AskSM, 'ask and save order/confirm order/Action_Ask'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'question': 'question', 'answer': 'answer'})
# x:348 y:66, x:351 y:97, x:339 y:159, x:336 y:195, x:518 y:89, x:515 y:175
_sm_ask_and_look_person_4 = ConcurrencyContainer(outcomes=['finished', 'failed'], input_keys=['personID', 'question'], output_keys=['answer'], conditions=[
('finished', [('ask', 'finished')]),
('failed', [('ask', 'failed')]),
('finished', [('keep looking', 'finished')]),
('failed', [('keep looking', 'failed')])
])
with _sm_ask_and_look_person_4:
# x:30 y:40
OperatableStateMachine.add('ask',
_sm_ask_2,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'question': 'question', 'answer': 'answer'})
# x:30 y:138
OperatableStateMachine.add('keep looking',
_sm_keep_looking_1,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'personID': 'personID'})
# x:30 y:458
_sm_look_around_2_5 = OperatableStateMachine(outcomes=['finished'])
with _sm_look_around_2_5:
# x:215 y:156
OperatableStateMachine.add('center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w2'},
autonomy={'done': Autonomy.Off})
# x:394 y:262
OperatableStateMachine.add('right',
SaraSetHeadAngle(pitch=0.1, yaw=-1.5),
transitions={'done': 'w3'},
autonomy={'done': Autonomy.Off})
# x:206 y:372
OperatableStateMachine.add('center2',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w4'},
autonomy={'done': Autonomy.Off})
# x:38 y:596
OperatableStateMachine.add('w1',
WaitState(wait_time=4),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:413 y:159
OperatableStateMachine.add('w2',
WaitState(wait_time=4),
transitions={'done': 'right'},
autonomy={'done': Autonomy.Off})
# x:415 y:370
OperatableStateMachine.add('w3',
WaitState(wait_time=4),
transitions={'done': 'center2'},
autonomy={'done': Autonomy.Off})
# x:39 y:371
OperatableStateMachine.add('w4',
WaitState(wait_time=4),
transitions={'done': 'left'},
autonomy={'done': Autonomy.Off})
# x:25 y:482
OperatableStateMachine.add('left',
SaraSetHeadAngle(pitch=0.1, yaw=1.5),
transitions={'done': 'w1'},
autonomy={'done': Autonomy.Off})
# x:30 y:458
_sm_look_around_6 = OperatableStateMachine(outcomes=['finished'])
with _sm_look_around_6:
# x:215 y:156
OperatableStateMachine.add('center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w2'},
autonomy={'done': Autonomy.Off})
# x:394 y:262
OperatableStateMachine.add('right',
SaraSetHeadAngle(pitch=0.1, yaw=-1.5),
transitions={'done': 'w3'},
autonomy={'done': Autonomy.Off})
# x:206 y:372
OperatableStateMachine.add('center2',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'w4'},
autonomy={'done': Autonomy.Off})
# x:38 y:596
OperatableStateMachine.add('w1',
WaitState(wait_time=4),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:413 y:159
OperatableStateMachine.add('w2',
WaitState(wait_time=4),
transitions={'done': 'right'},
autonomy={'done': Autonomy.Off})
# x:415 y:370
OperatableStateMachine.add('w3',
WaitState(wait_time=4),
transitions={'done': 'center2'},
autonomy={'done': Autonomy.Off})
# x:39 y:371
OperatableStateMachine.add('w4',
WaitState(wait_time=4),
transitions={'done': 'left'},
autonomy={'done': Autonomy.Off})
# x:25 y:482
OperatableStateMachine.add('left',
SaraSetHeadAngle(pitch=0.1, yaw=1.5),
transitions={'done': 'w1'},
autonomy={'done': Autonomy.Off})
# x:30 y:458, x:130 y:458
_sm_turn_around_7 = OperatableStateMachine(outcomes=['finished', 'failed'])
with _sm_turn_around_7:
# x:80 y:26
OperatableStateMachine.add('look around',
_sm_look_around_6,
transitions={'finished': 'pose with 180 rotation'},
autonomy={'finished': Autonomy.Inherit})
# x:66 y:286
OperatableStateMachine.add('rotation',
SaraMoveBase(reference="base_link"),
transitions={'arrived': 'look around_2', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'pose': 'pose'})
# x:56 y:388
OperatableStateMachine.add('look around_2',
_sm_look_around_2_5,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit})
# x:63 y:133
OperatableStateMachine.add('pose with 180 rotation',
GenPoseEuler(x=0, y=0, z=0, roll=0, pitch=0, yaw=3.14),
transitions={'done': 'rotation'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'pose'})
# x:30 y:458
_sm_get_waving_people_8 = OperatableStateMachine(outcomes=['finished'], output_keys=['waving_person'])
with _sm_get_waving_people_8:
# x:47 y:45
OperatableStateMachine.add('set name',
SetKey(Value="person"),
transitions={'done': 'list people'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'name'})
# x:40 y:236
OperatableStateMachine.add('filter waving',
FilterKey(filter_function=lambda x: x[0].pose.right_arm_up or x[0].pose.left_arm_up, input_keys=["input_list"]),
transitions={'not_empty': 'get first', 'empty': 'list people'},
autonomy={'not_empty': Autonomy.Off, 'empty': Autonomy.Off},
remapping={'input_list': 'entity_list', 'output_list': 'waving_peoples'})
# x:53 y:326
OperatableStateMachine.add('get first',
CalculationState(calculation=lambda x: x[0]),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'waving_peoples', 'output_value': 'waving_person'})
# x:40 y:133
OperatableStateMachine.add('list people',
list_entities_by_name(frontality_level=0.5, distance_max=5),
transitions={'found': 'filter waving', 'none_found': 'list people'},
autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off},
remapping={'name': 'name', 'entity_list': 'entity_list', 'number': 'number'})
# x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458
_sm_look_for_waving_people_9 = ConcurrencyContainer(outcomes=['finished', 'failed'], output_keys=['waving_person'], conditions=[
('failed', [('turn around', 'failed')]),
('failed', [('turn around', 'finished')]),
('finished', [('get waving people', 'finished')])
])
with _sm_look_for_waving_people_9:
# x:268 y:118
OperatableStateMachine.add('get waving people',
_sm_get_waving_people_8,
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit},
remapping={'waving_person': 'waving_person'})
# x:60 y:126
OperatableStateMachine.add('turn around',
_sm_turn_around_7,
transitions={'finished': 'failed', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:441 y:583, x:93 y:568
_sm_repeate_if_first_commande_10 = OperatableStateMachine(outcomes=['finished', 'repeate'], input_keys=['commandNumber'], output_keys=['commandNumber'])
with _sm_repeate_if_first_commande_10:
# x:242 y:97
OperatableStateMachine.add('if first command',
CheckConditionState(predicate=lambda x: x == 1),
transitions={'true': 'set second command', 'false': 'finished'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'commandNumber'})
# x:73 y:329
OperatableStateMachine.add('set second command',
SetKey(Value=2),
transitions={'done': 'repeate'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'commandNumber'})
# x:1053 y:687, x:1068 y:149
_sm_take_objects_and_bring_the_order_to_customer_11 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['barPosition', 'orderList', 'robotPositionToCustomer'])
with _sm_take_objects_and_bring_the_order_to_customer_11:
# x:73 y:26
OperatableStateMachine.add('set indexkey',
SetKey(Value=0),
transitions={'done': 'one element by one element from the list'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'indexKey'})
# x:591 y:684
OperatableStateMachine.add('increment indexKey',
CalculationState(calculation=lambda x: x+1),
transitions={'done': 'check if end of the list'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'indexKey', 'output_value': 'indexKey'})
# x:796 y:679
OperatableStateMachine.add('check if end of the list',
FlexibleCheckConditionState(predicate=lambda x: len(x[0]) <= x[1], input_keys=["orderList", "indexKey"]),
transitions={'true': 'finished', 'false': 'Action_Move_2'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'orderList': 'orderList', 'indexKey': 'indexKey'})
# x:31 y:243
OperatableStateMachine.add('Action_find',
self.use_behavior(Action_findSM, 'take objects and bring the order to customer/Action_find'),
transitions={'done': 'get entity ID', 'failed': 'say cannot find'},
autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'className': 'item', 'entity': 'entity'})
# x:76 y:183
OperatableStateMachine.add('say search and grip',
SaraSay(sentence=lambda x: "I am now searching for the "+x[0]+".", input_keys=["item"], emotion=0, block=True),
transitions={'done': 'Action_find'},
autonomy={'done': Autonomy.Off},
remapping={'item': 'item'})
# x:258 y:261
OperatableStateMachine.add('say cannot find',
SaraSay(sentence="I cannot find the item.", input_keys=[], emotion=0, block=True),
transitions={'done': 'place arm'},
autonomy={'done': Autonomy.Off})
# x:63 y:393
OperatableStateMachine.add('Action_pick',
self.use_behavior(Action_pickSM, 'take objects and bring the order to customer/Action_pick'),
transitions={'success': 'say go to the customer', 'unreachable': 'say cannot pick', 'not found': 'say cannot pick', 'dropped': 'say cannot pick'},
autonomy={'success': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'not found': Autonomy.Inherit, 'dropped': Autonomy.Inherit},
remapping={'objectID': 'entityID'})
# x:61 y:548
OperatableStateMachine.add('Action_Move',
self.use_behavior(Action_MoveSM, 'take objects and bring the order to customer/Action_Move'),
transitions={'finished': 'find table and place', 'failed': 'say cant get back to customer'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'robotPositionToCustomer'})
# x:83 y:326
OperatableStateMachine.add('get entity ID',
CalculationState(calculation=lambda x: x.ID),
transitions={'done': 'Action_pick'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'entity', 'output_value': 'entityID'})
# x:568 y:407
OperatableStateMachine.add('say put in the gripper',
SaraSay(sentence=lambda x: "Please, put the "+x[0]+" in my gripper. I will close it in 3, 2, 1, now.", input_keys=["item"], emotion=0, block=True),
transitions={'done': 'close gripper'},
autonomy={'done': Autonomy.Off},
remapping={'item': 'item'})
# x:243 y:387
OperatableStateMachine.add('say cannot pick',
SaraSay(sentence="I can not pick the item.", input_keys=[], emotion=0, block=True),
transitions={'done': 'place arm'},
autonomy={'done': Autonomy.Off})
# x:388 y:502
OperatableStateMachine.add('say thank you',
SaraSay(sentence="Thank you.", input_keys=[], emotion=0, block=True),
transitions={'done': 'place arm 2'},
autonomy={'done': Autonomy.Off})
# x:72 y:465
OperatableStateMachine.add('say go to the customer',
SaraSay(sentence="I will serve it to the customer.", input_keys=[], emotion=0, block=True),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:522 y:505
OperatableStateMachine.add('close gripper',
SetGripperState(width=0, effort=1),
transitions={'object': 'say thank you', 'no_object': 'open gripper'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:357 y:569
OperatableStateMachine.add('say cant get back to customer',
SaraSay(sentence="I am not able to go back to the customer.", input_keys=[], emotion=0, block=True),
transitions={'done': 'increment indexKey'},
autonomy={'done': Autonomy.Off})
# x:541 y:317
OperatableStateMachine.add('open gripper',
SetGripperState(width=0.10, effort=1),
transitions={'object': 'say put in the gripper', 'no_object': 'say put in the gripper'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:63 y:110
OperatableStateMachine.add('one element by one element from the list',
FlexibleCalculationState(calculation=lambda x: x[0][x[1]].data, input_keys=["orderList", "indexKey"]),
transitions={'done': 'say search and grip'},
autonomy={'done': Autonomy.Off},
remapping={'orderList': 'orderList', 'indexKey': 'indexKey', 'output_value': 'item'})
# x:376 y:324
OperatableStateMachine.add('place arm',
RunTrajectory(file="receive_object", duration=0),
transitions={'done': 'open gripper'},
autonomy={'done': Autonomy.Off})
# x:638 y:184
OperatableStateMachine.add('Action_Move_2',
self.use_behavior(Action_MoveSM, 'take objects and bring the order to customer/Action_Move_2'),
transitions={'finished': 'increment indexKey', 'failed': 'say cannot go back to bar'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'barPosition'})
# x:817 y:178
OperatableStateMachine.add('say cannot go back to bar',
SaraSay(sentence="I am not able to go back to the barman. I will try a new command.", input_keys=[], emotion=0, block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:85 y:740
OperatableStateMachine.add('find table and place',
_sm_find_table_and_place_0,
transitions={'finished': 'retour repos', 'failed': 'retour repos', 'no_table': 'retour repos'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'no_table': Autonomy.Inherit})
# x:248 y:505
OperatableStateMachine.add('place arm 2',
RunTrajectory(file="transport", duration=0),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:362 y:679
OperatableStateMachine.add('retour repos',
RunTrajectory(file="repos", duration=0),
transitions={'done': 'increment indexKey'},
autonomy={'done': Autonomy.Off})
# x:242 y:352, x:1086 y:243
_sm_get_the_order_12 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['orderList'])
with _sm_get_the_order_12:
# x:68 y:39
OperatableStateMachine.add('length 1',
CheckConditionState(predicate=lambda x: len(x) == 1),
transitions={'true': 'say order', 'false': 'length 2'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'orderList'})
# x:236 y:41
OperatableStateMachine.add('length 2',
CheckConditionState(predicate=lambda x: len(x) ==2),
transitions={'true': 'say order_2', 'false': 'say order_3'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'orderList'})
# x:228 y:111
OperatableStateMachine.add('say order_2',
SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+" and one "+x[0][1].data+", please.", input_keys=["orderList"], emotion=0, block=True),
transitions={'done': 'wait 5'},
autonomy={'done': Autonomy.Off},
remapping={'orderList': 'orderList'})
# x:374 y:109
OperatableStateMachine.add('say order_3',
SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+", one "+x[0][1].data+" and one "+x[0][2].data+", please.", input_keys=["orderList"], emotion=0, block=True),
transitions={'done': 'wait 5'},
autonomy={'done': Autonomy.Off},
remapping={'orderList': 'orderList'})
# x:67 y:117
OperatableStateMachine.add('say order',
SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+", please.", input_keys=["orderList"], emotion=0, block=True),
transitions={'done': 'wait 5'},
autonomy={'done': Autonomy.Off},
remapping={'orderList': 'orderList'})
# x:224 y:256
OperatableStateMachine.add('wait 5',
WaitState(wait_time=0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:216 y:528, x:572 y:238
_sm_go_to_the_barman_13 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['barPosition', 'barmanID'])
with _sm_go_to_the_barman_13:
# x:30 y:102
OperatableStateMachine.add('Action_Move',
self.use_behavior(Action_MoveSM, 'go to the barman/Action_Move'),
transitions={'finished': 'if barman id is 0', 'failed': 'say cannot'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'barPosition'})
# x:365 y:99
OperatableStateMachine.add('say cannot',
SaraSay(sentence="I can not reach my destination. I will take another order. If you have one, please raise your hand.", input_keys=[], emotion=0, block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:30 y:320
OperatableStateMachine.add('Action_findPersonByID',
self.use_behavior(Action_findPersonByIDSM, 'go to the barman/Action_findPersonByID',
default_keys=['className']),
transitions={'found': 'finished', 'not_found': 'finished'},
autonomy={'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit},
remapping={'personID': 'barmanID', 'personEntity': 'personEntity'})
# x:115 y:230
OperatableStateMachine.add('if barman id is 0',
CheckConditionState(predicate=lambda x: x == 0),
transitions={'true': 'finished', 'false': 'Action_findPersonByID'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'barmanID'})
# x:898 y:561, x:871 y:123
_sm_ask_and_save_order_14 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['customerID'], output_keys=['orderList'])
with _sm_ask_and_save_order_14:
# x:70 y:112
OperatableStateMachine.add('say ready',
SaraSay(sentence="Hello.", input_keys=[], emotion=0, block=True),
transitions={'done': 'set question'},
autonomy={'done': Autonomy.Off})
# x:69 y:187
OperatableStateMachine.add('set question',
SetKey(Value="What do you want to order?"),
transitions={'done': 'ask AND look person'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'question'})
# x:411 y:138
OperatableStateMachine.add('say cannot',
SaraSay(sentence="I am not able to understand your order. I will take another order.", input_keys=[], emotion=0, block=True),
transitions={'done': 'set question'},
autonomy={'done': Autonomy.Off})
# x:59 y:272
OperatableStateMachine.add('ask AND look person',
_sm_ask_and_look_person_4,
transitions={'finished': 'nlu restaurant', 'failed': 'say cannot'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'personID': 'customerID', 'question': 'question', 'answer': 'answer'})
# x:411 y:453
OperatableStateMachine.add('nlu restaurant',
SaraNLUrestaurant(),
transitions={'understood': 'confirm order', 'fail': 'say cannot'},
autonomy={'understood': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'sentence': 'answer', 'orderList': 'orderList'})
# x:657 y:417
OperatableStateMachine.add('confirm order',
_sm_confirm_order_3,
transitions={'finished': 'say bring', 'failed': 'set question'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'orderList': 'orderList'})
# x:852 y:402
OperatableStateMachine.add('say bring',
SaraSay(sentence=lambda x: "understood. I will bring you the "+str(x[0][0].data), input_keys=["orderList"], emotion=0, block=False),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'orderList': 'orderList'})
# x:677 y:318, x:735 y:160
_sm_move_to_table_and_save_position_15 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['customerPosition'], output_keys=['robotPositionToCustomer'])
with _sm_move_to_table_and_save_position_15:
# x:69 y:24
OperatableStateMachine.add('set distance to person',
SetKey(Value=0.7),
transitions={'done': 'compute robot pose to customer'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distanceToPerson'})
# x:344 y:115
OperatableStateMachine.add('say cannot',
SaraSay(sentence="I can not reach my destination. I will take another order. If you have one, please raise your hand.", input_keys=[], emotion=0, block=True),
transitions={'done': 'save pose to not get flexbe problems'},
autonomy={'done': Autonomy.Off})
# x:497 y:129
OperatableStateMachine.add('save pose to not get flexbe problems',
Get_Robot_Pose(),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'pose'})
# x:47 y:98
OperatableStateMachine.add('compute robot pose to customer',
Get_Reacheable_Waypoint(),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off},
remapping={'pose_in': 'customerPosition', 'distance': 'distanceToPerson', 'pose_out': 'robotPositionToCustomer'})
# x:78 y:199
OperatableStateMachine.add('Action_Move',
self.use_behavior(Action_MoveSM, 'move to table and save position/Action_Move'),
transitions={'finished': 'finished', 'failed': 'say cannot'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'robotPositionToCustomer'})
# x:112 y:525, x:363 y:46
_sm_detect_people_waving_16 = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['customerPosition', 'customerID'])
with _sm_detect_people_waving_16:
# x:84 y:25
OperatableStateMachine.add('say looking',
SaraSay(sentence="I am now looking for customers. If you want to order something, please raise your hand.", input_keys=[], emotion=0, block=False),
transitions={'done': 'Look for waving people'},
autonomy={'done': Autonomy.Off})
# x:73 y:375
OperatableStateMachine.add('reachable position_2',
Get_Reacheable_Waypoint(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'pose_in': 'customerPosition', 'distance': 'distance', 'pose_out': 'customerPosition'})
# x:71 y:291
OperatableStateMachine.add('set distance to customer_2',
SetKey(Value=0.5),
transitions={'done': 'reachable position_2'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distance'})
# x:94 y:216
OperatableStateMachine.add('get attributes',
GetAttribute(attributes=["ID", "position"]),
transitions={'done': 'set distance to customer_2'},
autonomy={'done': Autonomy.Off},
remapping={'object': 'waving_person', 'ID': 'customerID', 'position': 'customerPosition'})
# x:63 y:120
OperatableStateMachine.add('Look for waving people',
_sm_look_for_waving_people_9,
transitions={'finished': 'get attributes', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'waving_person': 'waving_person'})
# x:874 y:499, x:747 y:100
_sm_save_bar_position_and_initiation_17 = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['barPosition', 'commandNumber', 'barmanID'])
with _sm_save_bar_position_and_initiation_17:
# x:66 y:96
OperatableStateMachine.add('set question barman',
SetKey(Value="Are you the bartender?"),
transitions={'done': 'Action_FindPersonByQuestion'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'questionBarman'})
# x:488 y:85
OperatableStateMachine.add('say failed to find the barman',
SaraSay(sentence="I am not able to find the barman. I will get the order and come back here to get the items.", input_keys=[], emotion=0, block=True),
transitions={'done': 'barman ID to 0'},
autonomy={'done': Autonomy.Off})
# x:83 y:406
OperatableStateMachine.add('save barman ID',
CalculationState(calculation=lambda x: x.ID),
transitions={'done': 'get current pose'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'personFound', 'output_value': 'barmanID'})
# x:512 y:183
OperatableStateMachine.add('barman ID to 0',
SetKey(Value=0),
transitions={'done': 'get current pose'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'barmanID'})
# x:488 y:391
OperatableStateMachine.add('get current pose',
Get_Robot_Pose(),
transitions={'done': 'set first command'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'barPosition'})
# x:55 y:214
OperatableStateMachine.add('Action_FindPersonByQuestion',
self.use_behavior(Action_FindPersonByQuestionSM, 'save bar position and initiation/Action_FindPersonByQuestion'),
transitions={'found': 'save barman ID', 'failed': 'say failed to find the barman', 'not_found': 'say failed to find the barman'},
autonomy={'found': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'not_found': Autonomy.Inherit},
remapping={'question': 'questionBarman', 'entityFound': 'personFound'})
# x:489 y:478
OperatableStateMachine.add('set first command',
SetKey(Value=1),
transitions={'done': 'say instructions'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'commandNumber'})
# x:689 y:477
OperatableStateMachine.add('say instructions',
SaraSay(sentence="Good! I wil now start to serve people.", input_keys=[], emotion=0, block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('save bar position and initiation',
_sm_save_bar_position_and_initiation_17,
transitions={'finished': 'detect people waving', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'barPosition': 'barPosition', 'commandNumber': 'commandNumber', 'barmanID': 'barmanID'})
# x:131 y:141
OperatableStateMachine.add('detect people waving',
_sm_detect_people_waving_16,
transitions={'finished': 'move to table and save position', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'customerPosition': 'customerPosition', 'customerID': 'customerID'})
# x:233 y:250
OperatableStateMachine.add('move to table and save position',
_sm_move_to_table_and_save_position_15,
transitions={'finished': 'ask and save order', 'failed': 'detect people waving'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'customerPosition': 'customerPosition', 'robotPositionToCustomer': 'robotPositionToCustomer'})
# x:286 y:337
OperatableStateMachine.add('ask and save order',
_sm_ask_and_save_order_14,
transitions={'finished': 'go to the barman', 'failed': 'detect people waving'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'customerID': 'customerID', 'orderList': 'orderList'})
# x:264 y:434
OperatableStateMachine.add('go to the barman',
_sm_go_to_the_barman_13,
transitions={'finished': 'get the order', 'failed': 'detect people waving'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'barPosition': 'barPosition', 'barmanID': 'barmanID'})
# x:278 y:526
OperatableStateMachine.add('get the order',
_sm_get_the_order_12,
transitions={'finished': 'take objects and bring the order to customer', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'orderList': 'orderList'})
# x:260 y:626
OperatableStateMachine.add('take objects and bring the order to customer',
_sm_take_objects_and_bring_the_order_to_customer_11,
transitions={'finished': 'repeate if first commande', 'failed': 'repeate if first commande'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'barPosition': 'barPosition', 'orderList': 'orderList', 'robotPositionToCustomer': 'robotPositionToCustomer'})
# x:118 y:735
OperatableStateMachine.add('repeate if first commande',
_sm_repeate_if_first_commande_10,
transitions={'finished': 'say finish', 'repeate': 'detect people waving'},
autonomy={'finished': Autonomy.Inherit, 'repeate': Autonomy.Inherit},
remapping={'commandNumber': 'commandNumber'})
# x:431 y:743
OperatableStateMachine.add('say finish',
SaraSay(sentence="I reach the end of my scenario. I will go back to the bar.", input_keys=[], emotion=0, block=True),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:616 y:735
OperatableStateMachine.add('Action_Move',
self.use_behavior(Action_MoveSM, 'Action_Move'),
transitions={'finished': 'finished', 'failed': 'finished'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'barPosition'})
return _state_machine
def create(self):
# x:831 y:173, x:520 y:222, x:335 y:32, x:701 y:385
_state_machine = OperatableStateMachine(
outcomes=['success', 'unreachable', 'not found', 'dropped'],
input_keys=['objectID', 'Entity'])
_state_machine.userdata.objectID = 1585
_state_machine.userdata.PreGripPose = "PreGripPose"
_state_machine.userdata.PostGripPose = "PostGripPose"
_state_machine.userdata.Entity = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:130 y:365, x:230 y:365
_sm_look_at_it_for_3s_0 = ConcurrencyContainer(
outcomes=['done'],
input_keys=['ID'],
conditions=[('done', [('look', 'failed')]),
('done', [('wait', 'done')])])
with _sm_look_at_it_for_3s_0:
# x:30 y:54
OperatableStateMachine.add('look',
KeepLookingAt(),
transitions={'failed': 'done'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:187 y:111
OperatableStateMachine.add('wait',
WaitState(wait_time=3),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:30 y:324
_sm_getting_close_1 = OperatableStateMachine(outcomes=['done'],
input_keys=['pose_app'])
with _sm_getting_close_1:
# x:36 y:40
OperatableStateMachine.add(
'gripperopen',
SetGripperState(width=0.15, effort=1),
transitions={
'object': 'move_approach',
'no_object': 'move_approach'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:30 y:129
OperatableStateMachine.add('move_approach',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'almost have it',
'failed': 'almost have it'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'pose_app'})
# x:39 y:204
OperatableStateMachine.add('almost have it',
SaraSay(sentence="I am close",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:30 y:324, x:130 y:324
_sm_get_away_from_failure_2 = OperatableStateMachine(
outcomes=['done', 'failed'], input_keys=['PostGripPose'])
with _sm_get_away_from_failure_2:
# x:30 y:40
OperatableStateMachine.add(
'open 2',
SetGripperState(width=0.1, effort=1),
transitions={
'object': 'move back',
'no_object': 'move back'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:235 y:50
OperatableStateMachine.add('move back',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PostGripPose'})
# x:30 y:324, x:130 y:324
_sm_lift_object_3 = OperatableStateMachine(
outcomes=['done', 'failed'], input_keys=['pose_lift', 'pose_ret'])
with _sm_lift_object_3:
# x:30 y:176
OperatableStateMachine.add('move_lift_object',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'genpose',
'failed': 'genpose'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'pose_lift'})
# x:228 y:176
OperatableStateMachine.add('move_Return',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'pose_ret'})
# x:38 y:45
OperatableStateMachine.add('genpose',
GenPoseEuler(x=-0.2,
y=0,
z=0,
roll=0,
pitch=0,
yaw=0),
transitions={'done': 'move_back'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'backPose'})
# x:211 y:40
OperatableStateMachine.add('move_back',
SaraMoveBase(reference="base_link"),
transitions={
'arrived': 'move_Return',
'failed': 'move_Return'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'backPose'})
# x:499 y:227, x:263 y:214, x:271 y:492
_sm_pregrip_4 = OperatableStateMachine(
outcomes=['fail', 'failed', 'done'],
input_keys=['PreGripPose', 'posobjet'],
output_keys=['pose_app', 'grippose', 'pose_lift', 'pose_ret'])
with _sm_pregrip_4:
# x:70 y:40
OperatableStateMachine.add('gen_gripPose',
GenGripperPose(l=0.0, z=0, planar=True),
transitions={
'done': 'checkifposeaccess',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'posobjet',
'pose_out': 'grippose'
})
# x:56 y:256
OperatableStateMachine.add('move_PreGrip',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'gen_returnPose',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PreGripPose'})
# x:30 y:398
OperatableStateMachine.add('gen_approachPose',
GenGripperPose(l=0.15, z=0,
planar=True),
transitions={
'done': 'gen_liftPose',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'posobjet',
'pose_out': 'pose_app'
})
# x:40 y:469
OperatableStateMachine.add('gen_liftPose',
GenGripperPose(l=0, z=0.1, planar=True),
transitions={
'done': 'done',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'posobjet',
'pose_out': 'pose_lift'
})
# x:43 y:326
OperatableStateMachine.add('gen_returnPose',
GenGripperPose(l=0.2,
z=0.1,
planar=True),
transitions={
'done': 'gen_approachPose',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'posobjet',
'pose_out': 'pose_ret'
})
# x:63 y:184
OperatableStateMachine.add('say can reach',
SaraSay(sentence="I will grab it",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'move_PreGrip'},
autonomy={'done': Autonomy.Off})
# x:58 y:114
OperatableStateMachine.add('checkifposeaccess',
MoveitMove(move=False,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'say can reach',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'grippose'})
# x:315 y:40, x:130 y:465
_sm_get_object_5 = OperatableStateMachine(
outcomes=['not_found', 'finished'],
input_keys=['objectID'],
output_keys=['posobjet'])
with _sm_get_object_5:
# x:55 y:40
OperatableStateMachine.add('getobject',
GetEntityByID(),
transitions={
'found': 'Say_See_It',
'not_found': 'not_found'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'objectID',
'Entity': 'Entity'
})
# x:77 y:370
OperatableStateMachine.add(
'getpose',
CalculationState(calculation=lambda x: x.position),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'Entity',
'output_value': 'posobjet'
})
# x:71 y:262
OperatableStateMachine.add('Look at it for 3s',
_sm_look_at_it_for_3s_0,
transitions={'done': 'getpose'},
autonomy={'done': Autonomy.Inherit},
remapping={'ID': 'objectID'})
# x:31 y:133
OperatableStateMachine.add(
'Say_See_It',
SaraSay(sentence=lambda x: "I see the " + x.name,
input_keys=["Entity"],
emotion=0,
block=True),
transitions={'done': 'Look at it for 3s'},
autonomy={'done': Autonomy.Off},
remapping={'Entity': 'Entity'})
with _state_machine:
# x:77 y:23
OperatableStateMachine.add('Get object',
_sm_get_object_5,
transitions={
'not_found': 'not found',
'finished': 'transform point'
},
autonomy={
'not_found': Autonomy.Inherit,
'finished': Autonomy.Inherit
},
remapping={
'objectID': 'objectID',
'posobjet': 'posobjet'
})
# x:258 y:385
OperatableStateMachine.add(
'gripclose',
SetGripperState(width=0, effort=250),
transitions={
'object': 'say picked',
'no_object': 'get away from failure'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:53 y:374
OperatableStateMachine.add('move_on_object',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'gripclose',
'failed': 'cant reach'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'grippose'})
# x:650 y:290
OperatableStateMachine.add('move_PostGrip',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'welcome',
'failed': 'unreachable'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PostGripPose'})
# x:277 y:200
OperatableStateMachine.add('cant reach',
SaraSay(
sentence="Hum. I can't reach it.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'unreachable'},
autonomy={'done': Autonomy.Off})
# x:265 y:291
OperatableStateMachine.add('say picked',
SaraSay(sentence="I think I got it",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'Lift object'},
autonomy={'done': Autonomy.Off})
# x:837 y:291
OperatableStateMachine.add('welcome',
SaraSay(sentence="you are welcome",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'success'},
autonomy={'done': Autonomy.Off})
# x:62 y:190
OperatableStateMachine.add('PreGrip',
_sm_pregrip_4,
transitions={
'fail': 'cant reach',
'failed': 'cant reach',
'done': 'Getting close'
},
autonomy={
'fail': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'PreGripPose': 'PreGripPose',
'posobjet': 'posobjet',
'pose_app': 'pose_app',
'grippose': 'grippose',
'pose_lift': 'pose_lift',
'pose_ret': 'pose_ret'
})
# x:67 y:114
OperatableStateMachine.add('transform point',
TF_transformation(in_ref="map",
out_ref="base_link"),
transitions={
'done': 'PreGrip',
'fail': 'Get object'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'in_pos': 'posobjet',
'out_pos': 'posobjet'
})
# x:439 y:287
OperatableStateMachine.add('Lift object',
_sm_lift_object_3,
transitions={
'done': 'move_PostGrip',
'failed': 'move_PostGrip'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'pose_lift': 'pose_lift',
'pose_ret': 'pose_ret'
})
# x:441 y:391
OperatableStateMachine.add(
'get away from failure',
_sm_get_away_from_failure_2,
transitions={
'done': 'dropped',
'failed': 'dropped'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'PostGripPose': 'PostGripPose'})
# x:47 y:279
OperatableStateMachine.add('Getting close',
_sm_getting_close_1,
transitions={'done': 'move_on_object'},
autonomy={'done': Autonomy.Inherit},
remapping={'pose_app': 'pose_app'})
return _state_machine
def create(self):
# x:194 y:348, x:15 y:144
_state_machine = OperatableStateMachine(outcomes=['Done', 'Fail'],
input_keys=['DoorName'])
_state_machine.userdata.DoorName = "door"
_state_machine.userdata.otherSide = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:324
_sm_manage_door_opening_0 = OperatableStateMachine(
outcomes=['done'], output_keys=['otherSide'])
with _sm_manage_door_opening_0:
# x:41 y:40
OperatableStateMachine.add('Wait for door 1',
DoorDetector(timeout=1),
transitions={
'done': 'genfront',
'failed': 'door closed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
})
# x:199 y:43
OperatableStateMachine.add(
'door closed',
SaraSay(sentence="This door is closed. I need an human.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'wait for door 2'},
autonomy={'done': Autonomy.Off})
# x:30 y:151
OperatableStateMachine.add('genfront',
GenPoseEuler(x=1.5,
y=0,
z=0,
roll=0,
pitch=0,
yaw=0),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'otherSide'})
# x:357 y:44
OperatableStateMachine.add('wait for door 2',
DoorDetector(timeout=5),
transitions={
'done': 'Thanks',
'failed': 'callhelp'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
})
# x:534 y:45
OperatableStateMachine.add(
'callhelp',
SaraSay(sentence="Can someone open this door for me. Please.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'wait for door 2'},
autonomy={'done': Autonomy.Off})
# x:348 y:156
OperatableStateMachine.add('Thanks',
SaraSay(sentence="Thank you!",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'genfront'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:155 y:30
OperatableStateMachine.add(
'Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM,
'Action_Move'),
transitions={
'finished': 'Manage door opening',
'failed': 'Fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pose': 'DoorName'})
# x:154 y:132
OperatableStateMachine.add(
'Manage door opening',
_sm_manage_door_opening_0,
transitions={'done': 'move to the other side'},
autonomy={'done': Autonomy.Inherit},
remapping={'otherSide': 'otherSide'})
# x:160 y:234
OperatableStateMachine.add('move to the other side',
SaraMoveBase(reference="base_link"),
transitions={
'arrived': 'Done',
'failed': 'Fail'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'otherSide'})
return _state_machine
def create(self):
# x:938 y:315, x:130 y:302
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['name', 'index'])
_state_machine.userdata.name = ""
_state_machine.userdata.index = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:236 y:55
OperatableStateMachine.add(
'Wait_time',
WaitState(wait_time=3),
transitions={'done': 'Wonderland_Get_Entity'},
autonomy={'done': Autonomy.Off})
# x:748 y:182
OperatableStateMachine.add('Move to object',
SaraMoveBase(reference="map"),
transitions={
'arrived': 'finished',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'waypoint_pose'})
# x:685 y:73
OperatableStateMachine.add(
'read',
Wonderland_Read_Entity(index_function=lambda x: 0),
transitions={
'done': 'Move to object',
'empty': 'failed',
'error': 'failed'
},
autonomy={
'done': Autonomy.Off,
'empty': Autonomy.Off,
'error': Autonomy.Off
},
remapping={
'json_text': 'entity',
'input_value': 'name',
'id': 'id',
'name': 'name',
'time': 'time',
'x_pos': 'x_pos',
'y_pos': 'y_pos',
'z_pos': 'z_pos',
'waypoint_id': 'waypoint_id',
'waypoint_pose': 'waypoint_pose'
})
return _state_machine