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: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: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: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:26 y:401, x:564 y:234
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.pick_waypoint = Pose2D(-0.8, -3.65, -1.57)
_state_machine.userdata.place_waypoint = Pose2D(-8.0, 4.13, 3.14)
_state_machine.userdata.picking_state = [
0.385, 0.476, 1.266, -0.722, 1.954, -1.708, 2.0197, -1.415
]
_state_machine.userdata.prepare_state = [
0.337, 1.334, 1.328, -0.145, 1.811, 0.0, 1.639, 0.042
]
_state_machine.userdata.start_point = Point(0.48, 0.0, 0.5)
_state_machine.userdata.look_up_point = Point(2.0, 0.0, 1.1)
_state_machine.userdata.see_apriltag = Point(0.8, 0.3, 0.7)
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:46 y:23
OperatableStateMachine.add('look_down',
HeadActionState(),
transitions={
'head_arrived': 'nav_pick',
'command_error': 'failed'
},
autonomy={
'head_arrived': Autonomy.Off,
'command_error': Autonomy.Off
},
remapping={'point2see': 'start_point'})
# x:753 y:23
OperatableStateMachine.add('detect_obj',
DetectObjState(),
transitions={
'continue': 'pick_obj',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1056 y:539
OperatableStateMachine.add(
'nav_place',
MoveBaseState(),
transitions={
'arrived': 'find_apriltag',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'place_waypoint'})
# x:469 y:456
OperatableStateMachine.add('detect_plane',
DetectPlaneState(),
transitions={
'continue': 'place_obj',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1042 y:207
OperatableStateMachine.add(
'pick_moving_state',
MoveitToJointsState(
move_group='arm_with_torso',
joint_names=[
'torso_lift_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'upperarm_roll_joint',
'elbow_flex_joint', 'forearm_roll_joint',
'wrist_flex_joint', 'wrist_roll_joint'
],
action_topic='/move_group'),
transitions={
'reached': 'look_up',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={'joint_config': 'picking_state'})
# x:527 y:24
OperatableStateMachine.add(
'prepare_state',
MoveitToJointsState(
move_group='arm_with_torso',
joint_names=[
'torso_lift_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'upperarm_roll_joint',
'elbow_flex_joint', 'forearm_roll_joint',
'wrist_flex_joint', 'wrist_roll_joint'
],
action_topic='/move_group'),
transitions={
'reached': 'detect_obj',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={'joint_config': 'prepare_state'})
# x:305 y:24
OperatableStateMachine.add('nav_pick',
MoveBaseState(),
transitions={
'arrived': 'prepare_state',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'pick_waypoint'})
# x:1050 y:399
OperatableStateMachine.add(
'look_up',
HeadActionState(),
transitions={
'head_arrived': 'nav_place',
'command_error': 'failed'
},
autonomy={
'head_arrived': Autonomy.Off,
'command_error': Autonomy.Off
},
remapping={'point2see': 'look_up_point'})
# x:739 y:458
OperatableStateMachine.add('find_apriltag',
HeadActionState(),
transitions={
'head_arrived': 'detect_plane',
'command_error': 'failed'
},
autonomy={
'head_arrived': Autonomy.Off,
'command_error': Autonomy.Off
},
remapping={'point2see': 'see_apriltag'})
# x:216 y:456
OperatableStateMachine.add('place_obj',
PlaceState(),
transitions={
'continue': 'finished_pose',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1041 y:23
OperatableStateMachine.add('pick_obj',
PickState(),
transitions={
'continue': 'pick_moving_state',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:211 y:318
OperatableStateMachine.add(
'finished_pose',
MoveitToJointsState(
move_group='arm_with_torso',
joint_names=[
'torso_lift_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'upperarm_roll_joint',
'elbow_flex_joint', 'forearm_roll_joint',
'wrist_flex_joint', 'wrist_roll_joint'
],
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_config': 'picking_state'})
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):
# x:26 y:401, x:564 y:234
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.pick_waypoint = Pose2D(-1.057, 5.051, 1.826)
_state_machine.userdata.place_waypoint = Pose2D(-10.65, -17.47, -1.468)
_state_machine.userdata.picking_state = [0.385, 0.476, 1.266, -0.722, 1.954, -1.708, 2.0197, -1.415]
_state_machine.userdata.prepare_state = [0.337, 1.334, 1.328, -0.145, 1.811, 0.0, 1.639, 0.042]
_state_machine.userdata.start_point = Point(0.8, 0.0, 0.6)
_state_machine.userdata.look_up_point = Point(2.0, 0.0, 1.1)
_state_machine.userdata.see_apriltag = Point(0.8, 0.2, 0.7)
_state_machine.userdata.init_joints = [0.3675, 1.5585, 1.421, -0.1457, 1.1609, -0.027, 1.966, -1.371]
_state_machine.userdata.prepare_state2 = [1.334, 1.328, -0.145, 1.811, 0.0, 1.639, 0.042]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:25 y:72
OperatableStateMachine.add('init_arm_pose',
MoveitToJointsState(move_group='arm_with_torso', joint_names=['torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'], action_topic='/move_group'),
transitions={'reached': 'look_down', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'prepare_state'})
# x:1102 y:44
OperatableStateMachine.add('detect_obj',
DetectObjState(),
transitions={'continue': 'pick_obj', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:820 y:46
OperatableStateMachine.add('prepare_state',
MoveitToJointsState(move_group='arm_with_torso', joint_names=['torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'], action_topic='/move_group'),
transitions={'reached': 'detect_obj', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'prepare_state'})
# x:587 y:53
OperatableStateMachine.add('nav_pick',
MoveBaseState(),
transitions={'arrived': 'prepare_state', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'pick_waypoint'})
# x:1103 y:510
OperatableStateMachine.add('look_up',
HeadActionState(),
transitions={'head_arrived': 'nav_place', 'command_error': 'failed'},
autonomy={'head_arrived': Autonomy.Off, 'command_error': Autonomy.Off},
remapping={'point2see': 'look_up_point'})
# x:1095 y:214
OperatableStateMachine.add('pick_obj',
PickState(),
transitions={'continue': 'pick_moving_state', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:292 y:61
OperatableStateMachine.add('look_down',
HeadActionState(),
transitions={'head_arrived': 'nav_pick', 'command_error': 'failed'},
autonomy={'head_arrived': Autonomy.Off, 'command_error': Autonomy.Off},
remapping={'point2see': 'start_point'})
# x:537 y:507
OperatableStateMachine.add('find_apriltag',
HeadActionState(),
transitions={'head_arrived': 'detect_plane', 'command_error': 'failed'},
autonomy={'head_arrived': Autonomy.Off, 'command_error': Autonomy.Off},
remapping={'point2see': 'see_apriltag'})
# x:271 y:511
OperatableStateMachine.add('detect_plane',
DetectPlaneState(),
transitions={'continue': 'place_obj', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:178 y:396
OperatableStateMachine.add('place_obj',
PlaceState(),
transitions={'continue': 'finished_state', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:1125 y:372
OperatableStateMachine.add('pick_moving_state',
MoveitToJointsState(move_group='arm_with_torso', joint_names=['torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'], action_topic='/move_group'),
transitions={'reached': 'look_up', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'picking_state'})
# x:177 y:249
OperatableStateMachine.add('finished_state',
MoveitToJointsState(move_group='arm', joint_names=['shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'], 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_config': 'prepare_state2'})
# x:833 y:506
OperatableStateMachine.add('nav_place',
MoveBaseState(),
transitions={'arrived': 'find_apriltag', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'place_waypoint'})
return _state_machine
def create(self):
# x:328 y:654, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.part_type = 'gear_part_blue'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.move_group_R = 'Right_Arm'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.config_name_R = 'Right_Shelf'
_state_machine.userdata.pose = []
_state_machine.userdata.offset = 0.03
_state_machine.userdata.rotation = 0
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.tool_link = 'right_ee_link'
_state_machine.userdata.move_group = ''
_state_machine.userdata.config_name = 'Full_Shelf'
_state_machine.userdata.waypoint = [-3.7, 1.5]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('start',
StartAssignment(),
transitions={'continue': 'Find'},
autonomy={'continue': Autonomy.Off})
# x:290 y:149
OperatableStateMachine.add('MOve',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'cam',
'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_R',
'move_group': 'move_group_R',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:285 y:343
OperatableStateMachine.add(
'Compute grasp',
ComputeGraspAriacState(joint_names=[
'right_elbow_joint', 'right_shoulder_lift_joint',
'right_shoulder_pan_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'move',
'failed': 'Compute grasp'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_R',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:279 y:232
OperatableStateMachine.add(
'cam',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Compute grasp',
'failed': 'failed',
'not_found': 'Compute grasp'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:277 y:429
OperatableStateMachine.add('move',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'grip',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_R',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:274 y:503
OperatableStateMachine.add('grip',
GripperControl(enable=True),
transitions={
'continue': 'MOve_2_2',
'failed': 'move',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'move_group_R'})
# x:293 y:68
OperatableStateMachine.add('MOve_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MOve',
'planning_failed': 'failed',
'control_failed': 'MOve_2',
'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',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:469 y:553
OperatableStateMachine.add('MOve_2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'MOve_2_2',
'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',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:30 y:102
OperatableStateMachine.add('Find',
FindPart(time_out=0.2),
transitions={
'found': 'gantry move',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'found': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'gantry_pos': 'gantry_pos',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame'
})
# x:592 y:183
OperatableStateMachine.add('gantry move',
MoveBaseState(),
transitions={
'arrived': 'MOve_2',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'waypoint'})
return _state_machine
def create(self):
# x:26 y:401, x:564 y:234
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.pick_waypoint = Pose2D(-0.3, -3.8, 0)
_state_machine.userdata.place_waypoint = Pose2D(-8.0, 4.13, 3.14)
_state_machine.userdata.picking_state = [0.385, 0.476, 1.266, -0.722, 1.954, -1.708, 2.0197, -1.415]
_state_machine.userdata.prepare_state = [3.141, 2.75, 1.0, -1.57, 0, 1.483]
_state_machine.userdata.start_point = Point(0.48, 0.0, 0.5)
_state_machine.userdata.look_up_point = Point(2.0, 0.0, 1.1)
_state_machine.userdata.see_apriltag = Point(0.8, 0.3, 0.7)
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:123 y:37
OperatableStateMachine.add('prepare_state',
MoveitToJointsState(move_group='arm', joint_names=['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6'], action_topic='/move_group'),
transitions={'reached': 'nav_pick', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'prepare_state'})
# x:1137 y:421
OperatableStateMachine.add('nav_place',
MoveBaseState(),
transitions={'arrived': 'detect_plane', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'place_waypoint'})
# x:706 y:544
OperatableStateMachine.add('detect_plane',
DetectPlaneState(),
transitions={'continue': 'place_obj', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:1096 y:252
OperatableStateMachine.add('pick_moving_state',
MoveitToJointsState(move_group='arm', joint_names=['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6'], action_topic='/move_group'),
transitions={'reached': 'nav_place', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'prepare_state'})
# x:576 y:44
OperatableStateMachine.add('nav_pick',
MoveBaseState(),
transitions={'arrived': 'detect_obj', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'pick_waypoint'})
# x:372 y:536
OperatableStateMachine.add('place_obj',
PlaceState(),
transitions={'continue': 'finished_pose', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:1094 y:67
OperatableStateMachine.add('pick_obj',
PickState(),
transitions={'continue': 'pick_moving_state', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:119 y:540
OperatableStateMachine.add('finished_pose',
MoveitToJointsState(move_group='arm', joint_names=['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6'], 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_config': 'prepare_state'})
# x:822 y:30
OperatableStateMachine.add('detect_obj',
DetectObjState(),
transitions={'continue': 'pick_obj', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
return _state_machine
def create(self):
# x:383 y:390, x:182 y:190
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['reason'])
_state_machine.userdata.waypoint1 = {
'coordinate': {
'x': -7.0,
'y': -7.476,
'theta': 0.0
},
'increment': {
'x': 'none',
'y': 'none',
'theta': 'none'
}
}
_state_machine.userdata.reason = 0
# 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=3),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
# x:351 y:74
OperatableStateMachine.add('s1',
SubscriberState(topic='/pose',
blocking=True,
clear=False),
transitions={
'received': 's2',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'curr_pose'})
# x:351 y:224
OperatableStateMachine.add('s2',
SubscriberState(topic='/set_waypoint',
blocking=True,
clear=False),
transitions={
'received': 'm1',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'waypoint2'})
# x:81 y:309
OperatableStateMachine.add('m1',
MoveBaseState(),
transitions={
'arrived': 'finished',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'waypoint': 'waypoint1',
'curr_pose': 'curr_pose'
})
return _state_machine