def create(self):
# x:30 y:365, x:130 y:365
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.waypoint_left = {'coordinate':{'x':'none', 'y':'none', 'theta':'none'}, 'increment':{'x':2.0, 'y':-2.0, 'theta':3.1415/2}}
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('w1',
WaitState(wait_time=1),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
# x:651 y:124
OperatableStateMachine.add('s1',
SubscriberState(topic='/pose', blocking=True, clear=False),
transitions={'received': 'm1', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'curr_pose'})
# x:181 y:174
OperatableStateMachine.add('m1',
MoveBaseState(),
transitions={'arrived': 'finished', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'waypoint_left', 'curr_pose': 'curr_pose'})
return _state_machine
def create(self):
# x:30 y:463, x:130 y:463
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.Distance = 4.5
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:164 y:98
OperatableStateMachine.add('Get_Current_Pose',
SubscriberState(topic='/pose',
blocking=True,
clear=False),
transitions={
'received': 'finished',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'curr_pose'})
return _state_machine
def create(self):
# x:683 y:190, x:133 y:290
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.Direction = 'Right'
_state_machine.userdata.Turn_Metric = {'x': 1.5, 'y': 1.5}
_state_machine.userdata.Straight_Metric = 2.0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:107 y:117
OperatableStateMachine.add('w1',
WaitState(wait_time=1),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
# x:351 y:109
OperatableStateMachine.add('s1',
SubscriberState(topic='/pose',
blocking=True,
clear=False),
transitions={
'received': 'm1',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'curr_pose'})
# x:337 y:412
OperatableStateMachine.add('w2',
WaitState(wait_time=15),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:331 y:274
OperatableStateMachine.add('m1',
MoveBaseState(),
transitions={
'arrived': 'w2',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'Direction': 'Direction',
'curr_pose': 'curr_pose',
'Turn_Metric': 'Turn_Metric',
'Straight_Metric': 'Straight_Metric'
})
return _state_machine
def create(self):
# x:950 y:235, x:439 y:405
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.Direction = 'Parking'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:147 y:71
OperatableStateMachine.add('Get_Pose',
SubscriberState(topic='/pose', blocking=True, clear=False),
transitions={'received': 'Park', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'curr_pose'})
# x:441 y:157
OperatableStateMachine.add('Park',
MoveBaseState(),
transitions={'arrived': 'finished', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'Direction': 'Direction', 'curr_pose': 'curr_pose'})
return _state_machine
def create(self):
# x:33 y:340, x:133 y:340, x:233 y:340, x:144 y:140
_state_machine = OperatableStateMachine(
outcomes=['L_B', 'M_B', 'H_B', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:107 y:24
OperatableStateMachine.add('w1',
WaitState(wait_time=4),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
# x:351 y:24
OperatableStateMachine.add('s1',
SubscriberState(topic='/FourWD/battery',
blocking=True,
clear=False),
transitions={
'received': 'w2',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'battery_level'})
# x:357 y:224
OperatableStateMachine.add('w2',
WaitState(wait_time=1),
transitions={'done': 'd1'},
autonomy={'done': Autonomy.Off})
# x:105 y:224
OperatableStateMachine.add(
'd1',
DecisionState(outcomes=['Low', 'Medium', 'High'],
conditions=lambda x: 'Low'
if x.data < 98 else 'Medium'),
transitions={
'Low': 'L_B',
'Medium': 'M_B',
'High': 'H_B'
},
autonomy={
'Low': Autonomy.Off,
'Medium': Autonomy.Off,
'High': Autonomy.Off
},
remapping={'input_value': 'battery_level'})
return _state_machine
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):
# x:83 y:390, x:33 y:190
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.Direction = 'Stop'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:201 y:59
OperatableStateMachine.add('s1',
SubscriberState(topic='/pose',
blocking=True,
clear=False),
transitions={
'received': 'm1',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'curr_pose'})
# x:207 y:374
OperatableStateMachine.add('w1',
WaitState(wait_time=4),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:181 y:224
OperatableStateMachine.add('m1',
MoveBaseState(),
transitions={
'arrived': 'w1',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'Direction': 'Direction',
'curr_pose': 'curr_pose'
})
return _state_machine
def create(self):
# x:1569 y:533, x:771 y:388
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.input_value = 1
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:106 y:98
OperatableStateMachine.add('detect_object',
SubscriberState(
topic='/darknet_ros/bounding_boxes',
blocking=True,
clear=False),
transitions={
'received': 'Determine_Course',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'detected'})
# x:101 y:301
OperatableStateMachine.add('Determine_Course',
Carbonara(),
transitions={
'none': 'failed',
'Obstacle': 'failed',
'Left': 'failed',
'Right': 'failed'
},
autonomy={
'none': Autonomy.Off,
'Obstacle': Autonomy.Off,
'Left': Autonomy.Off,
'Right': Autonomy.Off
},
remapping={
'input_value': 'detected',
'Distance': 'Distance'
})
return _state_machine
def create(self):
# x:83 y:490, x:83 y:290
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.waypoint_charging = {'coordinate':{'x':0.0, 'y':0.0, 'theta':0.0}, 'increment':{'x':'none', 'y':'none', 'theta':'none'}}
_state_machine.userdata.curr_pose = 'none'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:157 y:74
OperatableStateMachine.add('w1',
WaitState(wait_time=1),
transitions={'done': 'm1'},
autonomy={'done': Autonomy.Off})
# x:481 y:74
OperatableStateMachine.add('m1',
MoveBaseState(),
transitions={'arrived': 's1', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'waypoint_charging', 'curr_pose': 'curr_pose'})
# x:501 y:174
OperatableStateMachine.add('s1',
SubscriberState(topic='/FourWD/battery', blocking=True, clear=False),
transitions={'received': 'd1', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'battery_level'})
# x:505 y:424
OperatableStateMachine.add('d1',
DecisionState(outcomes=['full', 'not_full'], conditions=lambda x: 'full' if x.data>99.9 else 'not_full'),
transitions={'full': 'finished', 'not_full': 'w2'},
autonomy={'full': Autonomy.Off, 'not_full': Autonomy.Off},
remapping={'input_value': 'battery_level'})
# x:357 y:324
OperatableStateMachine.add('w2',
WaitState(wait_time=1),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:30 y:365, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.Direction = 'Battery_in'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:72 y:44
OperatableStateMachine.add('get_pose',
SubscriberState(topic='/pose',
blocking=True,
clear=False),
transitions={
'received':
'enter_charging_station',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'curr_pose'})
# x:226 y:130
OperatableStateMachine.add('enter_charging_station',
MoveBaseState(),
transitions={
'arrived': 'finished',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'Direction': 'Direction',
'curr_pose': 'curr_pose'
})
return _state_machine
def create(self):
# x:832 y:94, x:692 y:405
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.joint_values_left = [4.824961332294557, 4.986228519472087, 2.016893253088309, 9.056585287421564, 1.7655888505129436, 2.5387597755555658]
_state_machine.userdata.joint_values_right = [4.761117371482087, 4.435244724700612, 1.6085220010066865, 3.2511244744831167, 1.706054283818386, 2.4325377312649925]
_state_machine.userdata.joint_names = ["m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3", "m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6"]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:106 y:117
OperatableStateMachine.add('sub',
SubscriberState(topic='/robotender', blocking=True, clear=False),
transitions={'received': 'decide', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'message'})
# x:280 y:116
OperatableStateMachine.add('decide',
DecisionState(outcomes=['go_left','go_right'], conditions=lambda x: "go_right" if x.data=="R" else "go_left"),
transitions={'go_left': 'left', 'go_right': 'right'},
autonomy={'go_left': Autonomy.Off, 'go_right': Autonomy.Off},
remapping={'input_value': 'message'})
# x:460 y:22
OperatableStateMachine.add('left',
FeedbackJointStateToMoveit(move_group="arm", action_topic="/move_group", robot_name="m1n6s200", position_topic='/m1n6s200_driver/joint_states', delta=1E-4),
transitions={'reached': 'sub', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'joint_values': 'joint_values_left', 'joint_names': 'joint_names'})
# x:452 y:234
OperatableStateMachine.add('right',
FeedbackJointStateToMoveit(move_group="arm", action_topic="/move_group", robot_name="m1n6s200", position_topic='/m1n6s200_driver/joint_states', delta=1E-4),
transitions={'reached': 'sub', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'joint_values': 'joint_values_right', 'joint_names': 'joint_names'})
return _state_machine
def create(self):
# x:733 y:490, x:733 y:140
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:107 y:124
OperatableStateMachine.add('w1',
WaitState(wait_time=3),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
# x:501 y:224
OperatableStateMachine.add('s1',
SubscriberState(
topic='/darknet_ros/bounding_boxes',
blocking=True,
clear=True),
transitions={
'received': 'c1',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'detection'})
# x:343 y:474
OperatableStateMachine.add('c1',
Carbonara(),
transitions={'done': 'w1'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'detection'})
return _state_machine
def create(self):
# x:1085 y:232, x:429 y:580
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.input_value = 1
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:117 y:87
OperatableStateMachine.add('Get_pose',
SubscriberState(topic='/pose', blocking=True, clear=False),
transitions={'received': 'Calculate_Need', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'curr_pose'})
# x:373 y:214
OperatableStateMachine.add('Calculate_Need',
Amatricianna(),
transitions={'Navigation': 'Navigation', 'Parking': 'Parking'},
autonomy={'Navigation': Autonomy.Off, 'Parking': Autonomy.Off},
remapping={'curr_pose': 'curr_pose', 'Direction': 'Direction'})
# x:659 y:61
OperatableStateMachine.add('Navigation',
self.use_behavior(NavigationSM, 'Navigation'),
transitions={'finished': 'Calculate_Need', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:652 y:250
OperatableStateMachine.add('Parking',
self.use_behavior(ParkingSM, 'Parking'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
return _state_machine
def create(self):
# x:822 y:88, x:146 y:424
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:152 y:124
OperatableStateMachine.add('s1',
SubscriberState(topic='/darknet_ros/bounding_boxes', blocking=True, clear=False),
transitions={'received': 'Obs_check', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'input_value'})
# x:846 y:355
OperatableStateMachine.add('Stop',
self.use_behavior(StopSM, 'Stop'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:427 y:102
OperatableStateMachine.add('Obs_check',
Avoidance_Check(),
transitions={'Stop': 'Stop', 'Back': 'go_back', 'Done': 'finished'},
autonomy={'Stop': Autonomy.Off, 'Back': Autonomy.Off, 'Done': Autonomy.Off},
remapping={'input_value': 'input_value', 'Distance': 'Distance'})
# x:447 y:276
OperatableStateMachine.add('go_back',
self.use_behavior(go_backSM, 'go_back'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
return _state_machine
def create(self):
# x:67 y:337, x:285 y:147
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], output_keys=['words'])
_state_machine.userdata.words = ""
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:38 y:133
OperatableStateMachine.add('subber',
SubscriberState(topic="/sara_command",
blocking=True,
clear=True),
transitions={
'received': 'get speech',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:40 y:234
OperatableStateMachine.add(
'get speech',
CalculationState(calculation=lambda x: x.data),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'message',
'output_value': 'words'
})
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: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': 'initialPub'},
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
})
# x:75 y:417
OperatableStateMachine.add('initialPub',
KylePubState(
cmd_topic='/makethisupcount',
valueToPub=0),
transitions={'done': 'getVelocity'},
autonomy={'done': Autonomy.Off})
# x:215 y:471
OperatableStateMachine.add('getVelocity',
SubscriberState(topic='/makethisupvel',
blocking=True,
clear=False),
transitions={
'received': 'getAng',
'unavailable': 'getCount'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'myvelocity'})
# x:402 y:461
OperatableStateMachine.add('getAng',
SubscriberState(topic='/makethisupang',
blocking=True,
clear=False),
transitions={
'received': 'Ball_in_image',
'unavailable': 'getCount'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'angularmy'})
# x:559 y:450
OperatableStateMachine.add(
'Ball_in_image',
KyleVerifyState(ValueToMeasureAgainst=7777.0),
transitions={
'verified': 'getCount',
'notVerified': 'move'
},
autonomy={
'verified': Autonomy.Off,
'notVerified': Autonomy.Off
},
remapping={
'inputValueVel': 'myvelocity',
'inputValueAng': 'angularmy'
})
# x:706 y:496
OperatableStateMachine.add(
'move',
KyleTwistState(
cmd_topic='/turtlebot/stamped_cmd_vel_mux/input/navi'),
transitions={
'done': 'ShouldRobotStop',
'getNewMove': 'getVelocity'
},
autonomy={
'done': Autonomy.Off,
'getNewMove': Autonomy.Off
},
remapping={
'input_velocity': 'myvelocity',
'input_rotation_rate': 'angularmy'
})
# x:960 y:523
OperatableStateMachine.add('ShouldRobotStop',
OperatorDecisionState(
outcomes=['yes', 'no'],
hint="Should the Robot Stop?",
suggestion=None),
transitions={
'yes': 'finished',
'no': 'Continue'
},
autonomy={
'yes': Autonomy.Off,
'no': Autonomy.Off
})
# x:658 y:615
OperatableStateMachine.add('getCount',
SubscriberState(
topic='/makethisupcount',
blocking=True,
clear=False),
transitions={
'received': 'verifyCount',
'unavailable': 'ShouldRobotStop'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'mycount'})
# x:443 y:679
OperatableStateMachine.add(
'verifyCount',
KyleVerifyState(ValueToMeasureAgainst=45),
transitions={
'verified': 'Continue',
'notVerified': 'increaseCount'
},
autonomy={
'verified': Autonomy.Off,
'notVerified': Autonomy.Off
},
remapping={
'inputValueVel': 'mycount',
'inputValueAng': 'mycount'
})
# x:242 y:708
OperatableStateMachine.add(
'increaseCount',
KylePubInputState(cmd_topic='/makethisupcount', increaseBy=1),
transitions={'done': 'rotate'},
autonomy={'done': Autonomy.Off},
remapping={'valueToIncrease': 'mycount'})
# x:75 y:709
OperatableStateMachine.add(
'rotate',
TimedTwistState(
target_time=.1,
velocity=0,
rotation_rate=.5,
cmd_topic='/turtlebot/stamped_cmd_vel_mux/input/navi'),
transitions={'done': 'getVelocity'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
joy_topic = '/hmi/joystick'
# x:33 y:423, x:683 y:568
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['be_evil'])
_state_machine.userdata.cycle_counter = 0
_state_machine.userdata.be_evil = self.be_evil
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:356, x:644 y:383
_sm_detectjoyevent_0 = OperatableStateMachine(
outcomes=['received', 'unavailable'])
with _sm_detectjoyevent_0:
# x:229 y:154
OperatableStateMachine.add('JoyEvent',
SubscriberState(topic=joy_topic,
blocking=True,
clear=False),
transitions={
'received': 'CheckEvent',
'unavailable': 'unavailable'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:229 y:296
OperatableStateMachine.add(
'CheckEvent',
DecisionState(
outcomes=['yes', 'no'],
conditions=lambda msg: 'yes'
if any(msg.buttons) or any(msg.axes[0:5]) else 'no'),
transitions={
'yes': 'received',
'no': 'JoyEvent'
},
autonomy={
'yes': Autonomy.Off,
'no': Autonomy.Off
},
remapping={'input_value': 'message'})
# x:778 y:316, x:75 y:354, x:294 y:358, x:776 y:358, x:777 y:406, x:530 y:356
_sm_checkjoy_1 = ConcurrencyContainer(
outcomes=['no_activity', 'activity_detected', 'failed'],
conditions=[('no_activity', [('WaitForJoy', 'done')]),
('failed', [('DetectJoyEvent', 'unavailable')]),
('activity_detected', [('DetectJoyEvent', 'received')])
])
with _sm_checkjoy_1:
# x:219 y:132
OperatableStateMachine.add('DetectJoyEvent',
_sm_detectjoyevent_0,
transitions={
'received': 'activity_detected',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Inherit,
'unavailable': Autonomy.Inherit
})
# x:624 y:152
OperatableStateMachine.add('WaitForJoy',
WaitState(wait_time=self.wait_time),
transitions={'done': 'no_activity'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:646 y:31
OperatableStateMachine.add(
'RandomChoose',
DecisionState(outcomes=['greeting', 'play', 'cheer', 'bad'],
conditions=self.select_behavior),
transitions={
'greeting': 'Greeting',
'play': 'Play',
'cheer': 'Cheer',
'bad': 'Bad'
},
autonomy={
'greeting': Autonomy.Low,
'play': Autonomy.Low,
'cheer': Autonomy.Low,
'bad': Autonomy.Low
},
remapping={'input_value': 'cycle_counter'})
# x:383 y:460
OperatableStateMachine.add('CheckJoy',
_sm_checkjoy_1,
transitions={
'no_activity': 'ResetCounter',
'activity_detected':
'AddCycleCounter',
'failed': 'failed'
},
autonomy={
'no_activity': Autonomy.Inherit,
'activity_detected':
Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:221 y:134
OperatableStateMachine.add(
'AddCycleCounter',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'RandomChoose'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'cycle_counter',
'output_value': 'cycle_counter'
})
# x:483 y:171
OperatableStateMachine.add('Greeting',
self.use_behavior(
GreetingSM, 'Greeting'),
transitions={
'finished': 'CheckJoy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:849 y:197
OperatableStateMachine.add('Cheer',
self.use_behavior(CheerSM, 'Cheer'),
transitions={
'finished': 'CheckJoy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:687 y:197
OperatableStateMachine.add('Play',
self.use_behavior(PlaySM, 'Play'),
transitions={
'finished': 'CheckJoy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:991 y:203
OperatableStateMachine.add('Bad',
self.use_behavior(BadSM, 'Bad'),
transitions={
'finished': 'CheckJoy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:115 y:364
OperatableStateMachine.add(
'ResetCounter',
CalculationState(calculation=lambda x: 0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'cycle_counter',
'output_value': 'cycle_counter'
})
return _state_machine
def create(self):
# x: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: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:1683 y:440, x:1433 y:40, x:1433 y:790
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'failed2'])
_state_machine.userdata.pub_roadworks = 'Roadworks'
_state_machine.userdata.pub_slippery_road = 'Slippery_Road'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:57 y:24
OperatableStateMachine.add('w1',
WaitState(wait_time=1),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
# x:143 y:224
OperatableStateMachine.add('carb1',
Carbonara(),
transitions={'continue': 'or1', 'Obstacle': 'w4'},
autonomy={'continue': Autonomy.Off, 'Obstacle': Autonomy.Off},
remapping={'input_value': 'detected', 'output_value': 'closest_object'})
# x:143 y:424
OperatableStateMachine.add('or1',
ObjectRecognition(),
transitions={'Left_Sign': 'turn_left', 'Right_Sign': 'turn_right', 'Roadworks': 'p1', 'Slippery_Road': 'p2', 'Turn_Left_Sign': 'turn_left', 'Turn_Right_Sign': 'turn_right', 'Charging_Station': 'finished', 'Stop_Sign': 'Stop', 'Parking_Sign': 'finished', 'Parking_Spot': 'finished', 'none': 'go_straight'},
autonomy={'Left_Sign': Autonomy.Off, 'Right_Sign': Autonomy.Off, 'Roadworks': Autonomy.Off, 'Slippery_Road': Autonomy.Off, 'Turn_Left_Sign': Autonomy.Off, 'Turn_Right_Sign': Autonomy.Off, 'Charging_Station': Autonomy.Off, 'Stop_Sign': Autonomy.Off, 'Parking_Sign': Autonomy.Off, 'Parking_Spot': Autonomy.Off, 'none': Autonomy.Off},
remapping={'input_value': 'closest_object'})
# x:251 y:24
OperatableStateMachine.add('s1',
SubscriberState(topic='/darknet_ros/bounding_boxes', blocking=True, clear=False),
transitions={'received': 'carb1', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'detected'})
# x:619 y:721
OperatableStateMachine.add('turn_left',
self.use_behavior(turn_leftSM, 'turn_left'),
transitions={'finished': 'finished', 'failed': 'failed2'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:619 y:621
OperatableStateMachine.add('turn_right',
self.use_behavior(turn_rightSM, 'turn_right'),
transitions={'finished': 'finished', 'failed': 'failed2'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:619 y:821
OperatableStateMachine.add('go_straight',
self.use_behavior(go_straightSM, 'go_straight'),
transitions={'finished': 'finished', 'failed': 'failed2'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:591 y:374
OperatableStateMachine.add('p1',
PublisherStringState(topic='/chatter'),
transitions={'done': 'w2'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'pub_roadworks'})
# x:591 y:474
OperatableStateMachine.add('p2',
PublisherStringState(topic='/chatter'),
transitions={'done': 'w3'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'pub_slippery_road'})
# x:857 y:324
OperatableStateMachine.add('w2',
WaitState(wait_time=3),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:857 y:524
OperatableStateMachine.add('w3',
WaitState(wait_time=3),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:769 y:221
OperatableStateMachine.add('Stop',
self.use_behavior(StopSM, 'Stop'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:457 y:74
OperatableStateMachine.add('w4',
WaitState(wait_time=1),
transitions={'done': 'd1'},
autonomy={'done': Autonomy.Off})
# x:805 y:124
OperatableStateMachine.add('d1',
DecisionState(outcomes=['go', 'stop', 'back'], conditions=lambda input_value: 'go' if input_value[1]>3 else ('back' if input_value[1]<1.5 else 'stop')),
transitions={'go': 'go_straight_2', 'stop': 'Stop_2', 'back': 'go_back'},
autonomy={'go': Autonomy.Off, 'stop': Autonomy.Off, 'back': Autonomy.Off},
remapping={'input_value': 'closest_object'})
# x:1519 y:171
OperatableStateMachine.add('go_back',
self.use_behavior(go_backSM, 'go_back'),
transitions={'finished': 'carb1', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:1169 y:221
OperatableStateMachine.add('go_straight_2',
self.use_behavior(go_straightSM, 'go_straight_2'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:469 y:156
OperatableStateMachine.add('Stop_2',
self.use_behavior(StopSM, 'Stop_2'),
transitions={'finished': 'carb1', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
return _state_machine
def create(self):
names1 = [
'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
'robot1_elbow_joint', 'robot1_wrist_1_joint',
'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
]
pick1_group = 'robot1'
robot1_loc = Pose2D(x=3.8, y=2.1, theta=-90.0)
gripper1 = "vacuum_gripper1_suction_cup"
pick2_group = 'robot2'
robot1_loc_o = Pose2D(x=-4.3, y=-0.9, theta=0.0)
names2 = [
'robot2_shoulder_pan_joint', 'robot2_shoulder_lift_joint',
'robot2_elbow_joint', 'robot2_wrist_1_joint',
'robot2_wrist_2_joint', 'robot2_wrist_3_joint'
]
gripper2 = "vacuum_gripper2_suction_cup"
# x:208 y:207, x:594 y:345
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.part_pose = []
_state_machine.userdata.robot1_loc = robot1_loc
_state_machine.userdata.pose_turtlebot = []
_state_machine.userdata.pick1_configuration = []
_state_machine.userdata.place1_configuration = []
_state_machine.userdata.speed = 100
_state_machine.userdata.robot1_loc_o = robot1_loc_o
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:32 y:56
OperatableStateMachine.add(
'Move R1 Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Start Conveyor',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1122 y:138
OperatableStateMachine.add('Compute pick',
ComputeGraspState(group=pick1_group,
offset=0.0,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Activate Gripper 1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick1_configuration',
'joint_names': 'joint_names'
})
# x:381 y:59
OperatableStateMachine.add('Start feeder',
ControlFeederState(activation=True),
transitions={
'succeeded': 'Wait for part',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:718 y:59
OperatableStateMachine.add('Stop feeder',
ControlFeederState(activation=False),
transitions={
'succeeded': 'Stop Conveyor',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1104 y:222
OperatableStateMachine.add('Activate Gripper 1',
VacuumGripperControlState(
enable=True,
service_name='/gripper1/control'),
transitions={
'continue': 'Move R1 to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:971 y:519
OperatableStateMachine.add('Compute place Turtlebot',
ComputeGraspState(group=pick1_group,
offset=0.6,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Move R1 to place',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'pose_turtlebot',
'joint_values':
'place1_configuration',
'joint_names': 'joint_names'
})
# x:1127 y:493
OperatableStateMachine.add(
'LocateTurtlebot',
LocateFactoryDeviceState(model_name='mobile_base',
output_frame_id='world'),
transitions={
'succeeded': 'Compute place Turtlebot',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose_turtlebot'})
# x:1125 y:370
OperatableStateMachine.add(
'Move R1 back Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Navigate To Robot1',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:5 y:296
OperatableStateMachine.add(
'Move R2 back to Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Move R2 To Bin',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1113 y:59
OperatableStateMachine.add(
'Detect Part Camera',
DetectPartCameraState(ref_frame='robot1_base',
camera_topic='/hrwros/logical_camera_1',
camera_frame='logical_camera_1_frame'),
transitions={
'continue': 'Compute pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:556 y:60
OperatableStateMachine.add('Wait for part',
SubscriberState(
topic='/break_beam_sensor_change',
blocking=True,
clear=True),
transitions={
'received': 'Stop feeder',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:1121 y:304
OperatableStateMachine.add(
'Move R1 to pick',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Move R1 back Home',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick1_configuration',
'joint_names': 'joint_names'
})
# x:827 y:519
OperatableStateMachine.add(
'Move R1 to place',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Deactivate Gripper',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'place1_configuration',
'joint_names': 'joint_names'
})
# x:215 y:107
OperatableStateMachine.add('Start Conveyor',
SetConveyorPowerState(stop=False),
transitions={
'succeeded': 'Start feeder',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'speed'})
# x:886 y:60
OperatableStateMachine.add('Stop Conveyor',
SetConveyorPowerState(stop=True),
transitions={
'succeeded': 'Detect Part Camera',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'speed'})
# x:1136 y:432
OperatableStateMachine.add('Navigate To Robot1',
hrwros_factory_states__MoveBaseState(),
transitions={
'arrived': 'LocateTurtlebot',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'robot1_loc'})
# x:661 y:517
OperatableStateMachine.add('Deactivate Gripper',
VacuumGripperControlState(
enable=False,
service_name='/gripper1/control'),
transitions={
'continue': 'Move R1 Home_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:501 y:519
OperatableStateMachine.add(
'Move R1 Home_2',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Navigate To Robot1_2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:353 y:522
OperatableStateMachine.add('Navigate To Robot1_2',
hrwros_factory_states__MoveBaseState(),
transitions={
'arrived': 'Move R2 Home',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'robot1_loc_o'})
# x:25 y:530
OperatableStateMachine.add(
'Detect object near robot2',
DetectPartCameraState(ref_frame='robot2_base',
camera_topic='/hrwros/logical_camera_2',
camera_frame='logical_camera_2_frame'),
transitions={
'continue': 'Compute pick_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:42 y:470
OperatableStateMachine.add('Compute pick_2',
ComputeGraspState(group=pick2_group,
offset=0.0,
joint_names=names2,
tool_link=gripper2,
rotation=3.1415),
transitions={
'continue':
'Activate Gripper 2 robot2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick1_configuration',
'joint_names': 'joint_names'
})
# x:20 y:410
OperatableStateMachine.add('Activate Gripper 2 robot2',
VacuumGripperControlState(
enable=True,
service_name='/gripper2/control'),
transitions={
'continue': 'Move R1 to pick_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:211 y:525
OperatableStateMachine.add(
'Move R2 Home',
hrwros_factory_states__SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'Detect object near robot2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:21 y:353
OperatableStateMachine.add(
'Move R1 to pick_2',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick2_group,
offset=0.0,
tool_link=gripper2,
action_topic='/move_group'),
transitions={
'reached': 'Move R2 back to Home',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick1_configuration',
'joint_names': 'joint_names'
})
# x:10 y:220
OperatableStateMachine.add(
'Move R2 To Bin',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R2Place',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Deactivate R2 Gripper',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:9 y:133
OperatableStateMachine.add('Deactivate R2 Gripper',
VacuumGripperControlState(
enable=False,
service_name='/gripper2/control'),
transitions={
'continue': 'finished',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
return _state_machine
def create(self):
# x:1683 y:419, x:605 y:337
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:58 y:69
OperatableStateMachine.add('w1',
WaitState(wait_time=1),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
# x:1090 y:488
OperatableStateMachine.add('turn_right',
self.use_behavior(
turn_rightSM, 'turn_right'),
transitions={
'finished': 'w2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:55 y:196
OperatableStateMachine.add('s1',
SubscriberState(
topic='/darknet_ros/bounding_boxes',
blocking=True,
clear=False),
transitions={
'received': 'carb1',
'unavailable': 'w1'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'detected'})
# x:286 y:212
OperatableStateMachine.add('carb1',
Carbonara(),
transitions={
'none': 'go_straight',
'Obstacle': 'Obstacle_Avoidance',
'Left': 'go_straight_2',
'Right': 'go_straight_3'
},
autonomy={
'none': Autonomy.Off,
'Obstacle': Autonomy.Off,
'Left': Autonomy.Off,
'Right': Autonomy.Off
},
remapping={
'input_value': 'detected',
'Distance': 'Distance'
})
# x:1180 y:246
OperatableStateMachine.add('w2',
WaitState(wait_time=1),
transitions={'done': 'w5'},
autonomy={'done': Autonomy.Off})
# x:1161 y:64
OperatableStateMachine.add('w5',
WaitState(wait_time=1),
transitions={'done': 'w1'},
autonomy={'done': Autonomy.Off})
# x:958 y:119
OperatableStateMachine.add('turn_left',
self.use_behavior(
turn_leftSM, 'turn_left'),
transitions={
'finished': 'w2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:906 y:276
OperatableStateMachine.add('go_straight',
self.use_behavior(
go_straightSM, 'go_straight'),
transitions={
'finished': 'w2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:679 y:118
OperatableStateMachine.add('go_straight_2',
self.use_behavior(
go_straightSM, 'go_straight_2'),
transitions={
'finished': 'turn_left',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:715 y:484
OperatableStateMachine.add('go_straight_3',
self.use_behavior(
go_straightSM, 'go_straight_3'),
transitions={
'finished': 'turn_right',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:381 y:495
OperatableStateMachine.add('Obstacle_Avoidance',
self.use_behavior(
Obstacle_AvoidanceSM,
'Obstacle_Avoidance'),
transitions={
'finished': 's1',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
return _state_machine
def create(self):
# x:1162 y:100, x:318 y:599
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:42 y:28
OperatableStateMachine.add('initialPub',
KylePubState(
cmd_topic='/makethisupcount',
valueToPub=0),
transitions={'done': 'GetVelocity'},
autonomy={'done': Autonomy.Off})
# x:721 y:137
OperatableStateMachine.add(
'move',
KyleTwistState(
cmd_topic='/turtlebot/stamped_cmd_vel_mux/input/navi'),
transitions={
'done': 'Should_Robot_Finish',
'getNewMove': 'GetVelocity'
},
autonomy={
'done': Autonomy.Off,
'getNewMove': Autonomy.Off
},
remapping={
'input_velocity': 'velocitymy',
'input_rotation_rate': 'angularmy'
})
# x:277 y:51
OperatableStateMachine.add('getang',
SubscriberState(topic="/makethisupang",
blocking=True,
clear=False),
transitions={
'received': 'Ball_NOT_in_image',
'unavailable': 'getCount'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'angularmy'})
# x:97 y:394
OperatableStateMachine.add(
'Rotate',
TimedTwistState(
target_time=.1,
velocity=0,
rotation_rate=.5,
cmd_topic='/turtlebot/stamped_cmd_vel_mux/input/navi'),
transitions={'done': 'GetVelocity'},
autonomy={'done': Autonomy.Off})
# x:496 y:22
OperatableStateMachine.add(
'Ball_NOT_in_image',
KyleVerifyState(ValueToMeasureAgainst=7777.0),
transitions={
'verified': 'getCount',
'notVerified': 'move'
},
autonomy={
'verified': Autonomy.Off,
'notVerified': Autonomy.Off
},
remapping={
'inputValueVel': 'velocitymy',
'inputValueAng': 'angularmy'
})
# x:983 y:151
OperatableStateMachine.add('Should_Robot_Finish',
OperatorDecisionState(
outcomes=["yes", "no"],
hint="Should the Robot Stop?",
suggestion=None),
transitions={
'yes': 'finished',
'no': 'failed'
},
autonomy={
'yes': Autonomy.Off,
'no': Autonomy.Off
})
# x:501 y:211
OperatableStateMachine.add('getCount',
SubscriberState(
topic='/makethisupcount',
blocking=True,
clear=False),
transitions={
'received': 'verifyCount',
'unavailable': 'Should_Robot_Finish'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'countMessage'})
# x:450 y:321
OperatableStateMachine.add(
'verifyCount',
KyleVerifyState(ValueToMeasureAgainst=45),
transitions={
'verified': 'failed',
'notVerified': 'increaseCount'
},
autonomy={
'verified': Autonomy.Off,
'notVerified': Autonomy.Off
},
remapping={
'inputValueVel': 'countMessage',
'inputValueAng': 'countMessage'
})
# x:143 y:120
OperatableStateMachine.add('GetVelocity',
SubscriberState(topic="/makethisupvel",
blocking=True,
clear=False),
transitions={
'received': 'getang',
'unavailable': 'getCount'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'velocitymy'})
# x:271 y:304
OperatableStateMachine.add(
'increaseCount',
KylePubInputState(cmd_topic='/makethisupcount', increaseBy=1),
transitions={'done': 'Rotate'},
autonomy={'done': Autonomy.Off},
remapping={'valueToIncrease': 'countMessage'})
return _state_machine
def create(self):
# x:683 y:490, x:740 y:211
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.incremental1 = {'coordinate':{'x':'none', 'y':'none', 'theta':'none'}, 'increment':{'x':1.0, 'y':0.0, 'theta':0.0}}
_state_machine.userdata.incremental2 = {'coordinate':{'x':'none', 'y':'none', 'theta':'none'}, 'increment':{'x':0.2929, 'y':0.2929, 'theta':-3.1415/4}}
_state_machine.userdata.incremental3 = {'coordinate':{'x':'none', 'y':'none', 'theta':'none'}, 'increment':{'x':0.0, 'y':1.0, 'theta':-3.1415/2}}
_state_machine.userdata.incremental4 = {'coordinate':{'x':'none', 'y':'none', 'theta':'none'}, 'increment':{'x':0.0, 'y':-1.0, 'theta':-3.1415/2}}
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:107 y:74
OperatableStateMachine.add('w1',
WaitState(wait_time=1),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
# x:301 y:74
OperatableStateMachine.add('s1',
SubscriberState(topic='/pose', blocking=True, clear=False),
transitions={'received': 'w2', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'curr_pose'})
# x:281 y:174
OperatableStateMachine.add('m1',
MoveBaseState(),
transitions={'arrived': 'w3', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'incremental1', 'curr_pose': 'curr_pose'})
# x:281 y:274
OperatableStateMachine.add('m2',
MoveBaseState(),
transitions={'arrived': 'w4', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'incremental2', 'curr_pose': 'curr_pose'})
# x:281 y:374
OperatableStateMachine.add('m3',
MoveBaseState(),
transitions={'arrived': 'w5', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'incremental3', 'curr_pose': 'curr_pose'})
# x:107 y:174
OperatableStateMachine.add('w2',
WaitState(wait_time=1),
transitions={'done': 'm1'},
autonomy={'done': Autonomy.Off})
# x:107 y:274
OperatableStateMachine.add('w3',
WaitState(wait_time=5),
transitions={'done': 'm2'},
autonomy={'done': Autonomy.Off})
# x:107 y:374
OperatableStateMachine.add('w4',
WaitState(wait_time=5),
transitions={'done': 'm3'},
autonomy={'done': Autonomy.Off})
# x:107 y:474
OperatableStateMachine.add('w5',
WaitState(wait_time=5),
transitions={'done': 'm4'},
autonomy={'done': Autonomy.Off})
# x:281 y:474
OperatableStateMachine.add('m4',
MoveBaseState(),
transitions={'arrived': 'finished', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'incremental4', 'curr_pose': 'curr_pose'})
return _state_machine
def create(self):
voice_topic = 'control'
torque_off_service = 'motion/controller/torque_off/set_operational'
joint_state_control_topic = 'motion/controller/joint_state/out_joints_src_reset'
joy_topic = '/hmi/joystick'
eyes_topic = 'control'
# x:68 y:535, x:786 y:525
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.be_evil = self.be_evil
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:554 y:394, x:130 y:397
_sm_waitforjoyevent_0 = OperatableStateMachine(
outcomes=['received', 'unavailable'])
with _sm_waitforjoyevent_0:
# x:288 y:158
OperatableStateMachine.add('JoyEvent',
SubscriberState(topic=joy_topic,
blocking=True,
clear=False),
transitions={
'received': 'CheckEvent',
'unavailable': 'unavailable'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:485 y:159
OperatableStateMachine.add(
'CheckEvent',
DecisionState(outcomes=['noactivity', 'activity'],
conditions=lambda msg: 'activity'
if any(msg.buttons) or any(msg.axes[0:5]) else
'noactivity'),
transitions={
'noactivity': 'JoyEvent',
'activity': 'received'
},
autonomy={
'noactivity': Autonomy.Off,
'activity': Autonomy.Off
},
remapping={'input_value': 'message'})
# x:538 y:386, x:347 y:384, x:134 y:383, x:831 y:385, x:906 y:386
_sm_idlebehavior_1 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('finished', [('RandomHeadMovements', 'done')]),
('failed', [('WaitForJoyEvent', 'unavailable')]),
('finished', [('WaitForJoyEvent', 'received')])])
with _sm_idlebehavior_1:
# x:474 y:110
OperatableStateMachine.add('RandomHeadMovements',
SweetieBotRandHeadMovements(
controller='joint_state_head',
duration=100,
interval=[1, 4],
max2356=[0.3, 0.3, 1, 1],
min2356=[-0.3, -0.3, -1, -1]),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:144 y:133
OperatableStateMachine.add('WaitForJoyEvent',
_sm_waitforjoyevent_0,
transitions={
'received': 'finished',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Inherit,
'unavailable': Autonomy.Inherit
})
with _state_machine:
# x:79 y:99
OperatableStateMachine.add(
'CheckEvil',
DecisionState(outcomes=['good', 'evil'],
conditions=lambda x: 'evil' if x else 'good'),
transitions={
'good': 'NormalEyes',
'evil': 'RedEyes'
},
autonomy={
'good': Autonomy.Off,
'evil': Autonomy.Off
},
remapping={'input_value': 'be_evil'})
# x:550 y:467
OperatableStateMachine.add('AskForAssistance',
TextCommandState(type='voice/play_wav',
command='assistance',
topic=voice_topic),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:440 y:126
OperatableStateMachine.add('WaitForMovementFinish',
WaitState(wait_time=0.0),
transitions={'done': 'DoTricks'},
autonomy={'done': Autonomy.Off})
# x:251 y:503
OperatableStateMachine.add(
'StartingStance',
PublisherState(topic=joint_state_control_topic,
msg_type=JointState,
value={
'name': [
'joint51', 'joint52', 'joint53',
'eyes_pitch', 'eyes_yaw'
],
'position': [0.2, 0.0, 0.2, 0.0, 0.0]
}),
transitions={
'done': 'CheckEvil2',
'failed': 'AskForAssistance'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
})
# x:731 y:153
OperatableStateMachine.add('DoTricks',
self.use_behavior(
DoTricksSM, 'DoTricks'),
transitions={
'finished': 'NormalLook',
'failed': 'AskForAssistance'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:27 y:219
OperatableStateMachine.add('RedEyes',
TextCommandState(type='eyes/emotion',
command='red_eyes',
topic=eyes_topic),
transitions={'done': 'IdleBehavior'},
autonomy={'done': Autonomy.Off})
# x:222 y:65
OperatableStateMachine.add('NormalEyes',
TextCommandState(type='eyes/emotion',
command='normal',
topic=eyes_topic),
transitions={'done': 'IdleBehavior'},
autonomy={'done': Autonomy.Off})
# x:481 y:305
OperatableStateMachine.add('IdleBehavior',
_sm_idlebehavior_1,
transitions={
'finished': 'StartingStance',
'failed': 'AskForAssistance'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:181 y:360
OperatableStateMachine.add(
'CheckEvil2',
DecisionState(outcomes=['evil', 'good'],
conditions=lambda x: 'evil' if x else 'good'),
transitions={
'evil': 'EvilLook',
'good': 'WaitForMovementFinish'
},
autonomy={
'evil': Autonomy.Off,
'good': Autonomy.Off
},
remapping={'input_value': 'be_evil'})
# x:188 y:294
OperatableStateMachine.add(
'EvilLook',
TextCommandState(type='eyes/emotion',
command='evil_look',
topic=eyes_topic),
transitions={'done': 'WaitForMovementFinish'},
autonomy={'done': Autonomy.Off})
# x:880 y:311
OperatableStateMachine.add('NormalLook',
TextCommandState(type='eyes/emotion',
command='normal_look',
topic=eyes_topic),
transitions={'done': 'IdleBehavior'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:144 y:532, x:605 y:337
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.waypoint_left = {
'coordinate': {
'x': 'none',
'y': 'none',
'theta': 'none'
},
'increment': {
'x': 2.0,
'y': 2.0,
'theta': 3.1415 / 2
}
}
_state_machine.userdata.waypoint_right = {
'coordinate': {
'x': 'none',
'y': 'none',
'theta': 'none'
},
'increment': {
'x': 2.0,
'y': -2.0,
'theta': -3.1415 / 2
}
}
_state_machine.userdata.waypoint_straight = {
'coordinate': {
'x': 'none',
'y': 'none',
'theta': 'none'
},
'increment': {
'x': 2.0,
'y': 0.0,
'theta': 0.0
}
}
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:58 y:69
OperatableStateMachine.add('w1',
WaitState(wait_time=1),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
# x:1090 y:488
OperatableStateMachine.add('turn_right',
self.use_behavior(
turn_rightSM, 'turn_right'),
transitions={
'finished': 'w2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:55 y:196
OperatableStateMachine.add('s1',
SubscriberState(
topic='/darknet_ros/bounding_boxes',
blocking=True,
clear=False),
transitions={
'received': 'carb1',
'unavailable': 'w1'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'detected'})
# x:173 y:276
OperatableStateMachine.add('carb1',
Carbonara(),
transitions={
'none': 'go_straight',
'Obstacle': 'finished',
'Left': 'go_straight_2',
'Right': 'go_straight_3'
},
autonomy={
'none': Autonomy.Off,
'Obstacle': Autonomy.Off,
'Left': Autonomy.Off,
'Right': Autonomy.Off
},
remapping={
'input_value': 'detected',
'output_value': 'closest_object'
})
# x:1180 y:246
OperatableStateMachine.add('w2',
WaitState(wait_time=1),
transitions={'done': 'w5'},
autonomy={'done': Autonomy.Off})
# x:1161 y:64
OperatableStateMachine.add('w5',
WaitState(wait_time=1),
transitions={'done': 'w1'},
autonomy={'done': Autonomy.Off})
# x:958 y:119
OperatableStateMachine.add('turn_left',
self.use_behavior(
turn_leftSM, 'turn_left'),
transitions={
'finished': 'w2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:906 y:276
OperatableStateMachine.add('go_straight',
self.use_behavior(
go_straightSM, 'go_straight'),
transitions={
'finished': 'w2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:679 y:118
OperatableStateMachine.add('go_straight_2',
self.use_behavior(
go_straightSM, 'go_straight_2'),
transitions={
'finished': 'turn_left',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:715 y:484
OperatableStateMachine.add('go_straight_3',
self.use_behavior(
go_straightSM, 'go_straight_3'),
transitions={
'finished': 'turn_right',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
return _state_machine
def create(self):
names1 = [
'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
'robot1_elbow_joint', 'robot1_wrist_1_joint',
'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
]
pick1_group = 'robot1'
pick2_group = 'robot2'
robot1_loc = Pose2D(x=-0.2, y=2.03, theta=0.0)
names2 = [
'robot2_shoulder_pan_joint', 'robot2_shoulder_lift_joint',
'robot2_elbow_joint', 'robot2_wrist_1_joint',
'robot2_wrist_2_joint', 'robot2_wrist_3_joint'
]
gripper1 = "vacuum_gripper1_suction_cup"
gripper2 = "vacuum_gripper2_suction_cup"
robot2_loc = Pose2D(x=-8.2, y=-1.4, theta=0.0)
# x:9 y:195, x:413 y:347
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.part_pose = []
_state_machine.userdata.speed = '100.0'
_state_machine.userdata.robot1_loc = robot1_loc
_state_machine.userdata.pick2_configuration = []
_state_machine.userdata.pose_turtlebot = []
_state_machine.userdata.robot2_loc = robot2_loc
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:87 y:217
OperatableStateMachine.add('Move R2 to Home',
SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Move R1 Home',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1025 y:148
OperatableStateMachine.add('Compute Pick for R1',
ComputeGraspState(group=pick1_group,
offset=0.0,
joint_names=names1,
tool_link=gripper1),
transitions={
'continue': 'Activate Gripper 1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick1_configuration',
'joint_names': 'joint_names'
})
# x:885 y:309
OperatableStateMachine.add(
'Move R1 to Pick',
MoveitToJointsDynState(move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Move R1 back Home',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick1_configuration',
'joint_names': 'joint_names'
})
# x:421 y:15
OperatableStateMachine.add('Start feeder',
ControlFeederState(activation=True),
transitions={
'succeeded': 'Wait for part',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:588 y:25
OperatableStateMachine.add('Stop feeder',
ControlFeederState(activation=False),
transitions={
'succeeded': 'Stop Conveyor',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:845 y:227
OperatableStateMachine.add('Activate Gripper 1',
VacuumGripperControlState(
enable=True,
service_name='/gripper1/control'),
transitions={
'continue': 'Move R1 to Pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:874 y:578
OperatableStateMachine.add('Compute place Turtlebot',
ComputeGraspState(group=pick1_group,
offset=0.55,
joint_names=names1,
tool_link=gripper1),
transitions={
'continue': 'Move R1 to place',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'pose_turtlebot',
'joint_values':
'place1_configuration',
'joint_names': 'joint_names'
})
# x:890 y:658
OperatableStateMachine.add(
'Move R1 to place',
MoveitToJointsDynState(move_group=pick1_group,
offset=0.55,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'place1_configuration',
'joint_names': 'joint_names'
})
# x:882 y:494
OperatableStateMachine.add(
'LocateTurtlebot',
LocateFactoryDeviceState(model_name='mobile_base',
output_frame_id='world'),
transitions={
'succeeded': 'Compute place Turtlebot',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose_turtlebot'})
# x:915 y:398
OperatableStateMachine.add('Move R1 back Home',
SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Move R1 Home',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:56 y:468
OperatableStateMachine.add('Move R2 back Home',
SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1011 y:56
OperatableStateMachine.add(
'Locate Part Camera1',
DetectPartCameraState(ref_frame='robot1_base',
camera_topic='/hrwros/logical_camera_1',
camera_frame='logical_camera_1_frame'),
transitions={
'continue': 'Compute Pick for R1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:508 y:144
OperatableStateMachine.add('Wait for part',
SubscriberState(
topic='/break_beam_sensor_change',
blocking=True,
clear=False),
transitions={
'received': 'Stop feeder',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:58 y:55
OperatableStateMachine.add('Move R1 Home',
SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Start Conveyor',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:212 y:22
OperatableStateMachine.add('Start Conveyor',
SetConveyorPowerState(stop=False),
transitions={
'succeeded': 'Start feeder',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'speed'})
# x:817 y:25
OperatableStateMachine.add('Stop Conveyor',
SetConveyorPowerState(stop=True),
transitions={
'succeeded': 'Locate Part Camera1',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'speed'})
return _state_machine
def create(self):
names1 = [
'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
'robot1_elbow_joint', 'robot1_wrist_1_joint',
'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
]
pick1_group = 'robot1'
robot1_loc = Pose2D(x=3.8, y=2.2, theta=-90.0)
gripper1 = "vacuum_gripper1_suction_cup"
robot2_loc = Pose2D(x=-4.3, y=-0.9, theta=0.0)
pick2_group = 'robot2'
gripper2 = "vacuum_gripper2_suction_cup"
names2 = [
'robot2_shoulder_pan_joint', 'robot2_shoulder_lift_joint',
'robot2_elbow_joint', 'robot2_wrist_1_joint',
'robot2_wrist_2_joint', 'robot2_wrist_3_joint'
]
# x:31 y:297, x:742 y:423
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.part_pose = []
_state_machine.userdata.robot1_loc = robot1_loc
_state_machine.userdata.pose_turtlebot = []
_state_machine.userdata.pick1_configuration = []
_state_machine.userdata.place1_configuration = []
_state_machine.userdata.conveyor_speed = 100
_state_machine.userdata.robot2_loc = robot2_loc
_state_machine.userdata.pick2_configuration = []
_state_machine.userdata.place2_configuration = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:13 y:113
OperatableStateMachine.add(
'Move R1 Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Navigate to robot1',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1347 y:308
OperatableStateMachine.add('Compute pick',
ComputeGraspState(group=pick1_group,
offset=0.0,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Activate Gripper R1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick1_configuration',
'joint_names': 'joint_names'
})
# x:1169 y:9
OperatableStateMachine.add('Start feeder',
ControlFeederState(activation=True),
transitions={
'succeeded': 'Wait for part',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1342 y:99
OperatableStateMachine.add('Stop feeder',
ControlFeederState(activation=False),
transitions={
'succeeded': 'stop conveyor',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1336 y:382
OperatableStateMachine.add('Activate Gripper R1',
VacuumGripperControlState(
enable=True,
service_name='/gripper1/control'),
transitions={
'continue': 'Move R1 to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:561 y:24
OperatableStateMachine.add('Compute place Turtlebot',
ComputeGraspState(group=pick1_group,
offset=0.8,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Move R1 to place',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'pose_turtlebot',
'joint_values':
'place1_configuration',
'joint_names': 'joint_names'
})
# x:379 y:32
OperatableStateMachine.add(
'LocateTurtlebot',
LocateFactoryDeviceState(model_name='mobile_base',
output_frame_id='world'),
transitions={
'succeeded': 'Compute place Turtlebot',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose_turtlebot'})
# x:1323 y:841
OperatableStateMachine.add(
'Move R1 back Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Move R2 Home',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1339 y:240
OperatableStateMachine.add(
'Detect Part Camera',
DetectPartCameraState(ref_frame='robot1_base',
camera_topic='/hrwros/logical_camera_1',
camera_frame='logical_camera_1_frame'),
transitions={
'continue': 'Compute pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:1369 y:26
OperatableStateMachine.add('Wait for part',
SubscriberState(
topic='/break_beam_sensor_change',
blocking=True,
clear=True),
transitions={
'received': 'Stop feeder',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:742 y:11
OperatableStateMachine.add(
'Move R1 to place',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'start conveyor',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'place1_configuration',
'joint_names': 'joint_names'
})
# x:948 y:8
OperatableStateMachine.add('start conveyor',
SetConveyorPowerState(stop=False),
transitions={
'succeeded': 'Start feeder',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'conveyor_speed'})
# x:1338 y:171
OperatableStateMachine.add('stop conveyor',
SetConveyorPowerState(stop=True),
transitions={
'succeeded': 'Detect Part Camera',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'conveyor_speed'})
# x:205 y:59
OperatableStateMachine.add('Navigate to robot1',
hrwros_factory_states__MoveBaseState(),
transitions={
'arrived': 'LocateTurtlebot',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'robot1_loc'})
# x:1313 y:762
OperatableStateMachine.add('Deactivate gripper 1',
VacuumGripperControlState(
enable=False,
service_name='/gripper1/control'),
transitions={
'continue': 'Move R1 back Home',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1351 y:448
OperatableStateMachine.add(
'Move R1 to pick',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Move R1 PreGrasp',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick1_configuration',
'joint_names': 'joint_names'
})
# x:1007 y:842
OperatableStateMachine.add('navigate to robot2',
hrwros_factory_states__MoveBaseState(),
transitions={
'arrived': 'Detect Part Camera_R2',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'robot2_loc'})
# x:839 y:839
OperatableStateMachine.add(
'Detect Part Camera_R2',
DetectPartCameraState(ref_frame='robot2_base',
camera_topic='/hrwros/logical_camera_2',
camera_frame='logical_camera_2_frame'),
transitions={
'continue': 'Compute pick_R2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:685 y:836
OperatableStateMachine.add('Compute pick_R2',
ComputeGraspState(group=pick2_group,
offset=0.0,
joint_names=names2,
tool_link=gripper2,
rotation=3.1415),
transitions={
'continue': 'Activate Gripper R2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick2_configuration',
'joint_names': 'joint_names'
})
# x:307 y:840
OperatableStateMachine.add(
'Move R2 to pick',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick2_group,
offset=0.0,
tool_link=gripper2,
action_topic='/move_group'),
transitions={
'reached': 'Move R2 Home_2',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick2_configuration',
'joint_names': 'joint_names'
})
# x:487 y:839
OperatableStateMachine.add('Activate Gripper R2',
VacuumGripperControlState(
enable=True,
service_name='/gripper2/control'),
transitions={
'continue': 'Move R2 to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1344 y:566
OperatableStateMachine.add(
'Move R1 Home_2',
hrwros_factory_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Compute place Turtlebot_2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1323 y:696
OperatableStateMachine.add(
'Move R1 to place_2',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Deactivate gripper 1',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'place1_configuration',
'joint_names': 'joint_names'
})
# x:1307 y:631
OperatableStateMachine.add('Compute place Turtlebot_2',
ComputeGraspState(group=pick1_group,
offset=0.6,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Move R1 to place_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'pose_turtlebot',
'joint_values':
'place1_configuration',
'joint_names': 'joint_names'
})
# x:1342 y:505
OperatableStateMachine.add(
'Move R1 PreGrasp',
hrwros_factory_states__SrdfStateToMoveit(
config_name='R1PreGrasp',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Move R1 Home_2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:95 y:817
OperatableStateMachine.add(
'Move R2 Home_2',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Move R2Place',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:30 y:721
OperatableStateMachine.add(
'Move R2Place',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R2Place',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Deactivate Gripper R2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1150 y:847
OperatableStateMachine.add(
'Move R2 Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'navigate to robot2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:34 y:486
OperatableStateMachine.add(
'Move R2 back Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:22 y:588
OperatableStateMachine.add('Deactivate Gripper R2',
VacuumGripperControlState(
enable=False,
service_name='/gripper2/control'),
transitions={
'continue': 'Move R2 back Home',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
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