def create(self):
# x:30 y:365, x:150 y:452
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:283 y:40
OperatableStateMachine.add('say',
Talk_Last_Response(target_time=1),
transitions={'continue': 'sub', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:457 y:297
OperatableStateMachine.add('logfalse',
LogState(text="false", severity=Logger.REPORT_HINT),
transitions={'done': 'sub'},
autonomy={'done': Autonomy.Off})
# x:391 y:168
OperatableStateMachine.add('unavailable_topic_string_log',
LogState(text="the topic is not available so finishing", severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:973 y:355
OperatableStateMachine.add('takephoto',
TakePhotoState(),
transitions={'continue': 'move_lips', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:975 y:444
OperatableStateMachine.add('move_lips',
MoveRobotLipsState(target_time=0),
transitions={'continue': 'finished', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:776 y:71
OperatableStateMachine.add('sub',
SubscriberState(topic='/stt_topic', blocking=True, clear=False),
transitions={'received': 'chyeck', 'unavailable': 'unavailable_topic_string_log'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'message'})
# x:778 y:303
OperatableStateMachine.add('chyeck',
CheckConditionState(predicate=lambda message: message.query == "take photo"),
transitions={'true': 'takephoto', 'false': 'logfalse'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'message'})
return _state_machine
def create(self):
hello = "Hello World!"
# x:30 y:365, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:129 y:81
OperatableStateMachine.add('Initial_Wait',
WaitState(wait_time=self.waiting_time),
transitions={'done': 'Hello_Greeting'},
autonomy={'done': Autonomy.Off})
# x:130 y:215
OperatableStateMachine.add('Hello_Greeting',
LogState(text=hello,
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.High})
return _state_machine
def create(self):
log_msg = "Hello World!"
# x:83 y:390
_state_machine = OperatableStateMachine(outcomes=['finished'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:52 y:78
OperatableStateMachine.add(
'Print_Message',
LogState(text=log_msg, severity=Logger.REPORT_HINT),
transitions={'done': 'Wait_After_Logging'},
autonomy={'done': Autonomy.Low})
# x:40 y:228
OperatableStateMachine.add('Wait_After_Logging',
WaitState(wait_time=self.waiting_time),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:83 y:340, x:283 y:190
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:60 y:78
OperatableStateMachine.add(
'Log_Sweep',
LogState(text='Now performing a short sweep...',
severity=Logger.REPORT_INFO),
transitions={'done': 'Perform_Sweep'},
autonomy={'done': Autonomy.Off})
# x:47 y:178
OperatableStateMachine.add('Perform_Sweep',
MetricSweepState(sweep_type='short'),
transitions={
'sweeped': 'finished',
'failed': 'failed'
},
autonomy={
'sweeped': Autonomy.Off,
'failed': Autonomy.Off
})
return _state_machine
def create(self):
hello = "hello world"
# x:248 y:340, x:429 y:201
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:182 y:32
OperatableStateMachine.add('Initial_Wait',
WaitState(wait_time=self.waiting_time),
transitions={'done': 'Print_Greeting'},
autonomy={'done': Autonomy.Off})
# x:176 y:186
OperatableStateMachine.add('Print_Greeting',
LogState(text=hello,
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.High})
return _state_machine
def create(self):
# x:909 y:103, x:830 y:367
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:105 y:53
OperatableStateMachine.add('Entry',
LogState(text="Entering the create twist demo", severity=Logger.REPORT_HINT),
transitions={'done': 'Twist1'},
autonomy={'done': Autonomy.Off})
# x:577 y:345
OperatableStateMachine.add('EmergencyStop',
CreateTimedStopState(timeout=0.2, cmd_topic='/create_node/cmd_vel', odom_topic='/create_node/odom'),
transitions={'done': 'failed', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off})
# x:577 y:145
OperatableStateMachine.add('Stop',
CreateTimedStopState(timeout=0.24, cmd_topic='/create_node/cmd_vel', odom_topic='/create_node/odom'),
transitions={'done': 'Exit', 'failed': 'EmergencyStop'},
autonomy={'done': Autonomy.High, 'failed': Autonomy.Off})
# x:756 y:65
OperatableStateMachine.add('Exit',
LogState(text="Exiting the create twist demo", severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.High})
# x:73 y:166
OperatableStateMachine.add('Twist1',
CreateTimedTwistState(target_time=10.0, velocity=0.25, rotation_rate=0.05, cmd_topic='/create_node/cmd_vel', sensor_topic='/create_node/sensor_state'),
transitions={'done': 'Stop', 'failed': 'EmergencyStop'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Off})
return _state_machine
def create(self):
hello = "Hello World"
# x:30 y:365, x:332 y:473
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:131 y:60
OperatableStateMachine.add(
'waiter',
WaitState(wait_time=self.waiting_time),
transitions={'done': 'listen for input'},
autonomy={'done': Autonomy.Off})
# x:633 y:55
OperatableStateMachine.add('printer',
LogState(text=hello,
severity=Logger.REPORT_HINT),
transitions={'done': 'phototaker'},
autonomy={'done': Autonomy.Off})
# x:567 y:320
OperatableStateMachine.add('phototaker',
TakePhotoState(),
transitions={
'continue': 'finished',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:376 y:48
OperatableStateMachine.add('listen for input',
ListeningState(),
transitions={
'continue': 'printer',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
return _state_machine
def create(self):
hello = "baka!!"
# x:30 y:365, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.target_joint = [0, 0, 0]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Initial_Wait',
WaitState(wait_time=self.waiting_time),
transitions={'done': 'test_moveit'},
autonomy={'done': Autonomy.Off})
# x:27 y:171
OperatableStateMachine.add('Print_Greeting',
LogState(text=hello,
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.High})
# x:172 y:148
OperatableStateMachine.add(
'test_moveit',
MoveitToJointsState(move_group='Gantry',
joint_names=[
"small_long_joint", "torso_rail_joint",
"torso_base_main_joint"
],
action_topic='/ariac/gantry/move_group'),
transitions={
'reached': 'Print_Greeting',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={'joint_config': 'target_joint'})
return _state_machine
def create(self):
# x:255 y:192, x:130 y:324
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:72 y:118
OperatableStateMachine.add('log',
LogState(text="test",
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:30 y:365
_state_machine = OperatableStateMachine(outcomes=['finished'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:66 y:154
OperatableStateMachine.add('Print_Msg',
LogState(text="My param is: " + self.msg, severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:682 y:306, x:452 y:252
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['pos'])
_state_machine.userdata.pos = {"x": 0.8, "y": -0.2, "z": 1}
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458, x:530 y:458, x:630 y:458, x:59 y:533, x:830 y:458
_sm_group_0 = ConcurrencyContainer(
outcomes=['threshold', 'watchdog', 'fail'],
conditions=[('threshold', [('read', 'threshold')]),
('watchdog', [('read', 'watchdog')]),
('fail', [('read', 'fail')]),
('threshold', [('read yaw', 'threshold')]),
('fail', [('read yaw', 'fail')]),
('watchdog', [('read yaw', 'watchdog')])])
with _sm_group_0:
# x:86 y:125
OperatableStateMachine.add('read',
ReadTorque(
watchdog=1,
Joint="right_elbow_pitch_joint",
Threshold=0.7,
min_time=0.4),
transitions={
'threshold': 'threshold',
'watchdog': 'watchdog',
'fail': 'fail'
},
autonomy={
'threshold': Autonomy.Off,
'watchdog': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'torque': 'torque'})
# x:252 y:135
OperatableStateMachine.add('read yaw',
ReadTorque(
watchdog=1,
Joint="right_elbow_pitch_joint",
Threshold=0.5,
min_time=0.4),
transitions={
'threshold': 'threshold',
'watchdog': 'watchdog',
'fail': 'fail'
},
autonomy={
'threshold': Autonomy.Off,
'watchdog': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'torque': 'torque'})
# x:30 y:458
_sm_read_torque_1 = OperatableStateMachine(outcomes=['done'])
with _sm_read_torque_1:
# x:142 y:61
OperatableStateMachine.add('log',
LogState(text="going down",
severity=Logger.REPORT_HINT),
transitions={'done': 'Group'},
autonomy={'done': Autonomy.Off})
# x:131 y:164
OperatableStateMachine.add('Group',
_sm_group_0,
transitions={
'threshold': 'done',
'watchdog': 'log',
'fail': 'done'
},
autonomy={
'threshold': Autonomy.Inherit,
'watchdog': Autonomy.Inherit,
'fail': Autonomy.Inherit
})
# x:30 y:458
_sm_go_down_2 = OperatableStateMachine(outcomes=['done'],
input_keys=['GripPose'])
with _sm_go_down_2:
# x:126 y:194
OperatableStateMachine.add('movePlace',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'done'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'GripPose'})
# x:30 y:324, x:130 y:324
_sm_retreate_arm_3 = OperatableStateMachine(
outcomes=['failed', 'done'],
input_keys=['approach_pose', 'PreGripPose'])
with _sm_retreate_arm_3:
# x:30 y:40
OperatableStateMachine.add('ReturnApproachPose',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'ReturnPreGrip',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'approach_pose'})
# x:202 y:41
OperatableStateMachine.add('ReturnPreGrip',
MoveitMove(move=True,
waitForExecution=False,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PreGripPose'})
# x:30 y:324, x:130 y:324
_sm_releasing_4 = OperatableStateMachine(
outcomes=['object', 'no_object'])
with _sm_releasing_4:
# x:30 y:40
OperatableStateMachine.add('say touchdown',
SaraSay(sentence="Touchdown!",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'open gripper'},
autonomy={'done': Autonomy.Off})
# x:139 y:176
OperatableStateMachine.add(
'open gripper',
SetGripperState(width=0.14, effort=1),
transitions={
'object': 'object',
'no_object': 'no_object'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:30 y:324, x:130 y:324
_sm_moveback_5 = OperatableStateMachine(outcomes=['arrived', 'failed'])
with _sm_moveback_5:
# x:30 y:40
OperatableStateMachine.add('genpose',
GenPoseEuler(x=-0.3,
y=-0.3,
z=0,
roll=0,
pitch=0,
yaw=0),
transitions={'done': 'move back'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'backPose'})
# x:40 y:163
OperatableStateMachine.add('move back',
SaraMoveBase(reference="base_link"),
transitions={
'arrived': 'arrived',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'backPose'})
# x:536 y:72, x:231 y:292
_sm_prepare_grip_6 = OperatableStateMachine(
outcomes=['failed', 'done'],
input_keys=['pos'],
output_keys=['approach_pose', 'grip_pose'])
with _sm_prepare_grip_6:
# x:50 y:40
OperatableStateMachine.add('Gen place_pos',
GenGripperPose(l=0,
z=-0.05,
planar=True),
transitions={
'done': 'Gen approach_pos',
'fail': 'failed'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'pos',
'pose_out': 'grip_pose'
})
# x:30 y:176
OperatableStateMachine.add('MoveIt_isReachable',
MoveitMove(move=False,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'log app',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'grip_pose'})
# x:37 y:108
OperatableStateMachine.add('Gen approach_pos',
GenGripperPose(l=0.0,
z=0.20,
planar=True),
transitions={
'done': 'log place pos',
'fail': 'failed'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'pos',
'pose_out': 'approach_pose'
})
# x:41 y:269
OperatableStateMachine.add('log app',
LogKeyState(
text="{}",
severity=Logger.REPORT_HINT),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'approach_pose'})
# x:360 y:180
OperatableStateMachine.add(
'log place pos',
LogKeyState(text="place pose is {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'MoveIt_isReachable'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'grip_pose'})
# x:30 y:324, x:130 y:324
_sm_pregrippose_7 = OperatableStateMachine(outcomes=['done', 'failed'],
output_keys=['PreGripPose'])
with _sm_pregrippose_7:
# x:30 y:40
OperatableStateMachine.add('setPreGripPose',
SetKey(Value="PrePlacePose"),
transitions={'done': 'gotoPreGrip'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'PreGripPose'})
# x:32 y:106
OperatableStateMachine.add('gotoPreGrip',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PreGripPose'})
# x:30 y:458, x:130 y:458, x:230 y:458
_sm_get_down_8 = ConcurrencyContainer(
outcomes=['done'],
input_keys=['GripPose'],
conditions=[('done', [('Go down', 'done')]),
('done', [('read torque', 'done')])])
with _sm_get_down_8:
# x:178 y:127
OperatableStateMachine.add('Go down',
_sm_go_down_2,
transitions={'done': 'done'},
autonomy={'done': Autonomy.Inherit},
remapping={'GripPose': 'GripPose'})
# x:405 y:150
OperatableStateMachine.add('read torque',
_sm_read_torque_1,
transitions={'done': 'done'},
autonomy={'done': Autonomy.Inherit})
# x:30 y:324, x:130 y:324
_sm_pretraitement_9 = OperatableStateMachine(outcomes=['fail', 'done'],
input_keys=['pos'],
output_keys=['pos'])
with _sm_pretraitement_9:
# x:30 y:40
OperatableStateMachine.add('TF_transformation',
TF_transformation(in_ref="map",
out_ref="base_link"),
transitions={
'done': 'LOG POSE',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'in_pos': 'pos',
'out_pos': 'pos'
})
# x:33 y:107
OperatableStateMachine.add('LOG POSE',
LogKeyState(
text="{}",
severity=Logger.REPORT_HINT),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'pos'})
with _state_machine:
# x:148 y:34
OperatableStateMachine.add('Pretraitement',
_sm_pretraitement_9,
transitions={
'fail': 'failed',
'done': 'PreGripPose'
},
autonomy={
'fail': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={'pos': 'pos'})
# x:634 y:410
OperatableStateMachine.add(
'close gripper',
SetGripperState(width=0, effort=1),
transitions={
'object': 'finished',
'no_object': 'finished'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:141 y:522
OperatableStateMachine.add('Get_down',
_sm_get_down_8,
transitions={'done': 'releasing'},
autonomy={'done': Autonomy.Inherit},
remapping={'GripPose': 'grip_pose'})
# x:159 y:352
OperatableStateMachine.add('look down',
SaraSetHeadAngle(pitch=0.6, yaw=-0.3),
transitions={'done': 'Move_approach'},
autonomy={'done': Autonomy.Off})
# x:160 y:127
OperatableStateMachine.add(
'PreGripPose',
_sm_pregrippose_7,
transitions={
'done': 'Prepare grip',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'PreGripPose': 'PreGripPose'})
# x:156 y:238
OperatableStateMachine.add('Prepare grip',
_sm_prepare_grip_6,
transitions={
'failed': 'failed',
'done': 'look down'
},
autonomy={
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'pos': 'pos',
'approach_pose': 'approach_pose',
'grip_pose': 'grip_pose'
})
# x:139 y:444
OperatableStateMachine.add('Move_approach',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'Get_down',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'approach_pose'})
# x:623 y:525
OperatableStateMachine.add('Moveback',
_sm_moveback_5,
transitions={
'arrived': 'close gripper',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:298 y:520
OperatableStateMachine.add('releasing',
_sm_releasing_4,
transitions={
'object': 'retreate arm',
'no_object': 'retreate arm'
},
autonomy={
'object': Autonomy.Inherit,
'no_object': Autonomy.Inherit
})
# x:459 y:522
OperatableStateMachine.add('retreate arm',
_sm_retreate_arm_3,
transitions={
'failed': 'failed',
'done': 'Moveback'
},
autonomy={
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'approach_pose': 'approach_pose',
'PreGripPose': 'PreGripPose'
})
return _state_machine
def create(self):
# x:30 y:365, x:242 y:161
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:241 y:23
OperatableStateMachine.add('log',
LogState(
text="logged to get started :-) ",
severity=Logger.REPORT_HINT),
transitions={'done': 'listennow'},
autonomy={'done': Autonomy.Off})
# x:936 y:282
OperatableStateMachine.add('wait4',
WaitState(wait_time=3),
transitions={'done': 'sub_is_listen2'},
autonomy={'done': Autonomy.Off})
# x:913 y:34
OperatableStateMachine.add('sub_is_listening',
SubscriberState(
topic='/is_robot_listening',
blocking=True,
clear=False),
transitions={
'received': 'check listen or not',
'unavailable': 'check listen or not'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:1122 y:153
OperatableStateMachine.add(
'check listen or not',
CheckConditionState(
predicate=lambda message: message.data == "not listening"),
transitions={
'true': 'listen2',
'false': 'sub_is_listening'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'message'})
# x:709 y:49
OperatableStateMachine.add(
'wait2',
WaitState(wait_time=1),
transitions={'done': 'sub_is_listening'},
autonomy={'done': Autonomy.Off})
# x:522 y:205
OperatableStateMachine.add(
'chk',
CheckConditionState(
predicate=lambda message: message.data == "not listening"),
transitions={
'true': 'fin',
'false': 'sub_is_listen2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'message'})
# x:328 y:237
OperatableStateMachine.add('fin',
LogState(text="finished interaction",
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:460 y:24
OperatableStateMachine.add('listennow',
ListeningState(),
transitions={
'continue': 'wait2',
'failed': 'wait2'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1048 y:421
OperatableStateMachine.add('listen2',
ListeningState(),
transitions={
'continue': 'wait4',
'failed': 'wait4'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:730 y:302
OperatableStateMachine.add('sub_is_listen2',
SubscriberState(
topic='/is_robot_listening',
blocking=True,
clear=False),
transitions={
'received': 'chk',
'unavailable': 'chk'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
return _state_machine
def create(self):
# x:923 y:114
_state_machine = OperatableStateMachine(outcomes=['finished'])
_state_machine.userdata.none = None
_state_machine.userdata.do_turn_torso = False
_state_machine.userdata.pushing_side = 'right' if self.hand_side == 'left' else 'left'
_state_machine.userdata.torso_side = self.hand_side
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:333 y:440, x:433 y:240
_sm_opening_motion_0 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['do_turn_torso', 'pushing_side', 'none', 'torso_side'])
with _sm_opening_motion_0:
# x:438 y:28
OperatableStateMachine.add(
'Branch_Torso_Open',
DecisionState(outcomes=['turn', 'fixed'],
conditions=lambda x: 'turn' if x else 'fixed'),
transitions={
'turn': 'Turn_Torso',
'fixed': 'Go_To_Open_Door_Pose_Straight'
},
autonomy={
'turn': Autonomy.Low,
'fixed': Autonomy.Low
},
remapping={'input_value': 'do_turn_torso'})
# x:57 y:278
OperatableStateMachine.add(
'Go_To_Open_Door_Pose_Turned',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.
DOOR_OPEN_POSE_TURNED,
vel_scaling=0.05,
ignore_collisions=True,
link_paddings={}),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'pushing_side'})
# x:705 y:228
OperatableStateMachine.add(
'Go_To_Open_Door_Pose_Straight',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.
DOOR_OPEN_POSE_STRAIGHT,
vel_scaling=0.05,
ignore_collisions=True,
link_paddings={}),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'pushing_side'})
# x:76 y:128
OperatableStateMachine.add(
'Turn_Torso',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.TURN_TORSO_FULL,
vel_scaling=0.05,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'Go_To_Open_Door_Pose_Turned',
'failed': 'Log_No_Turn'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'torso_side'})
# x:484 y:128
OperatableStateMachine.add(
'Set_Turn_Torso_False',
CalculationState(calculation=lambda x: False),
transitions={'done': 'Go_To_Open_Door_Pose_Straight'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'none',
'output_value': 'do_turn_torso'
})
# x:305 y:128
OperatableStateMachine.add(
'Log_No_Turn',
LogState(text="Skip turning because of collision",
severity=Logger.REPORT_INFO),
transitions={'done': 'Set_Turn_Torso_False'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:287 y:78
OperatableStateMachine.add(
'Decide_If_Turn',
OperatorDecisionState(outcomes=['turn_torso', 'fixed_torso'],
hint=None,
suggestion=None),
transitions={
'turn_torso': 'Decide_Go_To_Stand',
'fixed_torso': 'Go_To_Door_Ready_Pose'
},
autonomy={
'turn_torso': Autonomy.Full,
'fixed_torso': Autonomy.Full
})
# x:125 y:478
OperatableStateMachine.add(
'Go_To_Door_Ready_Pose',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE,
vel_scaling=0.2,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'Opening_Motion',
'failed': 'Decide_No_Collision_Avoidance'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'pushing_side'})
# x:887 y:378
OperatableStateMachine.add('Ask_If_Open',
OperatorDecisionState(
outcomes=['open', 'push_again'],
hint='Is the door open?',
suggestion='open'),
transitions={
'open': 'Go_To_Final_Stand',
'push_again': 'Log_Try_Push_Again'
},
autonomy={
'open': Autonomy.High,
'push_again': Autonomy.Full
})
# x:521 y:82
OperatableStateMachine.add('Log_Try_Push_Again',
LogState(
text='Move robot closer to door',
severity=Logger.REPORT_HINT),
transitions={'done': 'Decide_If_Turn'},
autonomy={'done': Autonomy.Full})
# x:620 y:628
OperatableStateMachine.add(
'Back_To_Door_Ready_Pose',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE,
vel_scaling=0.3,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'Branch_Torso_Retract',
'failed': 'Open_Manually'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'pushing_side'})
# x:136 y:328
OperatableStateMachine.add(
'Set_Turn_Torso_True',
CalculationState(calculation=lambda x: True),
transitions={'done': 'Go_To_Door_Ready_Pose'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'none',
'output_value': 'do_turn_torso'
})
# x:186 y:622
OperatableStateMachine.add('Opening_Motion',
_sm_opening_motion_0,
transitions={
'finished':
'Back_To_Door_Ready_Pose',
'failed': 'Open_Manually'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'do_turn_torso': 'do_turn_torso',
'pushing_side': 'pushing_side',
'none': 'none',
'torso_side': 'torso_side'
})
# x:893 y:201
OperatableStateMachine.add(
'Go_To_Final_Stand',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.STAND_POSE,
vel_scaling=0.2,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'finished',
'failed': 'finished'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'none'})
# x:826 y:528
OperatableStateMachine.add(
'Turn_Torso_Center_Pose',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.
TURN_TORSO_CENTER_POSE,
vel_scaling=0.05,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'Ask_If_Open',
'failed': 'Open_Manually'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'none'})
# x:1034 y:628
OperatableStateMachine.add(
'Branch_Torso_Retract',
DecisionState(outcomes=['turn', 'fixed'],
conditions=lambda x: 'turn' if x else 'fixed'),
transitions={
'turn': 'Turn_Torso_Center_Pose',
'fixed': 'Ask_If_Open'
},
autonomy={
'turn': Autonomy.Low,
'fixed': Autonomy.Low
},
remapping={'input_value': 'do_turn_torso'})
# x:26 y:278
OperatableStateMachine.add(
'Go_To_Stand',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.STAND_POSE,
vel_scaling=0.2,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'Set_Turn_Torso_True',
'failed': 'Go_To_Door_Ready_Pose'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'none'})
# x:136 y:128
OperatableStateMachine.add(
'Decide_Go_To_Stand',
DecisionState(outcomes=['stand', 'skip'],
conditions=lambda x: 'stand'
if not x else 'skip'),
transitions={
'stand': 'Check_Hand_Space',
'skip': 'Set_Turn_Torso_True'
},
autonomy={
'stand': Autonomy.Low,
'skip': Autonomy.Low
},
remapping={'input_value': 'do_turn_torso'})
# x:360 y:378
OperatableStateMachine.add(
'Decide_No_Collision_Avoidance',
OperatorDecisionState(outcomes=[
'replan_ignore_collisions', 'continue', 'open_manual'
],
hint="Try again and ignore collisions?",
suggestion='replan_ignore_collisions'),
transitions={
'replan_ignore_collisions': 'Go_To_Door_Ready_Pose_NC',
'continue': 'Opening_Motion',
'open_manual': 'Open_Manually'
},
autonomy={
'replan_ignore_collisions': Autonomy.Full,
'continue': Autonomy.Full,
'open_manual': Autonomy.Full
})
# x:365 y:528
OperatableStateMachine.add(
'Go_To_Door_Ready_Pose_NC',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE,
vel_scaling=0.3,
ignore_collisions=True,
link_paddings={}),
transitions={
'done': 'Opening_Motion',
'failed': 'Open_Manually'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'pushing_side'})
# x:651 y:528
OperatableStateMachine.add('Open_Manually',
LogState(text="Open door manually",
severity=Logger.REPORT_HINT),
transitions={'done': 'Ask_If_Open'},
autonomy={'done': Autonomy.Full})
# x:40 y:178
OperatableStateMachine.add(
'Check_Hand_Space',
LogState(
text="Make sure the hands have enough space to the door",
severity=Logger.REPORT_HINT),
transitions={'done': 'Go_To_Stand'},
autonomy={'done': Autonomy.Full})
return _state_machine
def create(self):
POINT_LOCATION = 0
SELECTED_OBJECT_ID = 1
WAYPOINT_GOAL_POSE = 2
GHOST_JOINT_STATES = 3
concurrent_states = dict()
concurrent_mapping = list()
type1 = POINT_LOCATION
type2 = SELECTED_OBJECT_ID
# x:717 y:559, x:344 y:460
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
concurrent_states['input1'] = InputState(
request=type1, message="Please give me data 1...")
concurrent_states['input2'] = InputState(
request=type2, message="Please give me data 2...")
concurrent_states['calc'] = CalculationState(lambda x: 0)
concurrent_mapping.append({
'outcome': 'done',
'condition': {
'input1': 'received',
'input2': 'received'
}
})
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'Request_Data',
InputState(request=SELECTED_OBJECT_ID,
message="Please give me some data..."),
transitions={
'received': 'Log_Received',
'aborted': 'Log_Aborted',
'no_connection': 'Log_No_Connection',
'data_error': 'Log_Data_Error'
},
autonomy={
'received': Autonomy.High,
'aborted': Autonomy.High,
'no_connection': Autonomy.High,
'data_error': Autonomy.High
},
remapping={'data': 'data'})
# x:30 y:247
OperatableStateMachine.add('Log_Received',
LogState(text="Got behavior data!",
severity=Logger.REPORT_INFO),
transitions={'done': 'Print_Result'},
autonomy={'done': Autonomy.Off})
# x:324 y:255
OperatableStateMachine.add(
'Log_Aborted',
LogState(text="Operator aborted data request.",
severity=Logger.REPORT_WARN),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:489 y:241
OperatableStateMachine.add('Log_No_Connection',
LogState(
text="Unable to connect to OCS!",
severity=Logger.REPORT_ERROR),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:158 y:243
OperatableStateMachine.add('Log_Data_Error',
LogState(
text="Failed to deserialize data.",
severity=Logger.REPORT_ERROR),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:27 y:411
OperatableStateMachine.add(
'Print_Result',
CalculationState(calculation=self.print_input_data),
transitions={'done': 'Concurrent_Request'},
autonomy={'done': Autonomy.Low},
remapping={
'input_value': 'data',
'output_value': 'output_value'
})
# x:242 y:586 {?input_keys = ['calc_input_value'],?output_keys = ['input1_data', 'input2_data', 'calc_output_value']}
OperatableStateMachine.add(
'Concurrent_Request',
ConcurrentState(states=concurrent_states,
outcomes=['done', 'failed'],
outcome_mapping=concurrent_mapping),
transitions={
'done': 'Print_Result_1',
'failed': 'failed'
},
autonomy={
'done': Autonomy.High,
'failed': Autonomy.Full
},
remapping={
'calc_input_value': 'data',
'input1_data': 'input1_data',
'input2_data': 'input2_data',
'calc_output_value': 'calc_output_value'
})
# x:520 y:481
OperatableStateMachine.add(
'Print_Result_1',
CalculationState(calculation=self.print_input_data),
transitions={'done': 'Print_Result_2'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'input1_data',
'output_value': 'output_value'
})
# x:495 y:646
OperatableStateMachine.add(
'Print_Result_2',
CalculationState(calculation=self.print_input_data),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'input2_data',
'output_value': 'output_value'
})
return _state_machine
def create(self):
# x:1534 y:100, x:62 y:356
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
output_keys=['ARMidMAIN', 'BothArmsFull', 'BothArmsEMPTY'])
_state_machine.userdata.arm_idR = 'right_arm'
_state_machine.userdata.arm_idL = 'left_arm'
_state_machine.userdata.ARMidMAIN = ''
_state_machine.userdata.BothArmsFull = ''
_state_machine.userdata.Ja_value = 'Ja'
_state_machine.userdata.Nee_value = 'Nee'
_state_machine.userdata.BothArmsEMPTY = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:70 y:32
OperatableStateMachine.add('CheckGripperRight',
CheckGripperattached(),
transitions={
'True': 'CheckGripperLeft',
'False': 'CheckGripperLeft_2',
'invalid_arm_id': 'failed'
},
autonomy={
'True': Autonomy.Off,
'False': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_idR'})
# x:69 y:94
OperatableStateMachine.add('CheckGripperLeft',
CheckGripperattached(),
transitions={
'True': 'SetBothArmFull',
'False': 'ResetBothArmFull',
'invalid_arm_id': 'failed'
},
autonomy={
'True': Autonomy.Off,
'False': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_idL'})
# x:702 y:22
OperatableStateMachine.add('SetArmRight',
ReplaceState(),
transitions={'done': 'LogRighterarm'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm_idR',
'result': 'ARMidMAIN'
})
# x:702 y:85
OperatableStateMachine.add('SetArmLeft',
ReplaceState(),
transitions={'done': 'LogLeftArm'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm_idL',
'result': 'ARMidMAIN'
})
# x:528 y:142
OperatableStateMachine.add('SetBothArmFull',
ReplaceState(),
transitions={'done': 'BothArmsFullLOG'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Ja_value',
'result': 'BothArmsFull'
})
# x:888 y:19
OperatableStateMachine.add(
'LogRighterarm',
LogState(text="The Right arm has nothing attached to it",
severity=Logger.REPORT_HINT),
transitions={'done': 'Arm_id is:'},
autonomy={'done': Autonomy.Off})
# x:887 y:77
OperatableStateMachine.add(
'LogLeftArm',
LogState(text="The Left arm has nothing attached to it",
severity=Logger.REPORT_HINT),
transitions={'done': 'Arm_id is:'},
autonomy={'done': Autonomy.Off})
# x:530 y:84
OperatableStateMachine.add('ResetBothArmFull',
ReplaceState(),
transitions={'done': 'SetArmLeft'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Nee_value',
'result': 'BothArmsFull'
})
# x:530 y:26
OperatableStateMachine.add('ResetBothArmFull_2',
ReplaceState(),
transitions={'done': 'SetArmRight'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Nee_value',
'result': 'BothArmsFull'
})
# x:1149 y:41
OperatableStateMachine.add('Arm_id is:',
LogKeyState(
text="The chosen arm_id is:{}",
severity=Logger.REPORT_HINT),
transitions={'done': 'BothArmsFullLOG'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'ARMidMAIN'})
# x:1365 y:90
OperatableStateMachine.add('BothArmsFullLOG',
LogKeyState(
text='Zijn beide armen vol: {}',
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'BothArmsFull'})
# x:300 y:28
OperatableStateMachine.add('CheckGripperLeft_2',
CheckGripperattached(),
transitions={
'True': 'ResetBothArmFull_2',
'False': 'SetBothArmEMPTY',
'invalid_arm_id': 'failed'
},
autonomy={
'True': Autonomy.Off,
'False': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_idL'})
# x:319 y:412
OperatableStateMachine.add(
'SetBothArmEMPTY',
ReplaceState(),
transitions={'done': 'ResetBothArmFull_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Ja_value',
'result': 'BothArmsEMPTY'
})
return _state_machine
def create(self):
pull_affordance = "pull"
affordance_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == "left" else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM
pull_displacement = 0.3 # meters
# x:383 y:840, x:483 y:490
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.hand_side = self.hand_side
_state_machine.userdata.none = None
_state_machine.userdata.step_back_distance = 1.0 # meters
_state_machine.userdata.grasp_preference = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
self._pull_displacement = pull_displacement
# [/MANUAL_CREATE]
# x:1033 y:40, x:333 y:90, x:1033 y:190
_sm_go_to_grasp_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'again'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference'])
with _sm_go_to_grasp_0:
# x:33 y:49
OperatableStateMachine.add('Get_Grasp_Info',
GetTemplateGraspState(),
transitions={'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'Inform_Grasp_Failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.Low},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'grasp': 'grasp_pose'})
# x:40 y:293
OperatableStateMachine.add('Convert_Waypoints',
CalculationState(calculation=lambda msg: [msg.pose]),
transitions={'done': 'Plan_To_Grasp'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_pose', 'output_value': 'grasp_waypoints'})
# x:242 y:292
OperatableStateMachine.add('Plan_To_Grasp',
PlanEndeffectorCartesianWaypointsState(ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'planned': 'Move_To_Grasp_Pose', 'incomplete': 'Move_To_Grasp_Pose', 'failed': 'Decide_Which_Grasp'},
autonomy={'planned': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.High},
remapping={'waypoints': 'grasp_waypoints', 'hand': 'hand_side', 'frame_id': 'grasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:494 y:175
OperatableStateMachine.add('Move_To_Grasp_Pose',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'Optional_Template_Adjustment', 'failed': 'Decide_Which_Grasp'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:226 y:177
OperatableStateMachine.add('Inform_Grasp_Failed',
LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:970 y:294
OperatableStateMachine.add('Increase_Preference_Index',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'again'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'})
# x:41 y:178
OperatableStateMachine.add('Extract_Frame_Id',
CalculationState(calculation=lambda pose: pose.header.frame_id),
transitions={'done': 'Convert_Waypoints'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_pose', 'output_value': 'grasp_frame_id'})
# x:712 y:78
OperatableStateMachine.add('Optional_Template_Adjustment',
OperatorDecisionState(outcomes=["grasp", "pregrasp", "skip"], hint="Consider adjusting the template's pose", suggestion="skip"),
transitions={'grasp': 'Get_Grasp_Info', 'pregrasp': 'again', 'skip': 'finished'},
autonomy={'grasp': Autonomy.Full, 'pregrasp': Autonomy.Full, 'skip': Autonomy.High})
# x:754 y:294
OperatableStateMachine.add('Decide_Which_Grasp',
OperatorDecisionState(outcomes=["same", "next"], hint='Try the same grasp or the next one?', suggestion='same'),
transitions={'same': 'Optional_Template_Adjustment', 'next': 'Increase_Preference_Index'},
autonomy={'same': Autonomy.High, 'next': Autonomy.High})
# x:133 y:390, x:433 y:190, x:983 y:140
_sm_perform_grasp_1 = OperatableStateMachine(outcomes=['finished', 'failed', 'next'], input_keys=['hand_side', 'grasp_preference', 'template_id', 'pregrasp_pose'], output_keys=['grasp_preference'])
with _sm_perform_grasp_1:
# x:68 y:76
OperatableStateMachine.add('Get_Finger_Configuration',
GetTemplateFingerConfigState(),
transitions={'done': 'Close_Fingers', 'failed': 'failed', 'not_available': 'Inform_Closing_Failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High, 'not_available': Autonomy.High},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'finger_config': 'finger_config'})
# x:293 y:328
OperatableStateMachine.add('Convert_Waypoints',
CalculationState(calculation=lambda msg: [msg.pose]),
transitions={'done': 'Plan_Back_To_Pregrasp'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_waypoints'})
# x:496 y:328
OperatableStateMachine.add('Plan_Back_To_Pregrasp',
PlanEndeffectorCartesianWaypointsState(ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'planned': 'Move_Back_To_Pregrasp_Pose', 'incomplete': 'Move_Back_To_Pregrasp_Pose', 'failed': 'failed'},
autonomy={'planned': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Low},
remapping={'waypoints': 'pregrasp_waypoints', 'hand': 'hand_side', 'frame_id': 'pregrasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:662 y:228
OperatableStateMachine.add('Move_Back_To_Pregrasp_Pose',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'Increase_Preference_Index', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:296 y:228
OperatableStateMachine.add('Extract_Frame_Id',
CalculationState(calculation=lambda pose: pose.header.frame_id),
transitions={'done': 'Convert_Waypoints'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_frame_id'})
# x:673 y:128
OperatableStateMachine.add('Increase_Preference_Index',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'next'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'})
# x:81 y:228
OperatableStateMachine.add('Close_Fingers',
HandTrajectoryState(hand_type=self.hand_type),
transitions={'done': 'finished', 'failed': 'Extract_Frame_Id'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'finger_trajectory': 'finger_config', 'hand_side': 'hand_side'})
# x:490 y:75
OperatableStateMachine.add('Inform_Closing_Failed',
LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:733 y:190, x:383 y:40
_sm_go_to_pregrasp_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference', 'pregrasp_pose'])
with _sm_go_to_pregrasp_2:
# x:27 y:68
OperatableStateMachine.add('Get_Pregrasp_Info',
GetTemplatePregraspState(),
transitions={'done': 'Plan_To_Pregrasp_Pose', 'failed': 'failed', 'not_available': 'Inform_Pregrasp_Failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High, 'not_available': Autonomy.High},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'pregrasp_pose'})
# x:269 y:153
OperatableStateMachine.add('Inform_Pregrasp_Failed',
LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Low})
# x:537 y:228
OperatableStateMachine.add('Move_To_Pregrasp_Pose',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'finished', 'failed': 'Decide_Which_Pregrasp'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:25 y:328
OperatableStateMachine.add('Increase_Preference_Index',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'Get_Pregrasp_Info'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'})
# x:266 y:228
OperatableStateMachine.add('Plan_To_Pregrasp_Pose',
PlanEndeffectorPoseState(ignore_collisions=False, include_torso=False, allowed_collisions=[], planner_id="RRTConnectkConfigDefault"),
transitions={'planned': 'Move_To_Pregrasp_Pose', 'failed': 'Decide_Which_Pregrasp'},
autonomy={'planned': Autonomy.Low, 'failed': Autonomy.High},
remapping={'target_pose': 'pregrasp_pose', 'hand': 'hand_side', 'joint_trajectory': 'joint_trajectory'})
# x:266 y:327
OperatableStateMachine.add('Decide_Which_Pregrasp',
OperatorDecisionState(outcomes=["same", "next"], hint='Try the same pregrasp or the next one?', suggestion='same'),
transitions={'same': 'Get_Pregrasp_Info', 'next': 'Increase_Preference_Index'},
autonomy={'same': Autonomy.High, 'next': Autonomy.High})
# x:30 y:444, x:162 y:478, x:230 y:478
_sm_planning_pipeline_3 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['stand_pose'], output_keys=['plan_header'])
with _sm_planning_pipeline_3:
# x:34 y:57
OperatableStateMachine.add('Create_Step_Goal',
CreateStepGoalState(pose_is_pelvis=True),
transitions={'done': 'Plan_To_Waypoint', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'target_pose': 'stand_pose', 'step_goal': 'step_goal'})
# x:553 y:481
OperatableStateMachine.add('Modify_Plan',
InputState(request=InputState.FOOTSTEP_PLAN_HEADER, message='Modify plan, VALIDATE, and confirm.'),
transitions={'received': 'finished', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'plan_header'})
# x:34 y:484
OperatableStateMachine.add('Plan_To_Waypoint',
PlanFootstepsState(mode=self.parameter_set),
transitions={'planned': 'Modify_Plan', 'failed': 'Decide_Replan_without_Collision'},
autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'})
# x:139 y:314
OperatableStateMachine.add('Decide_Replan_without_Collision',
OperatorDecisionState(outcomes=['replan', 'fail'], hint='Try replanning without collision avoidance.', suggestion='replan'),
transitions={'replan': 'Replan_without_Collision', 'fail': 'failed'},
autonomy={'replan': Autonomy.Low, 'fail': Autonomy.Full})
# x:319 y:406
OperatableStateMachine.add('Replan_without_Collision',
PlanFootstepsState(mode='drc_step_no_collision'),
transitions={'planned': 'Modify_Plan', 'failed': 'failed'},
autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'})
# x:1103 y:424, x:130 y:478
_sm_grasp_trigger_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'grasp_preference'])
with _sm_grasp_trigger_4:
# x:86 y:72
OperatableStateMachine.add('Go_to_Pregrasp',
_sm_go_to_pregrasp_2,
transitions={'finished': 'Open_Fingers', 'failed': 'Grasp_Manually'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'pregrasp_pose': 'pregrasp_pose'})
# x:789 y:172
OperatableStateMachine.add('Perform_Grasp',
_sm_perform_grasp_1,
transitions={'finished': 'finished', 'failed': 'Grasp_Manually', 'next': 'Close_Fingers'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'next': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'pregrasp_pose': 'pregrasp_pose'})
# x:332 y:178
OperatableStateMachine.add('Open_Fingers',
FingerConfigurationState(hand_type=self.hand_type, configuration=0.0),
transitions={'done': 'Go_to_Grasp', 'failed': 'Grasp_Manually'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.High},
remapping={'hand_side': 'hand_side'})
# x:332 y:28
OperatableStateMachine.add('Close_Fingers',
FingerConfigurationState(hand_type=self.hand_type, configuration=1.0),
transitions={'done': 'Go_to_Pregrasp', 'failed': 'Grasp_Manually'},
autonomy={'done': Autonomy.High, 'failed': Autonomy.High},
remapping={'hand_side': 'hand_side'})
# x:324 y:428
OperatableStateMachine.add('Grasp_Manually',
OperatorDecisionState(outcomes=["fingers_closed", "abort"], hint="Grasp the object manually, continue when fingers are closed.", suggestion=None),
transitions={'fingers_closed': 'finished', 'abort': 'failed'},
autonomy={'fingers_closed': Autonomy.Full, 'abort': Autonomy.Full})
# x:543 y:172
OperatableStateMachine.add('Go_to_Grasp',
_sm_go_to_grasp_0,
transitions={'finished': 'Perform_Grasp', 'failed': 'Grasp_Manually', 'again': 'Close_Fingers'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'again': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id'})
# x:30 y:478, x:130 y:478, x:230 y:478
_sm_walk_to_template_5 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['template_id', 'grasp_preference', 'hand_side'])
with _sm_walk_to_template_5:
# x:265 y:28
OperatableStateMachine.add('Decide_Request_Template',
DecisionState(outcomes=['request', 'continue'], conditions=lambda x: 'continue' if x is not None else 'request'),
transitions={'request': 'Request_Template', 'continue': 'Get_Stand_Pose'},
autonomy={'request': Autonomy.Low, 'continue': Autonomy.Off},
remapping={'input_value': 'template_id'})
# x:1033 y:106
OperatableStateMachine.add('Increment_Stand_Pose',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'Inform_About_Retry'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'})
# x:1162 y:29
OperatableStateMachine.add('Inform_About_Retry',
LogState(text="Stand pose choice failed. Trying again.", severity=Logger.REPORT_INFO),
transitions={'done': 'Get_Stand_Pose'},
autonomy={'done': Autonomy.Off})
# x:567 y:118
OperatableStateMachine.add('Inform_About_Fail',
LogState(text="Unable to find a suitable stand pose for the template.", severity=Logger.REPORT_WARN),
transitions={'done': 'Decide_Repeat_Request'},
autonomy={'done': Autonomy.Off})
# x:554 y:274
OperatableStateMachine.add('Get_Goal_from_Operator',
InputState(request=InputState.WAYPOINT_GOAL_POSE, message="Provide a waypoint in front of the template."),
transitions={'received': 'Walk_To_Waypoint', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'plan_header'})
# x:279 y:110
OperatableStateMachine.add('Request_Template',
InputState(request=InputState.SELECTED_OBJECT_ID, message="Specify target template"),
transitions={'received': 'Get_Stand_Pose', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.Off, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'template_id'})
# x:825 y:461
OperatableStateMachine.add('Wait_For_Stand',
CheckCurrentControlModeState(target_mode=CheckCurrentControlModeState.STAND, wait=True),
transitions={'correct': 'finished', 'incorrect': 'failed'},
autonomy={'correct': Autonomy.Low, 'incorrect': Autonomy.Full},
remapping={'control_mode': 'control_mode'})
# x:1143 y:277
OperatableStateMachine.add('Decide_Stand_Preference',
OperatorDecisionState(outcomes=["same", "next", "abort"], hint="Same or next stand pose?", suggestion="next"),
transitions={'same': 'Inform_About_Retry', 'next': 'Increment_Stand_Pose', 'abort': 'aborted'},
autonomy={'same': Autonomy.Full, 'next': Autonomy.Full, 'abort': Autonomy.Full})
# x:842 y:152
OperatableStateMachine.add('Planning_Pipeline',
_sm_planning_pipeline_3,
transitions={'finished': 'Walk_To_Waypoint', 'failed': 'Decide_Stand_Preference', 'aborted': 'Decide_Stand_Preference'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit},
remapping={'stand_pose': 'stand_pose', 'plan_header': 'plan_header'})
# x:833 y:276
OperatableStateMachine.add('Walk_To_Waypoint',
ExecuteStepPlanActionState(),
transitions={'finished': 'Wait_For_Stand', 'failed': 'Decide_Stand_Preference'},
autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'plan_header': 'plan_header'})
# x:554 y:195
OperatableStateMachine.add('Decide_Repeat_Request',
OperatorDecisionState(outcomes=['repeat_id', 'request_goal'], hint=None, suggestion=None),
transitions={'repeat_id': 'Request_Template', 'request_goal': 'Get_Goal_from_Operator'},
autonomy={'repeat_id': Autonomy.Low, 'request_goal': Autonomy.High})
# x:547 y:27
OperatableStateMachine.add('Get_Stand_Pose',
GetTemplateStandPoseState(),
transitions={'done': 'Planning_Pipeline', 'failed': 'Inform_About_Fail', 'not_available': 'Inform_About_Fail'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.High},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'stand_pose': 'stand_pose'})
# x:133 y:340, x:383 y:140
_sm_perform_step_back_6 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['step_back_distance'])
with _sm_perform_step_back_6:
# x:78 y:78
OperatableStateMachine.add('Plan_Steps_Back',
FootstepPlanRelativeState(direction=FootstepPlanRelativeState.DIRECTION_BACKWARD),
transitions={'planned': 'Do_Steps_Back', 'failed': 'failed'},
autonomy={'planned': Autonomy.High, 'failed': Autonomy.Full},
remapping={'distance': 'step_back_distance', 'plan_header': 'plan_header'})
# x:74 y:228
OperatableStateMachine.add('Do_Steps_Back',
ExecuteStepPlanActionState(),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'plan_header': 'plan_header'})
# x:133 y:340, x:333 y:90
_sm_release_trigger_7 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'none'])
with _sm_release_trigger_7:
# x:82 y:78
OperatableStateMachine.add('Open_Fingers',
FingerConfigurationState(hand_type=self.hand_type, configuration=0),
transitions={'done': 'Take_Hand_Back', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'hand_side': 'hand_side'})
# x:96 y:178
OperatableStateMachine.add('Take_Hand_Back',
LogState(text="Take hand slightly back", severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Full})
# x:733 y:240, x:33 y:289
_sm_pull_trigger_8 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none'])
with _sm_pull_trigger_8:
# x:202 y:28
OperatableStateMachine.add('Ready_To_Pull',
LogState(text="Ready to pull the trigger down", severity=Logger.REPORT_INFO),
transitions={'done': 'Get_Pull_Affordance'},
autonomy={'done': Autonomy.High})
# x:192 y:328
OperatableStateMachine.add('Plan_Pull',
PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'done': 'Execute_Pull', 'incomplete': 'Execute_Pull', 'failed': 'failed'},
autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full},
remapping={'affordance': 'affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:176 y:428
OperatableStateMachine.add('Execute_Pull',
ExecuteTrajectoryMsgState(controller=affordance_controller),
transitions={'done': 'Decide_Repeat_Pull', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:183 y:228
OperatableStateMachine.add('Scale_Pull_Affordance',
CalculationState(calculation=lambda x: x),
transitions={'done': 'Plan_Pull'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'affordance', 'output_value': 'affordance'})
# x:173 y:128
OperatableStateMachine.add('Get_Pull_Affordance',
GetTemplateAffordanceState(identifier=pull_affordance),
transitions={'done': 'Scale_Pull_Affordance', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'affordance'})
# x:437 y:228
OperatableStateMachine.add('Decide_Repeat_Pull',
OperatorDecisionState(outcomes=['done', 'repeat'], hint="Pull further?", suggestion='done'),
transitions={'done': 'finished', 'repeat': 'Get_Pull_Affordance'},
autonomy={'done': Autonomy.High, 'repeat': Autonomy.Full})
with _state_machine:
# x:73 y:78
OperatableStateMachine.add('Request_Trigger_Template',
InputState(request=InputState.SELECTED_OBJECT_ID, message="Place trigger template"),
transitions={'received': 'Decide_Walking', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'template_id'})
# x:337 y:78
OperatableStateMachine.add('Decide_Walking',
OperatorDecisionState(outcomes=["walk", "stand"], hint="Walk to template?", suggestion="walk"),
transitions={'walk': 'Walk_To_Template', 'stand': 'Set_Manipulate'},
autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full})
# x:844 y:322
OperatableStateMachine.add('Pull_Trigger',
_sm_pull_trigger_8,
transitions={'finished': 'Release_Trigger', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none'})
# x:836 y:422
OperatableStateMachine.add('Release_Trigger',
_sm_release_trigger_7,
transitions={'finished': 'Warn_Stand', 'failed': 'Warn_Stand'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'none': 'none'})
# x:826 y:678
OperatableStateMachine.add('Go_To_Stand_Pose',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False),
transitions={'done': 'Set_Stand', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'side': 'none'})
# x:566 y:78
OperatableStateMachine.add('Set_Manipulate',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE),
transitions={'changed': 'Set_Template_Frame', 'failed': 'failed'},
autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full})
# x:858 y:578
OperatableStateMachine.add('Warn_Stand',
LogState(text="Will go to stand now", severity=Logger.REPORT_INFO),
transitions={'done': 'Go_To_Stand_Pose'},
autonomy={'done': Autonomy.High})
# x:566 y:678
OperatableStateMachine.add('Set_Stand',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND),
transitions={'changed': 'Decide_Step_Back', 'failed': 'failed'},
autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full})
# x:337 y:678
OperatableStateMachine.add('Decide_Step_Back',
OperatorDecisionState(outcomes=["walk", "stand"], hint="Step back?", suggestion="walk"),
transitions={'walk': 'Perform_Step_Back', 'stand': 'finished'},
autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full})
# x:77 y:672
OperatableStateMachine.add('Perform_Step_Back',
_sm_perform_step_back_6,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'step_back_distance': 'step_back_distance'})
# x:330 y:172
OperatableStateMachine.add('Walk_To_Template',
_sm_walk_to_template_5,
transitions={'finished': 'Set_Manipulate', 'failed': 'failed', 'aborted': 'Set_Manipulate'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit},
remapping={'template_id': 'template_id', 'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side'})
# x:841 y:222
OperatableStateMachine.add('Grasp_Trigger',
_sm_grasp_trigger_4,
transitions={'finished': 'Pull_Trigger', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'grasp_preference': 'grasp_preference'})
# x:846 y:128
OperatableStateMachine.add('Look_At_Trigger',
LookAtTargetState(),
transitions={'done': 'Grasp_Trigger'},
autonomy={'done': Autonomy.Off},
remapping={'frame': 'template_frame'})
# x:837 y:28
OperatableStateMachine.add('Set_Template_Frame',
CalculationState(calculation=lambda x: "template_tf_%d" % x),
transitions={'done': 'Look_At_Trigger'},
autonomy={'done': Autonomy.Low},
remapping={'input_value': 'template_id', 'output_value': 'template_frame'})
return _state_machine
def create(self):
# x:1195 y:433, x:132 y:431, x:750 y:42, x:991 y:471
_state_machine = OperatableStateMachine(outcomes=['Given', 'Person_not_found', 'No_object_in_hand', 'fail'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:130 y:365
_sm_lookat_0 = OperatableStateMachine(outcomes=['failed'], input_keys=['ID'])
with _sm_lookat_0:
# x:114 y:127
OperatableStateMachine.add('look',
KeepLookingAt(),
transitions={'failed': 'look'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:299 y:300, x:263 y:535
_sm_give_1 = OperatableStateMachine(outcomes=['failed', 'given'], input_keys=['Object'])
with _sm_give_1:
# x:67 y:27
OperatableStateMachine.add('SetPose',
SetKey(Value="ShowGripper"),
transitions={'done': 'say_give'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'target'})
# x:53 y:413
OperatableStateMachine.add('read torque',
ReadTorque(watchdog=5, Joint="right_elbow_pitch_joint", Threshold=2, min_time=1),
transitions={'threshold': 'open gripper', 'watchdog': 'read torque', 'fail': 'failed'},
autonomy={'threshold': Autonomy.Off, 'watchdog': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'torque': 'torque'})
# x:52 y:500
OperatableStateMachine.add('open gripper',
SetGripperState(width=0.15, effort=1),
transitions={'object': 'given', 'no_object': 'given'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:64 y:248
OperatableStateMachine.add('say pull',
SaraSay(sentence="You can pull on it", input_keys=[], emotion=1, block=False),
transitions={'done': 'wait 1'},
autonomy={'done': Autonomy.Off})
# x:64 y:325
OperatableStateMachine.add('wait 1',
WaitState(wait_time=1),
transitions={'done': 'read torque'},
autonomy={'done': Autonomy.Off})
# x:57 y:175
OperatableStateMachine.add('moveArm',
MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15),
transitions={'done': 'say pull', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'target': 'target'})
# x:60 y:88
OperatableStateMachine.add('say_give',
SaraSay(sentence=lambda x: "Hi. I'm giving you this "+str(x), input_keys=[], emotion=0, block=True),
transitions={'done': 'moveArm'},
autonomy={'done': Autonomy.Off})
# x:596 y:480
_sm_follow_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID'])
with _sm_follow_2:
# x:180 y:123
OperatableStateMachine.add('follow',
SaraFollow(distance=1.5, ReplanPeriod=0.5),
transitions={'failed': 'follow'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:313 y:247, x:301 y:177, x:103 y:293, x:343 y:113, x:397 y:411, x:311 y:27, x:630 y:365
_sm_give_3 = ConcurrencyContainer(outcomes=['failed', 'given', 'continue'], input_keys=['ID', 'Object'], conditions=[
('failed', [('Give', 'failed')]),
('given', [('Give', 'given')]),
('given', [('Follow', 'finished')]),
('failed', [('LookAt', 'failed')])
])
with _sm_give_3:
# x:91 y:50
OperatableStateMachine.add('Follow',
_sm_follow_2,
transitions={'finished': 'given'},
autonomy={'finished': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:84 y:164
OperatableStateMachine.add('Give',
_sm_give_1,
transitions={'failed': 'failed', 'given': 'given'},
autonomy={'failed': Autonomy.Inherit, 'given': Autonomy.Inherit},
remapping={'Object': 'Object'})
# x:175 y:371
OperatableStateMachine.add('LookAt',
_sm_lookat_0,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit},
remapping={'ID': 'ID'})
with _state_machine:
# x:77 y:29
OperatableStateMachine.add('Get hand content',
GetRosParam(ParamName="behavior/GripperContent"),
transitions={'done': 'is object in hand?', 'failed': 'fail'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'Value': 'Object'})
# x:58 y:108
OperatableStateMachine.add('is object in hand?',
CheckConditionState(predicate=lambda x: x),
transitions={'true': 'name', 'false': 'log empty hand'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'Object'})
# x:70 y:277
OperatableStateMachine.add('list persons',
list_entities_by_name(frontality_level=0.5, distance_max=10),
transitions={'found': 'get id', 'none_found': 'Person_not_found'},
autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off},
remapping={'name': 'name', 'entity_list': 'People_list', 'number': 'number'})
# x:414 y:37
OperatableStateMachine.add('log empty hand',
LogState(text="The hand is empty. Set the GripperContent rosParam", severity=Logger.REPORT_HINT),
transitions={'done': 'No_object_in_hand'},
autonomy={'done': Autonomy.Off})
# x:754 y:223
OperatableStateMachine.add('log moveitfail',
LogState(text="moveit failed", severity=Logger.REPORT_HINT),
transitions={'done': 'fail'},
autonomy={'done': Autonomy.Off})
# x:606 y:371
OperatableStateMachine.add('log movebase fail',
LogState(text="giving Failed", severity=Logger.REPORT_HINT),
transitions={'done': 'fail'},
autonomy={'done': Autonomy.Off})
# x:402 y:133
OperatableStateMachine.add('set idle pose',
SetKey(Value="IdlePose"),
transitions={'done': 'say_good'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'target'})
# x:751 y:108
OperatableStateMachine.add('moveArm2',
MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15),
transitions={'done': 'set none', 'failed': 'log moveitfail'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'target': 'target'})
# x:920 y:278
OperatableStateMachine.add('close gripper',
SetGripperState(width=0, effort=1),
transitions={'object': 'Given', 'no_object': 'Given'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:1048 y:236
OperatableStateMachine.add('remove gripper content',
SetRosParam(ParamName="GripperContent"),
transitions={'done': 'close gripper'},
autonomy={'done': Autonomy.Off},
remapping={'Value': 'none'})
# x:910 y:182
OperatableStateMachine.add('set none',
SetKey(Value=None),
transitions={'done': 'close gripper'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'none'})
# x:408 y:333
OperatableStateMachine.add('give',
_sm_give_3,
transitions={'failed': 'log movebase fail', 'given': 'set idle pose', 'continue': 'give'},
autonomy={'failed': Autonomy.Inherit, 'given': Autonomy.Inherit, 'continue': Autonomy.Inherit},
remapping={'ID': 'ID', 'Object': 'Object'})
# x:256 y:278
OperatableStateMachine.add('get id',
CalculationState(calculation=lambda x: x[0].ID),
transitions={'done': 'give'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'People_list', 'output_value': 'ID'})
# x:68 y:192
OperatableStateMachine.add('name',
SetKey(Value="person"),
transitions={'done': 'list persons'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'name'})
# x:591 y:138
OperatableStateMachine.add('say_good',
SaraSay(sentence=lambda x: "Good, enjoy your "+str(x), input_keys=[], emotion=0, block=True),
transitions={'done': 'moveArm2'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
l_random = [-2.47557862791, -1.21674644795, -0.761510070214, -0.219080422969, -2.68076430953, 0.189685935246, -3.8825693651]
r_random = [1.10260663602, 2.67164332545, 3.1286914453, 1.52042152218, -1.94191336135, 0.805351035619, -3.64437671794]
# x:33 y:340, x:435 y:356
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.joint_values = [0] * 7
_state_machine.userdata.l_random = l_random
_state_machine.userdata.r_random = r_random
_state_machine.userdata.random = l_random if self.planning_group == "l_arm_group" else r_random
_state_machine.userdata.hand_side = 'left'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:151 y:30
OperatableStateMachine.add('Set_Manipulate',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE),
transitions={'changed': 'Go_To_Stand', 'failed': 'failed'},
autonomy={'changed': Autonomy.Off, 'failed': Autonomy.Off})
# x:65 y:513
OperatableStateMachine.add('Turn_Torso_Left_Pose',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.TURN_TORSO_LEFT_POSE, vel_scaling=0.3, ignore_collisions=True, link_paddings={}),
transitions={'done': 'Go_To_Open_Door_Pose', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'side': 'hand_side'})
# x:312 y:565
OperatableStateMachine.add('Go_To_Open_Door_Pose',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.DOOR_OPEN_POSE_STRAIGHT, vel_scaling=0.1, ignore_collisions=False, link_paddings={}),
transitions={'done': 'Back_To_Door_Ready_Pose', 'failed': 'failed'},
autonomy={'done': Autonomy.High, 'failed': Autonomy.Off},
remapping={'side': 'hand_side'})
# x:647 y:328
OperatableStateMachine.add('Go_To_Final_Stand',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}),
transitions={'done': 'Ask_If_Open', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'side': 'hand_side'})
# x:622 y:189
OperatableStateMachine.add('Ask_If_Open',
OperatorDecisionState(outcomes=['open','push_again'], hint='Is the door open?', suggestion='open'),
transitions={'open': 'finished', 'push_again': 'Log_Try_Push_Again'},
autonomy={'open': Autonomy.High, 'push_again': Autonomy.Full})
# x:355 y:189
OperatableStateMachine.add('Log_Try_Push_Again',
LogState(text='Move robot closer to door', severity=Logger.REPORT_HINT),
transitions={'done': 'Go_To_Door_Ready_Pose'},
autonomy={'done': Autonomy.Full})
# x:623 y:452
OperatableStateMachine.add('Turn_Torso_Center_Pose',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.TURN_TORSO_CENTER_POSE, vel_scaling=0.3, ignore_collisions=True, link_paddings={}),
transitions={'done': 'Go_To_Final_Stand', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'side': 'hand_side'})
# x:126 y:316
OperatableStateMachine.add('Go_To_Door_Ready_Pose',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}),
transitions={'done': 'Go_To_Open_Door_Pose', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'side': 'hand_side'})
# x:100 y:203
OperatableStateMachine.add('Go_To_Stand',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.5, ignore_collisions=False, link_paddings={}),
transitions={'done': 'Go_To_Door_Ready_Pose', 'failed': 'failed'},
autonomy={'done': Autonomy.High, 'failed': Autonomy.Off},
remapping={'side': 'hand_side'})
# x:551 y:580
OperatableStateMachine.add('Back_To_Door_Ready_Pose',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}),
transitions={'done': 'Go_To_Final_Stand', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'side': 'hand_side'})
return _state_machine
def create(self):
# x:821 y:66, x:331 y:203
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.both = 'same'
_state_machine.userdata.left = 'left'
_state_machine.userdata.right = 'right'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:100 y:355, x:321 y:250
_sm_prep_right_arm_0 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['hand_side'])
with _sm_prep_right_arm_0:
# x:61 y:89
OperatableStateMachine.add(
'Go_to_Camera_Pose',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.
CAR_DRIVE_CAMERA_POSE,
vel_scaling=0.2,
ignore_collisions=False,
link_paddings={},
is_cartesian=False),
transitions={
'done': 'Open_Fingers',
'failed': 'Go_to_Camera_Pose'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Full
},
remapping={'side': 'hand_side'})
# x:65 y:228
OperatableStateMachine.add('Open_Fingers',
FingerConfigurationState(
hand_type='robotiq',
configuration=0.0),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'hand_side': 'hand_side'})
# x:329 y:513, x:476 y:175
_sm_prep_left_arm_1 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['hand_side'])
with _sm_prep_left_arm_1:
# x:79 y:59
OperatableStateMachine.add(
'Look_Down',
TiltHeadState(desired_tilt=TiltHeadState.DOWN_60),
transitions={
'done': 'Go_to_Pre_Drive',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
})
# x:60 y:166
OperatableStateMachine.add(
'Go_to_Pre_Drive',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.
CAR_PREDRIVE_LARM_POSE,
vel_scaling=0.2,
ignore_collisions=False,
link_paddings={},
is_cartesian=False),
transitions={
'done': 'Open_Fingers',
'failed': 'Go_to_Pre_Drive'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Full
},
remapping={'side': 'hand_side'})
# x:61 y:397
OperatableStateMachine.add(
'Go_to_Drive',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.CAR_DRIVE_LARM_POSE,
vel_scaling=0.2,
ignore_collisions=False,
link_paddings={},
is_cartesian=False),
transitions={
'done': 'Notify_Field_Team',
'failed': 'Go_to_Drive'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Full
},
remapping={'side': 'hand_side'})
# x:65 y:283
OperatableStateMachine.add('Open_Fingers',
FingerConfigurationState(
hand_type='robotiq',
configuration=0.0),
transitions={
'done': 'Go_to_Drive',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'hand_side': 'hand_side'})
# x:71 y:504
OperatableStateMachine.add(
'Notify_Field_Team',
LogState(text='Ask field team to strap ATLAS',
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Full})
# x:409 y:455
_sm_prep_car_entry_2 = OperatableStateMachine(
outcomes=['finished'], input_keys=['both', 'left', 'right'])
with _sm_prep_car_entry_2:
# x:60 y:65
OperatableStateMachine.add(
'Car_Entry_Arms',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.CAR_ENTRY_ARMS_POSE,
vel_scaling=0.3,
ignore_collisions=True,
link_paddings={},
is_cartesian=False),
transitions={
'done': 'Car_Entry_Forearms',
'failed': 'Car_Entry_Arms'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Full
},
remapping={'side': 'both'})
# x:60 y:217
OperatableStateMachine.add(
'Car_Entry_Forearms',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.CAR_ENTRY_FORE_POSE,
vel_scaling=0.5,
ignore_collisions=True,
link_paddings={},
is_cartesian=False),
transitions={
'done': 'Car_Entry_Left_Leg',
'failed': 'Car_Entry_Forearms'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Full
},
remapping={'side': 'both'})
# x:60 y:351
OperatableStateMachine.add(
'Car_Entry_Left_Leg',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.CAR_ENTRY_LEGS_POSE,
vel_scaling=0.3,
ignore_collisions=False,
link_paddings={},
is_cartesian=False),
transitions={
'done': 'Car_Entry_Right_Leg',
'failed': 'Car_Entry_Left_Leg'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Full
},
remapping={'side': 'left'})
# x:357 y:351
OperatableStateMachine.add(
'Car_Entry_Right_Leg',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.CAR_ENTRY_LEGS_POSE,
vel_scaling=0.3,
ignore_collisions=False,
link_paddings={},
is_cartesian=False),
transitions={
'done': 'finished',
'failed': 'Car_Entry_Right_Leg'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Full
},
remapping={'side': 'right'})
with _state_machine:
# x:39 y:63
OperatableStateMachine.add('Start_in_FREEZE_HP',
ChangeControlModeActionState(
target_mode='freeze_high_pressure'),
transitions={
'changed': 'Go_to_VEHICLE_ENTRY',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Full,
'failed': Autonomy.High
})
# x:51 y:446
OperatableStateMachine.add(
'Prep_Car_Entry',
_sm_prep_car_entry_2,
transitions={'finished': 'Go_to_FREEZE_HP'},
autonomy={'finished': Autonomy.Inherit},
remapping={
'both': 'both',
'left': 'left',
'right': 'right'
})
# x:278 y:451
OperatableStateMachine.add('Go_to_FREEZE_HP',
ChangeControlModeActionState(
target_mode='freeze_high_pressure'),
transitions={
'changed': 'Confirm_All_Clear',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.High,
'failed': Autonomy.High
})
# x:547 y:226
OperatableStateMachine.add('Prep_Left_Arm',
_sm_prep_left_arm_1,
transitions={
'finished': 'Prep_Right_Arm',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'hand_side': 'left'})
# x:523 y:16
OperatableStateMachine.add(
'Go_to_VEHICLE_DRIVE',
ChangeControlModeActionState(target_mode='vehicle_drive'),
transitions={
'changed': 'finished',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Low,
'failed': Autonomy.High
})
# x:515 y:347
OperatableStateMachine.add(
'Back_to_VEHICLE_MANIPULATE',
ChangeControlModeActionState(target_mode='vehicle_manipulate'),
transitions={
'changed': 'Prep_Left_Arm',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Off,
'failed': Autonomy.High
})
# x:550 y:449
OperatableStateMachine.add(
'Confirm_All_Clear',
LogState(text='Confirm that all personnel is clear!',
severity=Logger.REPORT_HINT),
transitions={'done': 'Back_to_VEHICLE_MANIPULATE'},
autonomy={'done': Autonomy.Full})
# x:39 y:252
OperatableStateMachine.add(
'Go_to_VEHICLE_ENTRY',
ChangeControlModeActionState(target_mode='vehicle_entry'),
transitions={
'changed': 'Prep_Car_Entry',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Low,
'failed': Autonomy.High
})
# x:543 y:101
OperatableStateMachine.add('Prep_Right_Arm',
_sm_prep_right_arm_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'hand_side': 'right'})
return _state_machine
def create(self):
# x:119 y:369, x:527 y:258
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['my_input'])
_state_machine.userdata.my_input = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'Entry_Msg',
LogState(text="Now at inner behavior...",
severity=Logger.REPORT_INFO),
transitions={'done': 'FlexBE Testprint Behavior'},
autonomy={'done': Autonomy.Off})
# x:34 y:240
OperatableStateMachine.add('Wait_A_Bit',
WaitState(wait_time=2),
transitions={'done': 'Print_State'},
autonomy={'done': Autonomy.High})
# x:279 y:327
OperatableStateMachine.add('Decide_Outcome',
OperatorDecisionState(
outcomes=['finished', 'failed'],
hint="How should this end?",
suggestion='finished'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Full,
'failed': Autonomy.Full
})
# x:289 y:127
OperatableStateMachine.add(
'Print_State',
CalculationState(calculation=self.print_number_and_inc),
transitions={'done': 'Print_Param_Message'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'my_input',
'output_value': 'output_value'
})
# x:273 y:212
OperatableStateMachine.add(
'Print_Param_Message',
LogState(text="Wait, there is a message: " + self.message,
severity=Logger.REPORT_HINT),
transitions={'done': 'Decide_Outcome'},
autonomy={'done': Autonomy.Off})
# x:15 y:124
OperatableStateMachine.add('FlexBE Testprint Behavior',
self.use_behavior(
FlexBETestprintBehaviorSM,
'FlexBE Testprint Behavior'),
transitions={'finished': 'Wait_A_Bit'},
autonomy={'finished': Autonomy.Inherit})
return _state_machine
def create(self):
plug_in_affordance = "plug_in"
plug_out_affordance = "plug_out"
arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM
# x:202 y:568, x:374 y:401
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.none = None
_state_machine.userdata.hand_side = self.hand_side
_state_machine.userdata.grasp_preference = 0
_state_machine.userdata.step_back_distance = 1.0 # meters
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:588 y:126, x:324 y:44
_sm_back_to_pregrasp_0 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['template_id', 'hand_side', 'grasp_preference'])
with _sm_back_to_pregrasp_0:
# x:30 y:40
OperatableStateMachine.add('Get_Pregrasp',
GetTemplatePregraspState(),
transitions={
'done': 'Extract_Frame_Id',
'failed': 'failed',
'not_available': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full,
'not_available': Autonomy.Full
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'preference': 'grasp_preference',
'pre_grasp': 'grasp_pose'
})
# x:242 y:292
OperatableStateMachine.add(
'Plan_To_Pregrasp',
PlanEndeffectorCartesianWaypointsState(
ignore_collisions=True,
include_torso=False,
keep_endeffector_orientation=False,
allow_incomplete_plans=True,
vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault"),
transitions={
'planned': 'Move_To_Pregrasp_Pose',
'incomplete': 'Move_To_Pregrasp_Pose',
'failed': 'Get_Pregrasp'
},
autonomy={
'planned': Autonomy.Low,
'incomplete': Autonomy.High,
'failed': Autonomy.Full
},
remapping={
'waypoints': 'grasp_waypoints',
'hand': 'hand_side',
'frame_id': 'grasp_frame_id',
'joint_trajectory': 'joint_trajectory',
'plan_fraction': 'plan_fraction'
})
# x:568 y:295
OperatableStateMachine.add(
'Move_To_Pregrasp_Pose',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={
'done': 'finished',
'failed': 'Get_Pregrasp'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full
},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:41 y:178
OperatableStateMachine.add(
'Extract_Frame_Id',
CalculationState(
calculation=lambda pose: pose.header.frame_id),
transitions={'done': 'Convert_Waypoints'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'grasp_pose',
'output_value': 'grasp_frame_id'
})
# x:40 y:293
OperatableStateMachine.add(
'Convert_Waypoints',
CalculationState(calculation=lambda msg: [msg.pose]),
transitions={'done': 'Plan_To_Pregrasp'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'grasp_pose',
'output_value': 'grasp_waypoints'
})
# x:598 y:30, x:220 y:148, x:1035 y:174
_sm_go_to_grasp_1 = OperatableStateMachine(
outcomes=['finished', 'failed', 'again'],
input_keys=['hand_side', 'grasp_preference', 'template_id'],
output_keys=['grasp_preference'])
with _sm_go_to_grasp_1:
# x:33 y:49
OperatableStateMachine.add('Get_Grasp_Info',
GetTemplateGraspState(),
transitions={
'done': 'Extract_Frame_Id',
'failed': 'failed',
'not_available': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full,
'not_available': Autonomy.Full
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'preference': 'grasp_preference',
'grasp': 'grasp_pose'
})
# x:40 y:293
OperatableStateMachine.add(
'Convert_Waypoints',
CalculationState(calculation=lambda msg: [msg.pose]),
transitions={'done': 'Plan_To_Grasp'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'grasp_pose',
'output_value': 'grasp_waypoints'
})
# x:242 y:292
OperatableStateMachine.add(
'Plan_To_Grasp',
PlanEndeffectorCartesianWaypointsState(
ignore_collisions=True,
include_torso=False,
keep_endeffector_orientation=False,
allow_incomplete_plans=True,
vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault"),
transitions={
'planned': 'Move_To_Grasp_Pose',
'incomplete': 'Move_To_Grasp_Pose',
'failed': 'Decide_Which_Grasp'
},
autonomy={
'planned': Autonomy.High,
'incomplete': Autonomy.High,
'failed': Autonomy.Full
},
remapping={
'waypoints': 'grasp_waypoints',
'hand': 'hand_side',
'frame_id': 'grasp_frame_id',
'joint_trajectory': 'joint_trajectory',
'plan_fraction': 'plan_fraction'
})
# x:494 y:175
OperatableStateMachine.add(
'Move_To_Grasp_Pose',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={
'done': 'Optional_Template_Adjustment',
'failed': 'Decide_Which_Grasp'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full
},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:970 y:294
OperatableStateMachine.add(
'Increase_Preference_Index',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'again'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'grasp_preference',
'output_value': 'grasp_preference'
})
# x:41 y:178
OperatableStateMachine.add(
'Extract_Frame_Id',
CalculationState(
calculation=lambda pose: pose.header.frame_id),
transitions={'done': 'Convert_Waypoints'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'grasp_pose',
'output_value': 'grasp_frame_id'
})
# x:727 y:50
OperatableStateMachine.add(
'Optional_Template_Adjustment',
OperatorDecisionState(
outcomes=["grasp", "pregrasp", "skip"],
hint="Consider adjusting the template's pose",
suggestion="skip"),
transitions={
'grasp': 'Get_Grasp_Info',
'pregrasp': 'again',
'skip': 'finished'
},
autonomy={
'grasp': Autonomy.Full,
'pregrasp': Autonomy.Full,
'skip': Autonomy.High
})
# x:754 y:294
OperatableStateMachine.add(
'Decide_Which_Grasp',
OperatorDecisionState(
outcomes=["same", "next"],
hint='Try the same grasp or the next one?',
suggestion='same'),
transitions={
'same': 'Optional_Template_Adjustment',
'next': 'Increase_Preference_Index'
},
autonomy={
'same': Autonomy.High,
'next': Autonomy.High
})
# x:596 y:113, x:351 y:62
_sm_go_to_pregrasp_2 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['hand_side', 'grasp_preference', 'template_id'],
output_keys=['grasp_preference', 'pregrasp_pose'])
with _sm_go_to_pregrasp_2:
# x:27 y:68
OperatableStateMachine.add('Get_Pregrasp_Info',
GetTemplatePregraspState(),
transitions={
'done': 'Plan_To_Pregrasp_Pose',
'failed': 'failed',
'not_available': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full,
'not_available': Autonomy.Full
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'preference': 'grasp_preference',
'pre_grasp': 'pregrasp_pose'
})
# x:537 y:228
OperatableStateMachine.add(
'Move_To_Pregrasp_Pose',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={
'done': 'finished',
'failed': 'Decide_Which_Pregrasp'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full
},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:25 y:328
OperatableStateMachine.add(
'Increase_Preference_Index',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'Get_Pregrasp_Info'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'grasp_preference',
'output_value': 'grasp_preference'
})
# x:266 y:228
OperatableStateMachine.add(
'Plan_To_Pregrasp_Pose',
PlanEndeffectorPoseState(
ignore_collisions=False,
include_torso=False,
allowed_collisions=[],
planner_id="RRTConnectkConfigDefault"),
transitions={
'planned': 'Move_To_Pregrasp_Pose',
'failed': 'Decide_Which_Pregrasp'
},
autonomy={
'planned': Autonomy.Low,
'failed': Autonomy.Full
},
remapping={
'target_pose': 'pregrasp_pose',
'hand': 'hand_side',
'joint_trajectory': 'joint_trajectory'
})
# x:266 y:327
OperatableStateMachine.add(
'Decide_Which_Pregrasp',
OperatorDecisionState(
outcomes=["same", "next"],
hint='Try the same pregrasp or the next one?',
suggestion='same'),
transitions={
'same': 'Get_Pregrasp_Info',
'next': 'Increase_Preference_Index'
},
autonomy={
'same': Autonomy.High,
'next': Autonomy.Full
})
# x:30 y:365, x:130 y:365
_sm_perform_step_back_3 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['step_back_distance'])
with _sm_perform_step_back_3:
# x:78 y:78
OperatableStateMachine.add(
'Plan_Steps_Back',
FootstepPlanRelativeState(
direction=FootstepPlanRelativeState.DIRECTION_BACKWARD),
transitions={
'planned': 'Do_Steps_Back',
'failed': 'failed'
},
autonomy={
'planned': Autonomy.High,
'failed': Autonomy.Full
},
remapping={
'distance': 'step_back_distance',
'plan_header': 'plan_header'
})
# x:74 y:228
OperatableStateMachine.add(
'Do_Steps_Back',
ExecuteStepPlanActionState(),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Low,
'failed': Autonomy.Full
},
remapping={'plan_header': 'plan_header'})
# x:550 y:574, x:130 y:365
_sm_plug_in_cable_4 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'target_template_id', 'hand_side', 'grasp_preference',
'template_id', 'template_pose'
])
with _sm_plug_in_cable_4:
# x:82 y:59
OperatableStateMachine.add('Go_to_Pregrasp',
_sm_go_to_pregrasp_2,
transitions={
'finished': 'Go_to_Grasp',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'hand_side': 'hand_side',
'grasp_preference':
'grasp_preference',
'template_id': 'target_template_id',
'pregrasp_pose': 'pregrasp_pose'
})
# x:322 y:171
OperatableStateMachine.add('Go_to_Grasp',
_sm_go_to_grasp_1,
transitions={
'finished': 'Detach_Cable',
'failed': 'failed',
'again': 'Go_to_Pregrasp'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'again': Autonomy.Inherit
},
remapping={
'hand_side': 'hand_side',
'grasp_preference':
'grasp_preference',
'template_id': 'target_template_id'
})
# x:516 y:306
OperatableStateMachine.add('Open_Fingers',
FingerConfigurationState(
hand_type=self.hand_type,
configuration=0),
transitions={
'done': 'Back_To_Pregrasp',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full
},
remapping={'hand_side': 'hand_side'})
# x:472 y:406
OperatableStateMachine.add('Back_To_Pregrasp',
_sm_back_to_pregrasp_0,
transitions={
'finished': 'Close_Fingers',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'grasp_preference':
'grasp_preference'
})
# x:314 y:524
OperatableStateMachine.add('Close_Fingers',
FingerConfigurationState(
hand_type=self.hand_type,
configuration=1),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full
},
remapping={'hand_side': 'hand_side'})
# x:527 y:204
OperatableStateMachine.add('Detach_Cable',
DetachObjectState(),
transitions={
'done': 'Open_Fingers',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full
},
remapping={
'template_id': 'template_id',
'template_pose': 'template_pose'
})
# x:30 y:365, x:115 y:197
_sm_take_cable_5 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['template_id', 'hand_side', 'none'])
with _sm_take_cable_5:
# x:81 y:54
OperatableStateMachine.add(
'Get_Plug_Out_Affordance',
GetTemplateAffordanceState(identifier=plug_out_affordance),
transitions={
'done': 'Plan_Plug_Out',
'failed': 'failed',
'not_available': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full,
'not_available': Autonomy.Full
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'affordance': 'affordance'
})
# x:348 y:73
OperatableStateMachine.add(
'Plan_Plug_Out',
PlanAffordanceState(vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault"),
transitions={
'done': 'Execute_Plug_Out',
'incomplete': 'Execute_Plug_Out',
'failed': 'failed'
},
autonomy={
'done': Autonomy.High,
'incomplete': Autonomy.High,
'failed': Autonomy.Full
},
remapping={
'affordance': 'affordance',
'hand': 'hand_side',
'reference_point': 'none',
'joint_trajectory': 'joint_trajectory',
'plan_fraction': 'plan_fraction'
})
# x:322 y:255
OperatableStateMachine.add(
'Execute_Plug_Out',
ExecuteTrajectoryMsgState(controller=affordance_controller),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full
},
remapping={'joint_trajectory': 'joint_trajectory'})
with _state_machine:
# x:73 y:78
OperatableStateMachine.add(
'Request_Cable_Template',
InputState(request=InputState.SELECTED_OBJECT_ID,
message="Place cable template"),
transitions={
'received': 'Decide_Walking',
'aborted': 'failed',
'no_connection': 'failed',
'data_error': 'failed'
},
autonomy={
'received': Autonomy.Low,
'aborted': Autonomy.Full,
'no_connection': Autonomy.Full,
'data_error': Autonomy.Full
},
remapping={'data': 'template_id'})
# x:330 y:172
OperatableStateMachine.add(
'Walk_To_Template',
self.use_behavior(WalktoTemplateSM, 'Walk_To_Template'),
transitions={
'finished': 'Set_Manipulate',
'failed': 'failed',
'aborted': 'Set_Manipulate'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'aborted': Autonomy.Inherit
},
remapping={
'grasp_preference': 'grasp_preference',
'hand_side': 'hand_side',
'template_id': 'template_id'
})
# x:337 y:78
OperatableStateMachine.add('Decide_Walking',
OperatorDecisionState(
outcomes=["walk", "stand"],
hint="Walk to template?",
suggestion="walk"),
transitions={
'walk': 'Walk_To_Template',
'stand': 'Set_Manipulate'
},
autonomy={
'walk': Autonomy.High,
'stand': Autonomy.Full
})
# x:839 y:72
OperatableStateMachine.add('Grasp Object',
self.use_behavior(
GraspObjectSM, 'Grasp Object'),
transitions={
'finished': 'Attach_Cable',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'hand_side': 'hand_side',
'template_id': 'template_id'
})
# x:843 y:302
OperatableStateMachine.add('Take_Cable',
_sm_take_cable_5,
transitions={
'finished':
'Request_Target_Template',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'none': 'none'
})
# x:566 y:78
OperatableStateMachine.add(
'Set_Manipulate',
ChangeControlModeActionState(
target_mode=ChangeControlModeActionState.MANIPULATE),
transitions={
'changed': 'Grasp Object',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Low,
'failed': Autonomy.Full
})
# x:822 y:545
OperatableStateMachine.add('Plug_In_Cable',
_sm_plug_in_cable_4,
transitions={
'finished': 'Warn_Stand',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'target_template_id':
'target_template_id',
'hand_side': 'hand_side',
'grasp_preference':
'grasp_preference',
'template_id': 'template_id',
'template_pose': 'template_pose'
})
# x:826 y:185
OperatableStateMachine.add('Attach_Cable',
AttachObjectState(),
transitions={
'done': 'Take_Cable',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'template_pose': 'template_pose'
})
# x:811 y:400
OperatableStateMachine.add(
'Request_Target_Template',
InputState(request=InputState.SELECTED_OBJECT_ID,
message="Place target template"),
transitions={
'received': 'Plug_In_Cable',
'aborted': 'failed',
'no_connection': 'failed',
'data_error': 'failed'
},
autonomy={
'received': Autonomy.Low,
'aborted': Autonomy.High,
'no_connection': Autonomy.Full,
'data_error': Autonomy.Full
},
remapping={'data': 'target_template_id'})
# x:753 y:646
OperatableStateMachine.add(
'Warn_Stand',
LogState(text="Going to stand pose",
severity=Logger.REPORT_INFO),
transitions={'done': 'Go_To_Stand_Pose'},
autonomy={'done': Autonomy.High})
# x:581 y:573
OperatableStateMachine.add(
'Go_To_Stand_Pose',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.STAND_POSE,
vel_scaling=0.1,
ignore_collisions=False,
link_paddings={},
is_cartesian=False),
transitions={
'done': 'Set_Stand',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Full
},
remapping={'side': 'none'})
# x:446 y:653
OperatableStateMachine.add('Decide_Step_Back',
OperatorDecisionState(
outcomes=["walk", "stand"],
hint="Step back?",
suggestion="walk"),
transitions={
'walk': 'Perform_Step_Back',
'stand': 'finished'
},
autonomy={
'walk': Autonomy.High,
'stand': Autonomy.Full
})
# x:100 y:661
OperatableStateMachine.add(
'Perform_Step_Back',
_sm_perform_step_back_3,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'step_back_distance': 'step_back_distance'})
# x:383 y:527
OperatableStateMachine.add(
'Set_Stand',
ChangeControlModeActionState(
target_mode=ChangeControlModeActionState.STAND),
transitions={
'changed': 'Decide_Step_Back',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Low,
'failed': Autonomy.Full
})
return _state_machine
def create(self):
# x:807 y:293, x:1163 y:18
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:35 y:227, x:161 y:228, x:363 y:243, x:536 y:22, x:472 y:241, x:547 y:181, x:544 y:99, x:674 y:184, x:263 y:228
_sm_container_0 = ConcurrencyContainer(
outcomes=['finished', 'failed', 'danger', 'preempted'],
input_keys=['plan'],
conditions=[('failed', [('DWA', 'failed')]),
('finished', [('DWA', 'done')]),
('preempted', [('DWA', 'preempted')]),
('danger', [('Safety', 'cliff')]),
('danger', [('Safety', 'bumper')])])
with _sm_container_0:
# x:101 y:78
OperatableStateMachine.add(
'DWA',
FollowPathState(topic="low_level_planner"),
transitions={
'done': 'finished',
'failed': 'failed',
'preempted': 'preempted'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off,
'preempted': Autonomy.Off
},
remapping={'plan': 'plan'})
# x:343 y:91
OperatableStateMachine.add(
'Safety',
TurtlebotStatusState(bumper_topic='mobile_base/events/bumper',
cliff_topic='mobile_base/events/cliff'),
transitions={
'bumper': 'danger',
'cliff': 'danger'
},
autonomy={
'bumper': Autonomy.Off,
'cliff': Autonomy.Off
})
with _state_machine:
# x:193 y:26
OperatableStateMachine.add('ClearCostmap',
ClearCostmapsState(costmap_topics=[
'high_level_planner/clear_costmap',
'low_level_planner/clear_costmap'
],
timeout=5.0),
transitions={
'done': 'Receive Goal',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
})
# x:205 y:207
OperatableStateMachine.add(
'Receive Path',
GetPathState(planner_topic="high_level_planner"),
transitions={
'planned': 'ExecutePlan',
'empty': 'Continue',
'failed': 'Continue'
},
autonomy={
'planned': Autonomy.Off,
'empty': Autonomy.Low,
'failed': Autonomy.Low
},
remapping={
'goal': 'goal',
'plan': 'plan'
})
# x:194 y:301
OperatableStateMachine.add('ExecutePlan',
OperatorDecisionState(
outcomes=["yes", "no"],
hint="Execute the current plan?",
suggestion="yes"),
transitions={
'yes': 'Container',
'no': 'Continue'
},
autonomy={
'yes': Autonomy.High,
'no': Autonomy.Full
})
# x:446 y:275
OperatableStateMachine.add('Container',
_sm_container_0,
transitions={
'finished': 'Log Success',
'failed': 'AutoReplan',
'danger': 'EStop',
'preempted': 'Continue'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'danger': Autonomy.Inherit,
'preempted': Autonomy.Inherit
},
remapping={'plan': 'plan'})
# x:435 y:146
OperatableStateMachine.add(
'Continue',
OperatorDecisionState(
outcomes=["yes", "no", "recover", "clearcostmap"],
hint="Continue planning to new goal?",
suggestion="yes"),
transitions={
'yes': 'Receive Goal',
'no': 'finished',
'recover': 'LogRecovery',
'clearcostmap': 'ClearCostmap'
},
autonomy={
'yes': Autonomy.High,
'no': Autonomy.Full,
'recover': Autonomy.Full,
'clearcostmap': Autonomy.Full
})
# x:1052 y:227
OperatableStateMachine.add('Turtlebot Simple Recovery',
self.use_behavior(
TurtlebotSimpleRecoverySM,
'Turtlebot Simple Recovery'),
transitions={
'finished': 'AutoReplan',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:664 y:307
OperatableStateMachine.add('Log Success',
LogState(text="Success!",
severity=Logger.REPORT_HINT),
transitions={'done': 'Continue'},
autonomy={'done': Autonomy.Off})
# x:664 y:374
OperatableStateMachine.add('Log Fail',
LogState(text="Path execution failure",
severity=Logger.REPORT_HINT),
transitions={'done': 'Recover'},
autonomy={'done': Autonomy.Off})
# x:960 y:70
OperatableStateMachine.add('Log Recovered',
LogState(text="Re-plan after recovery",
severity=Logger.REPORT_HINT),
transitions={'done': 'New Plan'},
autonomy={'done': Autonomy.Off})
# x:772 y:71
OperatableStateMachine.add(
'New Plan',
GetPathState(planner_topic="high_level_planner"),
transitions={
'planned': 'Container',
'empty': 'Receive Goal',
'failed': 'Continue'
},
autonomy={
'planned': Autonomy.Off,
'empty': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'goal': 'goal',
'plan': 'plan'
})
# x:203 y:113
OperatableStateMachine.add(
'Receive Goal',
GetPoseState(topic='flex_nav_global/goal'),
transitions={'done': 'Receive Path'},
autonomy={'done': Autonomy.Low},
remapping={'goal': 'goal'})
# x:866 y:374
OperatableStateMachine.add('Recover',
OperatorDecisionState(
outcomes=["yes", "no"],
hint="Should we attempt recovery?",
suggestion="yes"),
transitions={
'yes': 'LogRecovery',
'no': 'finished'
},
autonomy={
'yes': Autonomy.High,
'no': Autonomy.Full
})
# x:887 y:236
OperatableStateMachine.add(
'LogRecovery',
LogState(text="Starting recovery behavior",
severity=Logger.REPORT_HINT),
transitions={'done': 'Turtlebot Simple Recovery'},
autonomy={'done': Autonomy.Off})
# x:875 y:162
OperatableStateMachine.add('AutoReplan',
OperatorDecisionState(
outcomes=["yes", "no"],
hint="Re-plan to current goal?",
suggestion="yes"),
transitions={
'yes': 'Log Recovered',
'no': 'Continue'
},
autonomy={
'yes': Autonomy.High,
'no': Autonomy.Full
})
# x:453 y:376
OperatableStateMachine.add(
'EStop',
TimedStopState(timeout=0.25,
cmd_topic='stamped_cmd_vel_mux/input/navi',
odom_topic='mobile_base/odom'),
transitions={
'done': 'Log Fail',
'failed': 'Log Fail'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
})
return _state_machine
def create(self):
# x:702 y:576, x:764 y:158, x:766 y:33
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed', 'critical_fail'],
input_keys=['Action'])
_state_machine.userdata.Action = ["Place", "table"]
_state_machine.userdata.Empty = None
_state_machine.userdata.IdlePos = "IdlePose"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:44 y:28
OperatableStateMachine.add(
'gripper contain',
GetRosParam(ParamName="behavior/GripperContent"),
transitions={
'done': 'if contain something',
'failed': 'cause1'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'Value': 'content'})
# x:222 y:497
OperatableStateMachine.add('Action_place',
self.use_behavior(
Action_placeSM, 'Action_place'),
transitions={
'finished': 'idlearm',
'failed': 'cause3'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'pos': 'MapPosition'})
# x:39 y:367
OperatableStateMachine.add(
'genPoseArm',
GenPoseEuler(x=0.75, y=-0.25, z=0.85, roll=0, pitch=0, yaw=0),
transitions={'done': 'referential from robot to map'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'position'})
# x:8 y:433
OperatableStateMachine.add('referential from robot to map',
TF_transformation(in_ref="base_link",
out_ref="map"),
transitions={
'done': 'log pose',
'fail': 'log tf error'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'in_pos': 'position',
'out_pos': 'MapPosition'
})
# x:25 y:98
OperatableStateMachine.add(
'if contain something',
CheckConditionState(predicate=lambda x: x != ''),
transitions={
'true': 'cond',
'false': 'say nothing in gripper'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'content'})
# x:209 y:98
OperatableStateMachine.add(
'say nothing in gripper',
SaraSay(sentence="It seems I have nothing in my gripper",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'cause1'},
autonomy={'done': Autonomy.Off})
# x:28 y:236
OperatableStateMachine.add(
'construction phrase',
FlexibleCalculationState(
calculation=lambda x: "I will place this " + str(x[
0]) + " on the " + str(x[1][1]),
input_keys=["content", "Action"]),
transitions={'done': 'Say_Place_object'},
autonomy={'done': Autonomy.Off},
remapping={
'content': 'content',
'Action': 'Action',
'output_value': 'sentence'
})
# x:33 y:167
OperatableStateMachine.add(
'cond',
CheckConditionState(predicate=lambda x: x[1] != ''),
transitions={
'true': 'construction phrase',
'false': 'cause2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'Action'})
# x:257 y:413
OperatableStateMachine.add('log tf error',
LogState(text="tf error",
severity=Logger.REPORT_HINT),
transitions={'done': 'cause3'},
autonomy={'done': Autonomy.Off})
# x:42 y:502
OperatableStateMachine.add(
'log pose',
LogKeyState(text="the placement pose will be: {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'Action_place'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'MapPosition'})
# x:493 y:535
OperatableStateMachine.add(
'empty hand',
SetRosParam(ParamName="behavior/GripperContent"),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'Value': 'Empty'})
# x:448 y:54
OperatableStateMachine.add(
'cause1',
SetKey(Value="I didn't have any object in my gripper"),
transitions={'done': 'setrosparamcause'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Key'})
# x:422 y:149
OperatableStateMachine.add(
'cause2',
SetKey(Value="I didn't know where to place the object."),
transitions={'done': 'setrosparamcause'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Key'})
# x:575 y:158
OperatableStateMachine.add(
'setrosparamcause',
SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off},
remapping={'Value': 'Key'})
# x:449 y:325
OperatableStateMachine.add(
'cause3',
SetKey(
Value="I was unable to calculate how to place the object."
),
transitions={'done': 'setrosparamcause'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'Key'})
# x:342 y:583
OperatableStateMachine.add('idlearm',
MoveitMove(move=True,
waitForExecution=False,
group="RightArm",
watchdog=15),
transitions={
'done': 'empty hand',
'failed': 'empty hand'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'IdlePos'})
# x:706 y:460
OperatableStateMachine.add(
'Say_Place_It_This_Place',
SaraSay(sentence=lambda x: "I will place this " + x +
" right there.",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:35 y:301
OperatableStateMachine.add('Say_Place_object',
SaraSay(sentence=lambda x: x,
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'genPoseArm'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:284 y:484, x:47 y:223
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:454, x:130 y:454
_sm_take_photo_container_(guess_age)_0 = OperatableStateMachine(outcomes=['finished', 'failed'])
with _sm_take_photo_container_(guess_age)_0:
# x:124 y:43
OperatableStateMachine.add('ok if you are ready show me a big smile',
TalkState(sentence_number=3),
transitions={'continue': 'smile', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:699 y:57
OperatableStateMachine.add('wait as if take photo',
WaitState(wait_time=3),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:481 y:41
OperatableStateMachine.add('smile',
FacialExpressionState(expression_num=1),
transitions={'continue': 'wait as if take photo', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
with _state_machine:
# x:109 y:52
OperatableStateMachine.add('launch video',
LaunchVideoStream(),
transitions={'continue': 'starting object track talk', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:514 y:19
OperatableStateMachine.add('detect and track',
LaunchObjDetectAndTrack(),
transitions={'continue': 'subscriber_is_nearby_face_State', 'failed': 'stop face detect'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:734 y:20
OperatableStateMachine.add('subscriber_is_nearby_face_State',
SubscriberState(topic='/is_person_nearby', blocking=True, clear=False),
transitions={'received': 'check_if_nearby', 'unavailable': 'stop obj detect state'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'message'})
# x:732 y:157
OperatableStateMachine.add('check_if_nearby',
CheckConditionState(predicate=lambda message:message.data == "yes"),
transitions={'true': 'stop obj detect state', 'false': 'logger'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'message'})
# x:588 y:115
OperatableStateMachine.add('logger',
LogState(text="false", severity=Logger.REPORT_HINT),
transitions={'done': 'subscriber_is_nearby_face_State'},
autonomy={'done': Autonomy.Off})
# x:935 y:201
OperatableStateMachine.add('stop obj detect state',
StopObjectDetectAndTrack(),
transitions={'continue': 'face identified', 'failed': 'stop face detect'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:960 y:281
OperatableStateMachine.add('launch face detect and track',
FaceDetTrack(),
transitions={'continue': 'wait to show face trk', 'failed': 'stop face detect'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:1178 y:285
OperatableStateMachine.add('wait to show face trk',
WaitState(wait_time=5),
transitions={'done': 'talk hello'},
autonomy={'done': Autonomy.Off})
# x:1165 y:361
OperatableStateMachine.add('talk hello',
TalkState(sentence_number=2),
transitions={'continue': 'listen_for_input_state', 'failed': 'stop face detect'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:342 y:308
OperatableStateMachine.add('stop face detect',
StopFaceDetectAndTrack(),
transitions={'continue': 'finished', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:967 y:38
OperatableStateMachine.add('face identified',
TalkState(sentence_number=100),
transitions={'continue': 'start face track', 'failed': 'stop obj detect state'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:1171 y:33
OperatableStateMachine.add('start face track',
TalkState(sentence_number=101),
transitions={'continue': 'launch face detect and track', 'failed': 'stop obj detect state'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:273 y:126
OperatableStateMachine.add('starting object track talk',
TalkState(sentence_number=99),
transitions={'continue': 'detect and track', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:1164 y:441
OperatableStateMachine.add('listen_for_input_state',
ListeningState(),
transitions={'continue': 'subscribe to user input1', 'failed': 'stop face detect'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:1160 y:516
OperatableStateMachine.add('subscribe to user input1',
SubscriberState(topic='/stt_topic', blocking=True, clear=False),
transitions={'received': 'check user input1', 'unavailable': 'stop face detect'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'message_stt'})
# x:1180 y:590
OperatableStateMachine.add('check user input1',
CheckConditionState(predicate=lambda message: message.query =="please take my photo"),
transitions={'true': 'yes take photo', 'false': 'no take photo not received'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'message_stt'})
# x:1005 y:585
OperatableStateMachine.add('yes take photo',
LogState(text="yes take photo recieved", severity=Logger.REPORT_HINT),
transitions={'done': 'take photo container (guess age)'},
autonomy={'done': Autonomy.Off})
# x:1367 y:517
OperatableStateMachine.add('no take photo not received',
LogState(text="no not recieved take photo", severity=Logger.REPORT_HINT),
transitions={'done': 'listen_for_input_state'},
autonomy={'done': Autonomy.Off})
# x:727 y:532
OperatableStateMachine.add('take photo container (guess age)',
_sm_take_photo_container_(guess_age)_0,
transitions={'finished': 'left container', 'failed': 'stop face detect'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:585 y:606
OperatableStateMachine.add('left container',
LogState(text="left container - now back on main", severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:30 y:365, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:54 y:385, x:100 y:523
_sm_safety_monitor_0 = OperatableStateMachine(
outcomes=['finished', 'failed'])
with _sm_safety_monitor_0:
# x:289 y:57
OperatableStateMachine.add('subscribe to safety topic 1',
SubscriberState(topic='/chatter',
blocking=True,
clear=False),
transitions={
'received':
'check if topic publishes warning',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:719 y:64
OperatableStateMachine.add(
'fine continue log',
LogState(text="continue log", severity=Logger.REPORT_HINT),
transitions={'done': 'subscribe to safety topic 1'},
autonomy={'done': Autonomy.Off})
# x:690 y:202
OperatableStateMachine.add(
'check if topic publishes warning',
CheckConditionState(
predicate=lambda message: message.data == "1"),
transitions={
'true': 'fine continue log',
'false': 'stop now log'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'message'})
# x:451 y:481
OperatableStateMachine.add('stop now log',
LogState(
text="stopping - message receieved",
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:30 y:456, x:130 y:456
_sm_normal_activity_1 = OperatableStateMachine(
outcomes=['finished', 'failed'])
with _sm_normal_activity_1:
# x:338 y:227
OperatableStateMachine.add('normalactivity',
LogState(text="normal activity",
severity=Logger.REPORT_HINT),
transitions={'done': 'wait1'},
autonomy={'done': Autonomy.Off})
# x:618 y:312
OperatableStateMachine.add('wait1',
WaitState(wait_time=1),
transitions={'done': 'normalactivity'},
autonomy={'done': Autonomy.Off})
# x:113 y:364, x:465 y:451, x:737 y:425, x:254 y:385, x:221 y:448
_sm_container_2 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('failed', [('normal_activity', 'failed'),
('safety_monitor', 'failed')]),
('finished', [('normal_activity', 'finished')]),
('finished', [('safety_monitor', 'finished')])])
with _sm_container_2:
# x:149 y:54
OperatableStateMachine.add('normal_activity',
_sm_normal_activity_1,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:676 y:54
OperatableStateMachine.add('safety_monitor',
_sm_safety_monitor_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
with _state_machine:
# x:317 y:100
OperatableStateMachine.add('Container',
_sm_container_2,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
return _state_machine
def create(self):
# x:787 y:587, x:280 y:361
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.hand_side = self.hand_side
_state_machine.userdata.none = None
_state_machine.userdata.grasp_preference = 0
_state_machine.userdata.step_back_distance = 0.5 # m
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:686 y:240, x:384 y:196
_sm_perform_walking_0 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['none', 'step_back_distance'])
with _sm_perform_walking_0:
# x:64 y:78
OperatableStateMachine.add(
'Plan_Realign_Feet',
FootstepPlanRealignCenterState(),
transitions={
'planned': 'Execute_Realign_Feet',
'failed': 'failed'
},
autonomy={
'planned': Autonomy.High,
'failed': Autonomy.Low
},
remapping={'plan_header': 'plan_header'})
# x:624 y:378
OperatableStateMachine.add(
'Perform_Step_Back',
ExecuteStepPlanActionState(),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Low,
'failed': Autonomy.Low
},
remapping={'plan_header': 'plan_header'})
# x:328 y:378
OperatableStateMachine.add(
'Plan_Step_Back',
FootstepPlanRelativeState(
direction=FootstepPlanRelativeState.DIRECTION_BACKWARD),
transitions={
'planned': 'Perform_Step_Back',
'failed': 'failed'
},
autonomy={
'planned': Autonomy.High,
'failed': Autonomy.Low
},
remapping={
'distance': 'step_back_distance',
'plan_header': 'plan_header'
})
# x:74 y:190
OperatableStateMachine.add(
'Execute_Realign_Feet',
ExecuteStepPlanActionState(),
transitions={
'finished': 'Wait_For_Stand',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Low,
'failed': Autonomy.Low
},
remapping={'plan_header': 'plan_header'})
# x:66 y:316
OperatableStateMachine.add(
'Wait_For_Stand',
CheckCurrentControlModeState(
target_mode=CheckCurrentControlModeState.STAND, wait=True),
transitions={
'correct': 'Plan_Step_Back',
'incorrect': 'failed'
},
autonomy={
'correct': Autonomy.Low,
'incorrect': Autonomy.Full
},
remapping={'control_mode': 'control_mode'})
# x:733 y:290, x:133 y:290
_sm_back_to_stand_1 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['none'])
with _sm_back_to_stand_1:
# x:66 y:78
OperatableStateMachine.add(
'Set_Manipulate',
ChangeControlModeActionState(
target_mode=ChangeControlModeActionState.MANIPULATE),
transitions={
'changed': 'Stand_Posture',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.High,
'failed': Autonomy.Low
})
# x:376 y:78
OperatableStateMachine.add(
'Stand_Posture',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.STAND_POSE,
vel_scaling=0.3,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'Set_Stand',
'failed': 'Set_Stand'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Low
},
remapping={'side': 'none'})
# x:666 y:78
OperatableStateMachine.add(
'Set_Stand',
ChangeControlModeActionState(
target_mode=ChangeControlModeActionState.STAND),
transitions={
'changed': 'finished',
'failed': 'finished'
},
autonomy={
'changed': Autonomy.Low,
'failed': Autonomy.Low
})
# x:120 y:404, x:298 y:222
_sm_step_back_2 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'hand_side', 'none', 'template_id', 'grasp_preference',
'step_back_distance'
])
with _sm_step_back_2:
# x:76 y:28
OperatableStateMachine.add(
'Go_To_Stand_Posture',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.STAND_POSE,
vel_scaling=0.1,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'Set_To_Stand_Manipulate',
'failed': 'Set_To_Stand_Manipulate'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Low
},
remapping={'side': 'none'})
# x:66 y:140
OperatableStateMachine.add(
'Set_To_Stand_Manipulate',
ChangeControlModeActionState(
target_mode=ChangeControlModeActionState.STAND_MANIPULATE),
transitions={
'changed': 'Perform_Walking',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.High,
'failed': Autonomy.Low
})
# x:84 y:272
OperatableStateMachine.add('Perform_Walking',
_sm_perform_walking_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'none':
'none',
'step_back_distance':
'step_back_distance'
})
# x:755 y:48, x:401 y:428
_sm_preparation_3 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['none'],
output_keys=['template_id'])
with _sm_preparation_3:
# x:66 y:164
OperatableStateMachine.add(
'Head_Look_Straight',
TiltHeadState(desired_tilt=TiltHeadState.STRAIGHT),
transitions={
'done': 'Place_Template',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Low
})
# x:566 y:163
OperatableStateMachine.add(
'Request_Template_ID',
InputState(request=InputState.SELECTED_OBJECT_ID,
message="Provide the ID of the placed template."),
transitions={
'received': 'finished',
'aborted': 'failed',
'no_connection': 'Log_No_Connection',
'data_error': 'Log_Data_Error'
},
autonomy={
'received': Autonomy.High,
'aborted': Autonomy.High,
'no_connection': Autonomy.Low,
'data_error': Autonomy.Low
},
remapping={'data': 'template_id'})
# x:287 y:204
OperatableStateMachine.add('Log_No_Connection',
LogState(
text="Have no connection to OCS!",
severity=Logger.REPORT_ERROR),
transitions={'done': 'Decide_Input'},
autonomy={'done': Autonomy.Off})
# x:344 y:144
OperatableStateMachine.add('Log_Data_Error',
LogState(
text="Received wrong data format!",
severity=Logger.REPORT_ERROR),
transitions={'done': 'Decide_Input'},
autonomy={'done': Autonomy.Off})
# x:483 y:44
OperatableStateMachine.add(
'Fake_Input',
CalculationState(calculation=lambda x: 0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'none',
'output_value': 'template_id'
})
# x:236 y:40
OperatableStateMachine.add(
'Decide_Input',
OperatorDecisionState(
outcomes=['fake_id', 'ocs_request'],
hint="How do you want to provide the template?",
suggestion=None),
transitions={
'fake_id': 'Fake_Input',
'ocs_request': 'Request_Template_ID'
},
autonomy={
'fake_id': Autonomy.Full,
'ocs_request': Autonomy.Full
})
# x:78 y:40
OperatableStateMachine.add(
'Place_Template',
LogState(text="Please place the drill template.",
severity=Logger.REPORT_HINT),
transitions={'done': 'Decide_Input'},
autonomy={'done': Autonomy.Full})
with _state_machine:
# x:44 y:72
OperatableStateMachine.add('Preparation',
_sm_preparation_3,
transitions={
'finished': 'Ask_Perform_Walking',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'none': 'none',
'template_id': 'template_id'
})
# x:547 y:571
OperatableStateMachine.add('Step_Back',
_sm_step_back_2,
transitions={
'finished': 'finished',
'failed': 'Back_To_Stand'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'hand_side':
'hand_side',
'none':
'none',
'template_id':
'template_id',
'grasp_preference':
'grasp_preference',
'step_back_distance':
'step_back_distance'
})
# x:284 y:78
OperatableStateMachine.add(
'Ask_Perform_Walking',
OperatorDecisionState(
outcomes=['walk', 'stand'],
hint="Does the robot need to walk to the table?",
suggestion='walk'),
transitions={
'walk': 'Walk to Template',
'stand': 'Manipulation Config'
},
autonomy={
'walk': Autonomy.High,
'stand': Autonomy.Full
})
# x:531 y:22
OperatableStateMachine.add(
'Walk to Template',
self.use_behavior(WalktoTemplateSM, 'Walk to Template'),
transitions={
'finished': 'Manipulation Config',
'failed': 'Walk_Manually',
'aborted': 'Walk_Manually'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'aborted': Autonomy.Inherit
},
remapping={
'grasp_preference': 'grasp_preference',
'hand_side': 'hand_side',
'template_id': 'template_id'
})
# x:525 y:143
OperatableStateMachine.add(
'Manipulation Config',
self.use_behavior(ManipulationConfigSM, 'Manipulation Config'),
transitions={
'finished': 'Head_Look_Down',
'failed': 'Manipulation_Config_Manually'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:264 y:228
OperatableStateMachine.add(
'Manipulation_Config_Manually',
OperatorDecisionState(
outcomes=["done", "abort"],
hint="Make sure the robot is ready to grasp",
suggestion=None),
transitions={
'done': 'Pickup Object',
'abort': 'failed'
},
autonomy={
'done': Autonomy.Full,
'abort': Autonomy.Full
})
# x:769 y:78
OperatableStateMachine.add(
'Walk_Manually',
LogState(text="Guide the robot to the template manually.",
severity=Logger.REPORT_HINT),
transitions={'done': 'Manipulation Config'},
autonomy={'done': Autonomy.Full})
# x:245 y:485
OperatableStateMachine.add('Back_To_Stand',
_sm_back_to_stand_1,
transitions={
'finished': 'failed',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'none': 'none'})
# x:538 y:353
OperatableStateMachine.add('Pickup Object',
self.use_behavior(
PickupObjectSM, 'Pickup Object'),
transitions={
'finished': 'Head_Look_Straight',
'failed': 'Back_To_Stand'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'hand_side': 'hand_side',
'template_id': 'template_id'
})
# x:545 y:257
OperatableStateMachine.add(
'Head_Look_Down',
TiltHeadState(desired_tilt=TiltHeadState.DOWN_45),
transitions={
'done': 'Pickup Object',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Low
})
# x:540 y:473
OperatableStateMachine.add(
'Head_Look_Straight',
TiltHeadState(desired_tilt=TiltHeadState.STRAIGHT),
transitions={
'done': 'Step_Back',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Low
})
return _state_machine
def create(self):
cart_lift = 0.20 # meters
arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM
wrist_frame_id = self.hand_side[0] + '_hand'
# x:220 y:518, x:463 y:233
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['hand_side', 'template_id'])
_state_machine.userdata.hand_side = self.hand_side
_state_machine.userdata.template_id = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:71 y:304, x:597 y:174
_sm_lift_object_0 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['hand_side', 'template_id'])
with _sm_lift_object_0:
# x:255 y:28
OperatableStateMachine.add('Get_Wrist_Pose',
GetWristPoseState(),
transitions={
'done': 'Set_Target_for_Lifting',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={
'hand_side': 'hand_side',
'wrist_pose': 'wrist_pose'
})
# x:525 y:29
OperatableStateMachine.add(
'Set_Target_for_Lifting',
CalculationState(
calculation=self.lift_template(cart_lift, False)),
transitions={'done': 'Transform_to_Wrist_Frame'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'wrist_pose',
'output_value': 'lift_target'
})
# x:887 y:296
OperatableStateMachine.add(
'Prepare_Plan_Frame_Wrist',
CalculationState(
calculation=lambda pose: pose.header.frame_id),
transitions={'done': 'Plan_Cartesian_Lift'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'lift_target_hand',
'output_value': 'hand_frame_id'
})
# x:523 y:296
OperatableStateMachine.add(
'Plan_Cartesian_Lift',
PlanEndeffectorCartesianWaypointsState(
ignore_collisions=False,
include_torso=False,
keep_endeffector_orientation=True,
allow_incomplete_plans=True,
vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault"),
transitions={
'planned': 'Perform_Lifting',
'incomplete': 'Perform_Lifting',
'failed': 'failed'
},
autonomy={
'planned': Autonomy.Low,
'incomplete': Autonomy.High,
'failed': Autonomy.High
},
remapping={
'waypoints': 'lift_waypoints',
'hand': 'hand_side',
'frame_id': 'hand_frame_id',
'joint_trajectory': 'joint_trajectory',
'plan_fraction': 'plan_fraction'
})
# x:886 y:29
OperatableStateMachine.add(
'Transform_to_Wrist_Frame',
GetPoseInFrameState(target_frame=wrist_frame_id),
transitions={
'done': 'Convert_to_List_of_Poses',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={
'pose_in': 'lift_target',
'pose_out': 'lift_target_hand'
})
# x:888 y:160
OperatableStateMachine.add(
'Convert_to_List_of_Poses',
CalculationState(calculation=lambda pose: [pose.pose]),
transitions={'done': 'Prepare_Plan_Frame_Wrist'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'lift_target_hand',
'output_value': 'lift_waypoints'
})
# x:206 y:297
OperatableStateMachine.add(
'Perform_Lifting',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'joint_trajectory': 'joint_trajectory'})
with _state_machine:
# x:187 y:25
OperatableStateMachine.add('Grasp_Object',
self.use_behavior(
GraspObjectSM, 'Grasp_Object'),
transitions={
'finished':
'Confirm-Successful_Grasp',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'hand_side': 'hand_side',
'template_id': 'template_id'
})
# x:181 y:275
OperatableStateMachine.add('Attach_Object_to_Hand',
AttachObjectState(),
transitions={
'done': 'Lift_Object',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'template_pose': 'template_pose'
})
# x:190 y:383
OperatableStateMachine.add('Lift_Object',
_sm_lift_object_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'hand_side': 'hand_side',
'template_id': 'template_id'
})
# x:176 y:159
OperatableStateMachine.add(
'Confirm-Successful_Grasp',
LogState(text='Confirm that the grasp was successful!',
severity=Logger.REPORT_HINT),
transitions={'done': 'Attach_Object_to_Hand'},
autonomy={'done': Autonomy.Full})
return _state_machine
def create(self):
# x:456 y:478, x:347 y:237
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:487 y:245
_sm_guess_age_interaction_0 = OperatableStateMachine(outcomes=['finished', 'failed'])
with _sm_guess_age_interaction_0:
# x:153 y:68
OperatableStateMachine.add('smile',
FacialExpressionState(expression_num=1),
transitions={'continue': 'wait 1', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:378 y:48
OperatableStateMachine.add('wait 1',
WaitState(wait_time=1),
transitions={'done': 'blink'},
autonomy={'done': Autonomy.Off})
# x:605 y:69
OperatableStateMachine.add('blink',
FacialExpressionState(expression_num=2),
transitions={'continue': 'introduction', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:817 y:120
OperatableStateMachine.add('introduction',
TalkState(sentence_number=1),
transitions={'continue': 'listen for name', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:919 y:202
OperatableStateMachine.add('listen for name',
ListeningState(),
transitions={'continue': 'welcome to', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:1004 y:291
OperatableStateMachine.add('welcome to',
TalkState(sentence_number=2),
transitions={'continue': 'sub please take my photo', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:1027 y:401
OperatableStateMachine.add('sub please take my photo',
SubscriberState(topic='/stt_topic', blocking=True, clear=False),
transitions={'received': 'cond please take my photo', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'message'})
# x:1025 y:515
OperatableStateMachine.add('cond please take my photo',
CheckConditionState(predicate=lambda message:message.data =="please take my photo"),
transitions={'true': 'cool go ahead', 'false': 'not heard take photo'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'message'})
# x:823 y:458
OperatableStateMachine.add('not heard take photo',
LogState(text=not yet heard "please take my photo", severity=Logger.REPORT_HINT),
transitions={'done': 'sub please take my photo'},
autonomy={'done': Autonomy.Off})
# x:938 y:641
OperatableStateMachine.add('cool go ahead',
TalkState(sentence_number=3),
transitions={'continue': 'finished', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:161 y:371, x:268 y:170
_sm_start_face_det_and_trk_1 = OperatableStateMachine(outcomes=['finished', 'failed'])
with _sm_start_face_det_and_trk_1:
# x:149 y:36
OperatableStateMachine.add('say face found',
TalkState(sentence_number=100),
transitions={'continue': 'face det trk', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:519 y:73
OperatableStateMachine.add('face det trk',
FaceDetTrack(),
transitions={'continue': 'say start face det trk', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:563 y:328
OperatableStateMachine.add('say start face det trk',
TalkState(sentence_number=101),
transitions={'continue': 'finished', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:128 y:223, x:130 y:365
_sm_stop_video_detects_and_tracks_2 = OperatableStateMachine(outcomes=['finished', 'failed'])
with _sm_stop_video_detects_and_tracks_2:
# x:135 y:64
OperatableStateMachine.add('stop obj det trk',
StopFaceDetectAndTrack(),
transitions={'continue': 'stop face det trk', 'failed': 'stop face det trk'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:410 y:78
OperatableStateMachine.add('stop face det trk',
StopFaceDetectAndTrack(),
transitions={'continue': 'stop video stream', 'failed': 'stop video stream'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:466 y:250
OperatableStateMachine.add('stop video stream',
StopCameraStream(),
transitions={'continue': 'finished', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:438 y:355, x:121 y:296
_sm_check_face_nearby_3 = OperatableStateMachine(outcomes=['finished', 'failed'])
with _sm_check_face_nearby_3:
# x:237 y:77
OperatableStateMachine.add('subscr face found topic',
SubscriberState(topic='/is_person_nearby', blocking=True, clear=False),
transitions={'received': 'face condition', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'message'})
# x:521 y:63
OperatableStateMachine.add('face condition',
CheckConditionState(predicate=lambda message:message.data == "yes"),
transitions={'true': 'finished', 'false': 'no face found yet'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'message'})
# x:297 y:218
OperatableStateMachine.add('no face found yet',
LogState(text="no face found yet", severity=Logger.REPORT_HINT),
transitions={'done': 'subscr face found topic'},
autonomy={'done': Autonomy.Off})
# x:30 y:365, x:132 y:205
_sm_launch_stream_and_object_detector_4 = OperatableStateMachine(outcomes=['finished', 'failed'])
with _sm_launch_stream_and_object_detector_4:
# x:126 y:42
OperatableStateMachine.add('launch video stream',
LaunchVideoStream(),
transitions={'continue': 'say launching video', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:389 y:56
OperatableStateMachine.add('say launching video',
TalkState(sentence_number=99),
transitions={'continue': 'launch obj det trk', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:483 y:152
OperatableStateMachine.add('launch obj det trk',
LaunchObjDetectAndTrack(),
transitions={'continue': 'finished', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
with _state_machine:
# x:135 y:38
OperatableStateMachine.add('launch stream and object detector',
_sm_launch_stream_and_object_detector_4,
transitions={'finished': 'check face nearby', 'failed': 'stop video detects and tracks'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:469 y:33
OperatableStateMachine.add('check face nearby',
_sm_check_face_nearby_3,
transitions={'finished': 'stop obj det trk', 'failed': 'stop video detects and tracks'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:546 y:220
OperatableStateMachine.add('stop video detects and tracks',
_sm_stop_video_detects_and_tracks_2,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:701 y:69
OperatableStateMachine.add('stop obj det trk',
StopObjectDetectAndTrack(),
transitions={'continue': 'start face det and trk', 'failed': 'stop video detects and tracks'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:892 y:135
OperatableStateMachine.add('start face det and trk',
_sm_start_face_det_and_trk_1,
transitions={'finished': 'guess age interaction', 'failed': 'stop video detects and tracks'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:887 y:356
OperatableStateMachine.add('guess age interaction',
_sm_guess_age_interaction_0,
transitions={'finished': 'finished', 'failed': 'stop video detects and tracks'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
return _state_machine
def create(self):
test = "test"
hello = "hello World!"
# x:1142 y:435, x:1117 y:356
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:104 y:43
OperatableStateMachine.add('Initial_Wait',
WaitState(wait_time=self.waiting_time),
transitions={'done': 'Print_Greeting'},
autonomy={'done': Autonomy.Off})
# x:920 y:287
OperatableStateMachine.add('Print_Bye',
LogState(text="bye bye", severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:285 y:53
OperatableStateMachine.add('Print_Greeting',
LogState(text=hello, severity=Logger.REPORT_HINT),
transitions={'done': 'state1'},
autonomy={'done': Autonomy.Off})
# x:785 y:217
OperatableStateMachine.add('Initial_Wait_2',
WaitState(wait_time=5),
transitions={'done': 'Print_Bye'},
autonomy={'done': Autonomy.Off})
# x:667 y:123
OperatableStateMachine.add('Log_test',
LogState(text=test, severity=Logger.REPORT_HINT),
transitions={'done': 'Initial_Wait_2'},
autonomy={'done': Autonomy.Off})
# x:446 y:78
OperatableStateMachine.add('state1',
TestPublisherState(),
transitions={'done': 'Show_Current_State_Page'},
autonomy={'done': Autonomy.Off})
# x:601 y:339
OperatableStateMachine.add('Show_Modal',
tiago_flexbe_states__WebsiteDummyModalState(),
transitions={'done': 'Log_test', 'failed': 'Show_Modal'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off})
# x:388 y:280
OperatableStateMachine.add('Show_Current_State_Page',
WebsiteDummyState1(),
transitions={'done': 'Show_Modal', 'failed': 'Show_Current_State_Page'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off})
return _state_machine
def create(self):
# x:283 y:267
_state_machine = OperatableStateMachine(outcomes=['Shutdown'])
_state_machine.userdata.Command = "no nothing"
_state_machine.userdata.End = False
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:887 y:420
_sm_sara_brain_0 = OperatableStateMachine(
outcomes=['error'],
input_keys=['HighFIFO', 'LowFIFO', 'MedFIFO', 'DoNow', 'End'])
with _sm_sara_brain_0:
# x:270 y:346
OperatableStateMachine.add(
'sara_command_manager',
self.use_behavior(
sara_command_managerSM,
'Sara parallel Runtime/Sara brain/sara_command_manager'),
transitions={
'finished': 'if stop',
'failed': 'set end'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'HighFIFO': 'HighFIFO',
'MedFIFO': 'MedFIFO',
'LowFIFO': 'LowFIFO',
'DoNow': 'DoNow'
})
# x:725 y:388
OperatableStateMachine.add(
'set end',
CalculationState(calculation=lambda x: True),
transitions={'done': 'error'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'End',
'output_value': 'End'
})
# x:516 y:448
OperatableStateMachine.add(
'if stop',
CheckConditionState(predicate=lambda x: x[0][0] == "Stop"),
transitions={
'true': 'set end',
'false': 'sara_command_manager'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'DoNow'})
# x:841 y:231
_sm_sara_action_executor_1 = OperatableStateMachine(
outcomes=['shutdown'],
input_keys=['HighFIFO', 'MedFIFO', 'LowFIFO', 'DoNow', 'End'])
with _sm_sara_action_executor_1:
# x:128 y:134
OperatableStateMachine.add(
'log',
LogState(text="Start action executor",
severity=Logger.REPORT_HINT),
transitions={'done': 'Sara action executor'},
autonomy={'done': Autonomy.Off})
# x:636 y:111
OperatableStateMachine.add(
'Critical failure',
LogState(text="Critical fail in action executer!",
severity=Logger.REPORT_HINT),
transitions={'done': 'shutdown'},
autonomy={'done': Autonomy.Off})
# x:366 y:149
OperatableStateMachine.add(
'Sara action executor',
self.use_behavior(
SaraactionexecutorSM,
'Sara parallel Runtime/Sara action executor/Sara action executor'
),
transitions={
'CriticalFail': 'Critical failure',
'Shutdown': 'shutdown'
},
autonomy={
'CriticalFail': Autonomy.Inherit,
'Shutdown': Autonomy.Inherit
},
remapping={
'HighFIFO': 'HighFIFO',
'MedFIFO': 'MedFIFO',
'LowFIFO': 'LowFIFO',
'End': 'End'
})
# x:30 y:365
_sm_sara_init_2 = OperatableStateMachine(outcomes=['done'])
with _sm_sara_init_2:
# x:30 y:40
OperatableStateMachine.add('set head',
SaraSetHeadAngle(angle=0.1),
transitions={'done': 'set face'},
autonomy={'done': Autonomy.Off})
# x:47 y:119
OperatableStateMachine.add('set arm',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'hello',
'failed': 'hello'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'target'})
# x:242 y:288
OperatableStateMachine.add(
'hello',
SaraSay(
sentence=
"Good morning. I am Sara the robot. Please give me orders.",
emotion=1,
block=True),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:239 y:21
OperatableStateMachine.add('set face',
SetExpression(emotion=0,
brightness=200),
transitions={'done': 'www'},
autonomy={'done': Autonomy.Off})
# x:339 y:116
OperatableStateMachine.add('on face',
SetExpression(emotion=1, brightness=-1),
transitions={'done': 'setTarget'},
autonomy={'done': Autonomy.Off})
# x:484 y:41
OperatableStateMachine.add('www',
WaitState(wait_time=2),
transitions={'done': 'on face'},
autonomy={'done': Autonomy.Off})
# x:197 y:113
OperatableStateMachine.add('setTarget',
SetKey(Value="IdlePose"),
transitions={'done': 'set arm'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'target'})
# x:55 y:366
_sm_create_fifos_3 = OperatableStateMachine(
outcomes=['done'],
output_keys=['HighFIFO', 'MedFIFO', 'LowFIFO', 'DoNow'])
with _sm_create_fifos_3:
# x:33 y:40
OperatableStateMachine.add('Create HighFIFO',
FIFO_New(),
transitions={'done': 'Create MedFIFO'},
autonomy={'done': Autonomy.Off},
remapping={'FIFO': 'HighFIFO'})
# x:30 y:114
OperatableStateMachine.add('Create MedFIFO',
FIFO_New(),
transitions={'done': 'Create LowFIFO'},
autonomy={'done': Autonomy.Off},
remapping={'FIFO': 'MedFIFO'})
# x:30 y:189
OperatableStateMachine.add('Create LowFIFO',
FIFO_New(),
transitions={'done': 'Create DoNow'},
autonomy={'done': Autonomy.Off},
remapping={'FIFO': 'LowFIFO'})
# x:34 y:260
OperatableStateMachine.add('Create DoNow',
FIFO_New(),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={'FIFO': 'DoNow'})
# x:30 y:322
_sm_sara_shutdown_4 = OperatableStateMachine(outcomes=['finished'])
with _sm_sara_shutdown_4:
# x:57 y:86
OperatableStateMachine.add('log',
LogState(text="shutdown",
severity=Logger.REPORT_HINT),
transitions={'done': 'say'},
autonomy={'done': Autonomy.Off})
# x:122 y:228
OperatableStateMachine.add(
'say',
SaraSay(sentence="I'm goint to shutdown for safety reasons",
emotion=1,
block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:336 y:314, x:315 y:433, x:276 y:271
_sm_sara_parallel_runtime_5 = ConcurrencyContainer(
outcomes=['Shutdown'],
input_keys=['HighFIFO', 'MedFIFO', 'LowFIFO', 'DoNow', 'End'],
conditions=[('Shutdown', [('Sara brain', 'error')]),
('Shutdown', [('Sara action executor', 'shutdown')])])
with _sm_sara_parallel_runtime_5:
# x:52 y:416
OperatableStateMachine.add('Sara action executor',
_sm_sara_action_executor_1,
transitions={'shutdown': 'Shutdown'},
autonomy={'shutdown': Autonomy.Inherit},
remapping={
'HighFIFO': 'HighFIFO',
'MedFIFO': 'MedFIFO',
'LowFIFO': 'LowFIFO',
'DoNow': 'DoNow',
'End': 'End'
})
# x:57 y:333
OperatableStateMachine.add('Sara brain',
_sm_sara_brain_0,
transitions={'error': 'Shutdown'},
autonomy={'error': Autonomy.Inherit},
remapping={
'HighFIFO': 'HighFIFO',
'LowFIFO': 'LowFIFO',
'MedFIFO': 'MedFIFO',
'DoNow': 'DoNow',
'End': 'End'
})
with _state_machine:
# x:43 y:60
OperatableStateMachine.add('log',
LogState(text="Start Sara",
severity=Logger.REPORT_HINT),
transitions={'done': 'low head'},
autonomy={'done': Autonomy.Off})
# x:306 y:365
OperatableStateMachine.add(
'Sara parallel Runtime',
_sm_sara_parallel_runtime_5,
transitions={'Shutdown': 'Sara shutdown'},
autonomy={'Shutdown': Autonomy.Inherit},
remapping={
'HighFIFO': 'HighFIFO',
'MedFIFO': 'MedFIFO',
'LowFIFO': 'LowFIFO',
'DoNow': 'DoNow',
'End': 'End'
})
# x:315 y:462
OperatableStateMachine.add('Sara shutdown',
_sm_sara_shutdown_4,
transitions={'finished': 'low head'},
autonomy={'finished': Autonomy.Inherit})
# x:328 y:284
OperatableStateMachine.add(
'Create_FIFOs',
_sm_create_fifos_3,
transitions={'done': 'Sara parallel Runtime'},
autonomy={'done': Autonomy.Inherit},
remapping={
'HighFIFO': 'HighFIFO',
'MedFIFO': 'MedFIFO',
'LowFIFO': 'LowFIFO',
'DoNow': 'DoNow'
})
# x:150 y:207
OperatableStateMachine.add('listen',
GetSpeech(watchdog=10),
transitions={
'done': 'check hello',
'nothing': 'listen',
'fail': 'Shutdown'
},
autonomy={
'done': Autonomy.Off,
'nothing': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'words': 'words'})
# x:345 y:195
OperatableStateMachine.add('Sara init',
_sm_sara_init_2,
transitions={'done': 'Create_FIFOs'},
autonomy={'done': Autonomy.Inherit})
# x:355 y:96
OperatableStateMachine.add(
'check hello',
RegexTester(
regex=
".*((wake up)|(sarah?)|(shut up)|(hello)(robot)|(hi)|(morning)|(greet)).*"
),
transitions={
'true': 'Sara init',
'false': 'listen'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'text': 'words',
'result': 'result'
})
# x:15 y:220
OperatableStateMachine.add('close face',
SetExpression(emotion=0, brightness=20),
transitions={'done': 'listen'},
autonomy={'done': Autonomy.Off})
# x:102 y:115
OperatableStateMachine.add('low head',
SaraSetHeadAngle(angle=0.8),
transitions={'done': 'close face'},
autonomy={'done': Autonomy.Off})
return _state_machine
