def create(self):
# x:1274 y:459, x:654 y:517
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.move_groupL = 'Left_Arm'
_state_machine.userdata.move_groupR = 'Right_Arm'
_state_machine.userdata.armidl = 'left'
_state_machine.userdata.armidr = 'right'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.config_namePBL = 'Left_Pre_Band'
_state_machine.userdata.config_namePBR = 'Right_Pre_Band'
_state_machine.userdata.Power = 100
_state_machine.userdata.NoPower = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:51 y:37
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'BeltOff'},
autonomy={'continue': Autonomy.Off})
# x:1464 y:55
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:1219 y:45
OperatableStateMachine.add('belt_to_bin',
self.use_behavior(belt_to_binSM, 'belt_to_bin'),
transitions={'finished': 'EndAssignment', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:616 y:39
OperatableStateMachine.add('RH',
SrdfStateToMoveitAriac(),
transitions={'reached': 'GripperUitR', '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_namePBR', 'move_group': 'move_groupR', '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:428 y:38
OperatableStateMachine.add('GripperUitL',
VacuumGripperControlState(enable=False),
transitions={'continue': 'RH', 'failed': 'failed', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'armidl'})
# x:791 y:39
OperatableStateMachine.add('GripperUitR',
VacuumGripperControlState(enable=False),
transitions={'continue': 'BeltOn', 'failed': 'failed', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'armidr'})
# x:225 y:36
OperatableStateMachine.add('LH',
SrdfStateToMoveitAriac(),
transitions={'reached': 'GripperUitL', '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_namePBL', 'move_group': 'move_groupL', '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:112 y:134
OperatableStateMachine.add('BeltOff',
SetConveyorbeltPowerState(),
transitions={'continue': 'LH', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'NoPower'})
# x:1011 y:39
OperatableStateMachine.add('BeltOn',
SetConveyorbeltPowerState(),
transitions={'continue': 'belt_to_bin', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'Power'})
return _state_machine
def create(self):
# x:1089 y:770, x:1143 y:423, x:1266 y:674
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed', 'end'],
input_keys=[
'move_group_prefix', 'move_group_right_arm',
'move_group_left_arm', 'action_topic', 'camera_frame',
'camera_topic', 'ref_frame', 'move_group_gantry', 'robot_name',
'offset_var_left', 'offset_var_right', 'offset_gasket',
'offset_piston', 'arm_idL', 'arm_idR',
'config_name_Right_Home', 'config_name_Left_Home',
'config_name_RA_pre', 'config_name_LA_pre'
])
_state_machine.userdata.move_group_prefix = ''
_state_machine.userdata.move_group_right_arm = ''
_state_machine.userdata.move_group_left_arm = ''
_state_machine.userdata.action_topic = ''
_state_machine.userdata.camera_frame = '/ariac/logical_camera_0'
_state_machine.userdata.camera_topic = 'logical_camera_0_frame'
_state_machine.userdata.ref_frame = ''
_state_machine.userdata.move_group_gantry = ''
_state_machine.userdata.robot_name = ''
_state_machine.userdata.offset_var_left = 0
_state_machine.userdata.offset_var_right = 0
_state_machine.userdata.offset_gasket = 0
_state_machine.userdata.offset_piston = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.tool_link_left = 'left_ee_link'
_state_machine.userdata.tool_link_right = 'right_ee_link'
_state_machine.userdata.arm_idL = ''
_state_machine.userdata.arm_idR = ''
_state_machine.userdata.config_name_Right_Home = ''
_state_machine.userdata.config_name_Left_Home = ''
_state_machine.userdata.pose = ''
_state_machine.userdata.bin_id = ''
_state_machine.userdata.bin1 = 'bin1'
_state_machine.userdata.bin2 = 'bin2'
_state_machine.userdata.offset_id = 0
_state_machine.userdata.arm_id = ''
_state_machine.userdata.config_name_R_bin1 = 'Gantry_PreGrasp_R_bin1'
_state_machine.userdata.config_name_L_bin1 = 'Gantry_PreGrasp_L_bin1'
_state_machine.userdata.config_name_bin1_var = ''
_state_machine.userdata.config_name_RA_pre = ''
_state_machine.userdata.config_name_LA_pre = ''
_state_machine.userdata.offsetx_bin1 = 0
_state_machine.userdata.offsety_bin1 = 0
_state_machine.userdata.offsetx_set = 0.33418
_state_machine.userdata.offsety_set = 0.1827866667
_state_machine.userdata.plus1 = 1
_state_machine.userdata.xproduct_bin1 = 0
_state_machine.userdata.yproduct_bin1 = 0
_state_machine.userdata.drie = 3
_state_machine.userdata.twee = 2
_state_machine.userdata.zero = 0
_state_machine.userdata.offsety_bin2 = 0
_state_machine.userdata.offsetx_bin2 = 0
_state_machine.userdata.yproduct_bin2 = 0
_state_machine.userdata.xproduct_bin2 = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:1623 y:384, x:886 y:382, x:1759 y:258
_sm_offset_verschuiven_0 = OperatableStateMachine(
outcomes=['finished', 'failed', 'end'],
input_keys=[
'bin_id', 'bin1', 'zero', 'twee', 'drie', 'bin2',
'offsetx_set', 'offsety_set', 'plus1', 'offsetx_bin1',
'offsety_bin2', 'offsetx_bin2', 'offsety_bin1',
'xproduct_bin1', 'yproduct_bin1', 'xproduct_bin2',
'yproduct_bin2'
],
output_keys=[
'offsety_bin2', 'offsetx_bin2', 'offsety_bin1', 'offsetx_bin1'
])
with _sm_offset_verschuiven_0:
# x:146 y:40
OperatableStateMachine.add('bin1 vergelijk',
EqualState(),
transitions={
'true': 'set offsetyyy',
'false': 'set offsetyy'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin1',
'value_b': 'bin_id'
})
# x:610 y:50
OperatableStateMachine.add(
'x producten tellen',
AddNumericState(),
transitions={'done': '3 products x bin?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'xproduct_bin1',
'value_b': 'plus1',
'result': 'xproduct_bin1'
})
# x:818 y:45
OperatableStateMachine.add('3 products x bin?',
EqualState(),
transitions={
'true': 'set offsety',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'xproduct_bin1',
'value_b': 'drie'
})
# x:1069 y:54
OperatableStateMachine.add(
'set offsety',
ReplaceState(),
transitions={'done': 'set offsety back to zero'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetx_set',
'result': 'offsety_bin1'
})
# x:1263 y:51
OperatableStateMachine.add('set offsety back to zero',
ReplaceState(),
transitions={'done': '2 products x?'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'offsety_bin1'
})
# x:1471 y:55
OperatableStateMachine.add('2 products x?',
EqualState(),
transitions={
'true': 'st offsetx back to zero',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'twee',
'value_b': 'xproduct_bin1'
})
# x:389 y:38
OperatableStateMachine.add(
'set offsetyyy',
AddNumericState(),
transitions={'done': 'x producten tellen'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'offsety_set',
'value_b': 'offsetx_bin1',
'result': 'offsetx_bin1'
})
# x:610 y:161
OperatableStateMachine.add(
'y producten tellen_2',
AddNumericState(),
transitions={'done': '3 products y bin2?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'yproduct_bin2',
'value_b': 'plus1',
'result': 'yproduct_bin2'
})
# x:818 y:152
OperatableStateMachine.add('3 products y bin2?',
EqualState(),
transitions={
'true': 'set offsety_2',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'yproduct_bin2',
'value_b': 'drie'
})
# x:1066 y:165
OperatableStateMachine.add(
'set offsety_2',
ReplaceState(),
transitions={'done': 'set offsety back to zero_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetx_set',
'result': 'offsetx_bin2'
})
# x:1263 y:176
OperatableStateMachine.add(
'set offsety back to zero_2',
ReplaceState(),
transitions={'done': '2 products x bin2?'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'offsety_bin2'
})
# x:1472 y:149
OperatableStateMachine.add('2 products x bin2?',
EqualState(),
transitions={
'true':
'set offsetx back to zero bin2',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'twee',
'value_b': 'xproduct_bin2'
})
# x:389 y:152
OperatableStateMachine.add(
'set offsetyy',
AddNumericState(),
transitions={'done': 'y producten tellen_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'offsety_set',
'value_b': 'offsety_bin2',
'result': 'offsety_bin2'
})
# x:1651 y:50
OperatableStateMachine.add('st offsetx back to zero',
ReplaceState(),
transitions={'done': 'end'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'offsetx_bin1'
})
# x:1653 y:146
OperatableStateMachine.add('set offsetx back to zero bin2',
ReplaceState(),
transitions={'done': 'end'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'offsetx_bin2'
})
# x:30 y:401, x:130 y:401
_sm_setten_voor_right_1 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'arm_idR', 'move_group_right_arm', 'config_name_Right_Home',
'tool_link_right', 'config_name_RA_pre', 'offset_var_right'
],
output_keys=[
'arm_id_var', 'move_group_var', 'config_name_var',
'tool_link_var', 'config_name_pre_var', 'offset_id'
])
with _sm_setten_voor_right_1:
# x:30 y:40
OperatableStateMachine.add('Set arm id',
ReplaceState(),
transitions={'done': 'Set config name'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm_idR',
'result': 'arm_id_var'
})
# x:30 y:117
OperatableStateMachine.add('Set config name',
ReplaceState(),
transitions={'done': 'Set toollink'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_Right_Home',
'result': 'config_name_var'
})
# x:30 y:194
OperatableStateMachine.add('Set toollink',
ReplaceState(),
transitions={'done': 'Set move group'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'tool_link_right',
'result': 'tool_link_var'
})
# x:30 y:271
OperatableStateMachine.add(
'Set move group',
ReplaceState(),
transitions={'done': 'Set config name_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'move_group_right_arm',
'result': 'move_group_var'
})
# x:230 y:266
OperatableStateMachine.add(
'Set config name_2',
ReplaceState(),
transitions={'done': 'set config pre pick'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset_var_right',
'result': 'offset_id'
})
# x:233 y:344
OperatableStateMachine.add('laat zien',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'move_group_var'})
# x:438 y:268
OperatableStateMachine.add('set config pre pick',
ReplaceState(),
transitions={'done': 'laat zien'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_RA_pre',
'result': 'config_name_pre_var'
})
# x:30 y:401
_sm_setten_voor_left_2 = OperatableStateMachine(
outcomes=['finished'],
input_keys=[
'arm_idL', 'move_group_left_arm', 'config_name_Left_Home',
'tool_link_left', 'config_name_LA_pre', 'offset_var_left'
],
output_keys=[
'arm_id_var', 'move_group_var', 'config_name_var',
'tool_link_var', 'config_name_pre_var', 'offset_id'
])
with _sm_setten_voor_left_2:
# x:30 y:40
OperatableStateMachine.add('Set arm id',
ReplaceState(),
transitions={'done': 'Set config name'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm_idL',
'result': 'arm_id_var'
})
# x:30 y:117
OperatableStateMachine.add('Set config name',
ReplaceState(),
transitions={'done': 'Set toollink'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_Left_Home',
'result': 'config_name_var'
})
# x:30 y:194
OperatableStateMachine.add('Set toollink',
ReplaceState(),
transitions={'done': 'Set move group'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'tool_link_left',
'result': 'tool_link_var'
})
# x:30 y:271
OperatableStateMachine.add('Set move group',
ReplaceState(),
transitions={'done': 'Set offset'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'move_group_left_arm',
'result': 'move_group_var'
})
# x:397 y:386
OperatableStateMachine.add('laat zien',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'move_group_var'})
# x:383 y:274
OperatableStateMachine.add('set config name pre pick',
ReplaceState(),
transitions={'done': 'laat zien'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_LA_pre',
'result': 'config_name_pre_var'
})
# x:209 y:266
OperatableStateMachine.add(
'Set offset',
ReplaceState(),
transitions={'done': 'set config name pre pick'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset_var_left',
'result': 'offset_id'
})
with _state_machine:
# x:97 y:17
OperatableStateMachine.add(
'Setten voor Left',
_sm_setten_voor_left_2,
transitions={'finished': 'Left product'},
autonomy={'finished': Autonomy.Inherit},
remapping={
'arm_idL': 'arm_idL',
'move_group_left_arm': 'move_group_left_arm',
'config_name_Left_Home': 'config_name_Left_Home',
'tool_link_left': 'tool_link_left',
'config_name_LA_pre': 'config_name_LA_pre',
'offset_var_left': 'offset_var_left',
'arm_id_var': 'arm_id_var',
'move_group_var': 'move_group_var',
'config_name_var': 'config_name_var',
'tool_link_var': 'tool_link_var',
'config_name_pre_var': 'config_name_pre_var',
'offset_id': 'offset_id'
})
# x:190 y:708
OperatableStateMachine.add(
'Setten voor Right',
_sm_setten_voor_right_1,
transitions={
'finished': 'Right product',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'arm_idR': 'arm_idR',
'move_group_right_arm': 'move_group_right_arm',
'config_name_Right_Home': 'config_name_Right_Home',
'tool_link_right': 'tool_link_right',
'config_name_RA_pre': 'config_name_RA_pre',
'offset_var_right': 'offset_var_right',
'arm_id_var': 'arm_id_var',
'move_group_var': 'move_group_var',
'config_name_var': 'config_name_var',
'tool_link_var': 'tool_link_var',
'config_name_pre_var': 'config_name_pre_var',
'offset_id': 'offset_id'
})
# x:77 y:172
OperatableStateMachine.add('Left product',
EqualState(),
transitions={
'true': 'bin1',
'false': 'bin2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'offset_gasket',
'value_b': 'offset_id'
})
# x:311 y:29
OperatableStateMachine.add('bin1',
ReplaceState(),
transitions={'done': 'Zie bin1_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin1',
'result': 'bin_id'
})
# x:300 y:252
OperatableStateMachine.add('bin2',
ReplaceState(),
transitions={'done': 'Zie bin1_2_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin2',
'result': 'bin_id'
})
# x:116 y:534
OperatableStateMachine.add('Right product',
EqualState(),
transitions={
'true': 'bin1_2',
'false': 'bin2_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'offset_gasket',
'value_b': 'offset_id'
})
# x:1673 y:531
OperatableStateMachine.add('turn off vacuum',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'Move to home',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_var'})
# x:299 y:610
OperatableStateMachine.add('bin1_2',
ReplaceState(),
transitions={'done': 'Zie bin1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin1',
'result': 'bin_id'
})
# x:294 y:377
OperatableStateMachine.add('bin2_2',
ReplaceState(),
transitions={'done': 'Zie bin2_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin2',
'result': 'bin_id'
})
# x:731 y:21
OperatableStateMachine.add('gantry bin pose',
ReplaceState(),
transitions={'done': 'Move to bin'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_L_bin1',
'result': 'config_name_bin1_var'
})
# x:722 y:342
OperatableStateMachine.add('gantry bin pose_2',
ReplaceState(),
transitions={'done': 'Move to bin'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_R_bin1',
'result': 'config_name_bin1_var'
})
# x:1691 y:675
OperatableStateMachine.add('Move to home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Offset verschuiven',
'planning_failed': 'retry_4',
'control_failed': 'retry_4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_var',
'move_group': 'move_group_var',
'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:987 y:701
OperatableStateMachine.add('Right robot end',
EqualState(),
transitions={
'true': 'finished',
'false': 'Setten voor Right'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'move_group_right_arm',
'value_b': 'move_group_var'
})
# x:954 y:146
OperatableStateMachine.add('retry',
WaitState(wait_time=0.1),
transitions={'done': 'Move to bin'},
autonomy={'done': Autonomy.Off})
# x:1578 y:610
OperatableStateMachine.add('retry_4',
WaitState(wait_time=0.1),
transitions={'done': 'Move to home'},
autonomy={'done': Autonomy.Off})
# x:956 y:21
OperatableStateMachine.add('Move to bin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Check for bin',
'planning_failed': 'retry',
'control_failed': 'retry',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin1_var',
'move_group': 'move_group_gantry',
'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:1691 y:248
OperatableStateMachine.add(
'Computebin right',
ComputeDropBinsState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Move to drop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_var',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_var',
'pose': 'pose',
'offset': 'offset_id',
'rotation': 'rotation',
'offsety': 'offsety',
'offsetx': 'offsetx',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1550 y:336
OperatableStateMachine.add(
'Computebin left',
ComputeDropBinsState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Move to drop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_var',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_var',
'pose': 'pose',
'offset': 'offset_id',
'rotation': 'rotation',
'offsety': 'offsety',
'offsetx': 'offsetx',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:516 y:599
OperatableStateMachine.add('Zie bin1',
GetObjectPoseState(
object_frame='bin1_frame',
ref_frame='world'),
transitions={
'continue': 'gantry bin pose_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:529 y:34
OperatableStateMachine.add('Zie bin1_2',
GetObjectPoseState(
object_frame='bin1_frame',
ref_frame='world'),
transitions={
'continue': 'gantry bin pose',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:534 y:239
OperatableStateMachine.add('Zie bin1_2_2',
GetObjectPoseState(
object_frame='bin2_frame',
ref_frame='world'),
transitions={
'continue': 'gantry bin pose_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:523 y:373
OperatableStateMachine.add('Zie bin2_2',
GetObjectPoseState(
object_frame='bin2_frame',
ref_frame='world'),
transitions={
'continue': 'gantry bin pose',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:1668 y:30
OperatableStateMachine.add('Right robot end_2',
EqualState(),
transitions={
'true': 'Computebin right',
'false': 'Computebin left'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'move_group_right_arm',
'value_b': 'move_group_var'
})
# x:1657 y:420
OperatableStateMachine.add('Move to drop',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'turn off vacuum',
'planning_failed': 'failed',
'control_failed': 'turn off vacuum'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_var',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1428 y:672
OperatableStateMachine.add('Offset verschuiven',
_sm_offset_verschuiven_0,
transitions={
'finished': 'Right robot end',
'failed': 'failed',
'end': 'end'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'end': Autonomy.Inherit
},
remapping={
'bin_id': 'bin_id',
'bin1': 'bin1',
'zero': 'zero',
'twee': 'twee',
'drie': 'drie',
'bin2': 'bin2',
'offsetx_set': 'offsetx_set',
'offsety_set': 'offsety_set',
'plus1': 'plus1',
'offsetx_bin1': 'offsetx_bin1',
'offsety_bin2': 'offsety_bin2',
'offsetx_bin2': 'offsetx_bin2',
'offsety_bin1': 'offsety_bin1',
'xproduct_bin1': 'xproduct_bin1',
'yproduct_bin1': 'yproduct_bin1',
'xproduct_bin2': 'xproduct_bin2',
'yproduct_bin2': 'yproduct_bin2'
})
# x:1156 y:3
OperatableStateMachine.add('Check for bin',
EqualState(),
transitions={
'true': 'set offset voor bin 1',
'false': 'set offset voor bin 2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin1',
'value_b': 'bin_id'
})
# x:1406 y:0
OperatableStateMachine.add(
'set offset voor bin 1',
ReplaceState(),
transitions={'done': 'set offset voor y bin 1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetx_bin1',
'result': 'offsetx'
})
# x:1441 y:64
OperatableStateMachine.add(
'set offset voor y bin 1',
ReplaceState(),
transitions={'done': 'Right robot end_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsety_bin1',
'result': 'offsety'
})
# x:1236 y:110
OperatableStateMachine.add(
'set offset voor bin 2',
ReplaceState(),
transitions={'done': 'set offset voor y bin 2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetx_bin2',
'result': 'offsetx'
})
# x:1425 y:141
OperatableStateMachine.add(
'set offset voor y bin 2',
ReplaceState(),
transitions={'done': 'Right robot end_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsety_bin2',
'result': 'offsety'
})
return _state_machine
def create(self):
# x:92 y:307, x:966 y:441
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'offset_part_left', 'PreGrasp', 'camera_topic', 'camera_frame',
'offset_part_left', 'part_type', 'arm_id_left', 'part_pose'
],
output_keys=['part_pose_left'])
_state_machine.userdata.move_group_gantry = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.move_group_right_arm = 'Right_Arm'
_state_machine.userdata.move_group_left_arm = 'Left_Arm'
_state_machine.userdata.config_name_pre_shelves = 'Gantry_PreGrasp_Pre_shelves'
_state_machine.userdata.PreGrasp = ''
_state_machine.userdata.camera_ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.offset_part_left = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.tool_link_left = 'left_ee_link'
_state_machine.userdata.arm_id_left = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.part_pose_left = []
_state_machine.userdata.config_name_left_arm2 = 'Left_Home_shelves'
_state_machine.userdata.config_name_shelves = 'Gantry_PreGrasp_shelves'
_state_machine.userdata.config_name_right_arm2 = 'Right_Home_shelves'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:745 y:152
OperatableStateMachine.add('PreShelf',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DetectPart',
'planning_failed': 'Retry_4',
'control_failed': 'Retry_4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_pre_shelves',
'move_group': 'move_group_gantry',
'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:773 y:27
OperatableStateMachine.add('Retry_4',
WaitState(wait_time=0.3),
transitions={'done': 'PreShelf'},
autonomy={'done': Autonomy.Off})
# x:1558 y:259
OperatableStateMachine.add('PreGrasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputePick',
'planning_failed': 'Retry_5',
'control_failed': 'Retry_5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'PreGrasp',
'move_group': 'move_group_gantry',
'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:1730 y:262
OperatableStateMachine.add('Retry_5',
WaitState(wait_time=0.3),
transitions={'done': 'PreGrasp'},
autonomy={'done': Autonomy.Off})
# x:1548 y:361
OperatableStateMachine.add(
'ComputePick',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Move_to_pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_left_arm',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_left',
'pose': 'pose',
'offset': 'offset_part_left',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:918 y:149
OperatableStateMachine.add(
'DetectPart',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Move_left_arm_home2',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:1734 y:461
OperatableStateMachine.add('Retry_6',
WaitState(wait_time=0.3),
transitions={'done': 'Move_to_pick'},
autonomy={'done': Autonomy.Off})
# x:1533 y:463
OperatableStateMachine.add('Move_to_pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperOn',
'planning_failed': 'Retry_6',
'control_failed': 'GripperOn'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_left_arm',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1535 y:573
OperatableStateMachine.add('GripperOn',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'Move_left_arm_home_2',
'failed': 'ComputePick',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_left'})
# x:1378 y:653
OperatableStateMachine.add('Move_left_arm_home_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Shelves_2',
'planning_failed': 'Retry_7',
'control_failed': 'Retry_7',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_left_arm2',
'move_group': 'move_group_left_arm',
'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:1426 y:747
OperatableStateMachine.add(
'Retry_7',
WaitState(wait_time=0.3),
transitions={'done': 'Move_left_arm_home_2'},
autonomy={'done': Autonomy.Off})
# x:867 y:641
OperatableStateMachine.add('PreShelf_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Pose',
'planning_failed': 'Retry_8',
'control_failed': 'Retry_8',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_pre_shelves',
'move_group': 'move_group_gantry',
'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:889 y:738
OperatableStateMachine.add('Retry_8',
WaitState(wait_time=0.3),
transitions={'done': 'PreShelf_2'},
autonomy={'done': Autonomy.Off})
# x:127 y:421
OperatableStateMachine.add('Pose',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'part_pose',
'result': 'part_pose_left'
})
# x:1135 y:148
OperatableStateMachine.add('Move_left_arm_home2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move_right_arm_home2',
'planning_failed': 'Retry_9',
'control_failed': 'Retry_9',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_left_arm2',
'move_group': 'move_group_left_arm',
'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:1159 y:31
OperatableStateMachine.add(
'Retry_9',
WaitState(wait_time=0.3),
transitions={'done': 'Move_left_arm_home2'},
autonomy={'done': Autonomy.Off})
# x:1346 y:147
OperatableStateMachine.add(
'Move_right_arm_home2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Shelves',
'planning_failed': 'Retry_10',
'control_failed': 'Retry_10',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_right_arm2',
'move_group': 'move_group_right_arm',
'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:1381 y:35
OperatableStateMachine.add(
'Retry_10',
WaitState(wait_time=0.3),
transitions={'done': 'Move_right_arm_home2'},
autonomy={'done': Autonomy.Off})
# x:1561 y:148
OperatableStateMachine.add('Shelves',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreGrasp',
'planning_failed': 'Retry_11',
'control_failed': 'Retry_11',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_shelves',
'move_group': 'move_group_gantry',
'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:1591 y:35
OperatableStateMachine.add('Retry_11',
WaitState(wait_time=0.3),
transitions={'done': 'Shelves'},
autonomy={'done': Autonomy.Off})
# x:1063 y:639
OperatableStateMachine.add('Shelves_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreShelf_2',
'planning_failed': 'Retry_12',
'control_failed': 'Retry_12',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_shelves',
'move_group': 'move_group_gantry',
'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:1074 y:746
OperatableStateMachine.add('Retry_12',
WaitState(wait_time=0.3),
transitions={'done': 'Shelves_2'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:944 y:732, x:764 y:355
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.part_type = 'gear_part_red'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.offset = 0.025
_state_machine.userdata.move_group_G = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_bins1'
_state_machine.userdata.camera_frame = 'logical_camera_bins1_frame'
_state_machine.userdata.tool_link_R = 'right_ee_link'
_state_machine.userdata.arm_id = 'Left_Arm'
_state_machine.userdata.config_name_Gantry_pick = 'Gantry_Bins_LinksBovenRechts'
_state_machine.userdata.null = 0
_state_machine.userdata.config_name_Gantry_Transportband = 'Gantry_Transportband'
_state_machine.userdata.config_name_Gantry_AGV = 'Gantry_AGV_Rechts_L'
_state_machine.userdata.config_name_Left_home = 'Left_Home'
_state_machine.userdata.config_name_Right_home = 'Right_Home'
_state_machine.userdata.move_group_L = 'Left_Arm'
_state_machine.userdata.move_group_R = 'Right_Arm'
_state_machine.userdata.config_name_L = 'Links_PreDrop_Stelling'
_state_machine.userdata.config_name_R = 'Rechts_PreDrop_Stelling'
_state_machine.userdata.tool_link_L = 'left_ee_link'
_state_machine.userdata.rotation = 0
_state_machine.userdata.camera_ref_frame = 'world'
_state_machine.userdata.config_name_PrePick = 'Links_PreGrasp_Bins'
_state_machine.userdata.config_name_PreDrop = 'Links_PreDrop_AGV'
_state_machine.userdata.pose = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:73 y:57
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'stop belt'},
autonomy={'continue': Autonomy.Off})
# x:1574 y:26
OperatableStateMachine.add('Pregrasp_G',
SrdfStateToMoveitAriac(),
transitions={'reached': 'PrePick', 'planning_failed': 'failed', 'control_failed': 'PrePick', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_Gantry_pick', 'move_group': 'move_group_G', '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:261 y:30
OperatableStateMachine.add('stop belt',
SetConveyorbeltPowerState(),
transitions={'continue': 'PreLeftHome', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'null'})
# x:988 y:18
OperatableStateMachine.add('LeftHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'RightHome', 'planning_failed': 'failed', 'control_failed': 'RightHome', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_Left_home', 'move_group': 'move_group_L', '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:1259 y:20
OperatableStateMachine.add('RightHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'Pregrasp_G', 'planning_failed': 'failed', 'control_failed': 'Pregrasp_G', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_Right_home', '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:522 y:12
OperatableStateMachine.add('PreLeftHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'PreRightHome', 'planning_failed': 'failed', 'control_failed': 'PreRightHome', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_L', 'move_group': 'move_group_L', '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:759 y:14
OperatableStateMachine.add('PreRightHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'LeftHome', 'planning_failed': 'failed', 'control_failed': 'LeftHome', '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:1566 y:263
OperatableStateMachine.add('CheckPartPoseBin',
DetectPartCameraAriacState(time_out=2),
transitions={'continue': 'ComputePick', 'failed': 'failed', 'not_found': 'Retry'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ref_frame': 'camera_ref_frame', 'camera_topic': 'camera_topic', 'camera_frame': 'camera_frame', 'part': 'part_type', 'pose': 'pose'})
# x:1579 y:349
OperatableStateMachine.add('ComputePick',
ComputeGraspAriacState(joint_names=['left_shoulder_pan_joint', 'left_shoulder_lift_joint', 'left_elbow_joint', 'left_wrist_1_joint', 'left_wrist_2_joint', 'left_wrist_3_joint']),
transitions={'continue': 'Pick', 'failed': 'Retry'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'move_group': 'move_group_L', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link_L', 'pose': 'pose', 'offset': 'offset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1326 y:337
OperatableStateMachine.add('Retry',
WaitState(wait_time=0.2),
transitions={'done': 'PrePick'},
autonomy={'done': Autonomy.Off})
# x:1579 y:157
OperatableStateMachine.add('PrePick',
SrdfStateToMoveitAriac(),
transitions={'reached': 'CheckPartPoseBin', 'planning_failed': 'CheckPartPoseBin', '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_PrePick', 'move_group': 'move_group_L', '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:1329 y:732
OperatableStateMachine.add('LeftHome_2',
SrdfStateToMoveitAriac(),
transitions={'reached': 'finished', 'planning_failed': 'failed', 'control_failed': 'finished', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_Left_home', 'move_group': 'move_group_L', '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:1602 y:444
OperatableStateMachine.add('Pick',
MoveitToJointsDynAriacState(),
transitions={'reached': 'GripperEnable', 'planning_failed': 'failed', 'control_failed': 'GripperEnable'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group_L', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1597 y:703
OperatableStateMachine.add('PrePick_2',
SrdfStateToMoveitAriac(),
transitions={'reached': 'LeftHome_2', 'planning_failed': 'failed', 'control_failed': 'LeftHome_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_PrePick', 'move_group': 'move_group_L', '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:1582 y:575
OperatableStateMachine.add('GripperEnable',
VacuumGripperControlState(enable=True),
transitions={'continue': 'PrePick_2', 'failed': 'Retry', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_id'})
return _state_machine
def create(self):
# x:779 y:685, x:708 y:446
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'part_pose_right', 'part_pose_left', 'PreDropright',
'PreDropleft', 'arm_id_right', 'arm_id_left'
])
_state_machine.userdata.part_pose_right = []
_state_machine.userdata.part_pose_left = []
_state_machine.userdata.PreDropleft = ''
_state_machine.userdata.PreDropright = ''
_state_machine.userdata.value_b = 0
_state_machine.userdata.value_a = 0
_state_machine.userdata.arm_id_left = ''
_state_machine.userdata.arm_id_right = ''
_state_machine.userdata.robot_name = ''
_state_machine.userdata.tool_link_right = 'right_ee_link'
_state_machine.userdata.tool_link_left = 'left_ee_link'
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group_left_arm = 'Left_Arm'
_state_machine.userdata.move_group_right_arm = 'Right_Arm'
_state_machine.userdata.config_name_right_arm = 'Right_Home'
_state_machine.userdata.config_name_left_arm = 'Left_Home'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.move_group_gantry = 'Gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.drop_offset = 0.145
_state_machine.userdata.config_name_gantry = 'Gantry_Home'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:41 y:519
OperatableStateMachine.add('GantryHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ArmHome_3',
'planning_failed': 'Retry_7',
'control_failed': 'Retry_7',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_gantry',
'move_group': 'move_group_gantry',
'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:216 y:127
OperatableStateMachine.add('PreDropTray',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputeDrop',
'planning_failed': 'Retry',
'control_failed': 'Retry',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'PreDropleft',
'move_group': 'move_group_gantry',
'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:237 y:8
OperatableStateMachine.add('Retry',
WaitState(wait_time=0.5),
transitions={'done': 'PreDropTray'},
autonomy={'done': Autonomy.Off})
# x:603 y:127
OperatableStateMachine.add('MoveToDrop',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperOff',
'planning_failed': 'Retry_2',
'control_failed': 'Retry_2'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_left_arm',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:640 y:3
OperatableStateMachine.add('Retry_2',
WaitState(wait_time=0.5),
transitions={'done': 'MoveToDrop'},
autonomy={'done': Autonomy.Off})
# x:814 y:126
OperatableStateMachine.add('GripperOff',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'ArmHome',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_left'})
# x:1028 y:124
OperatableStateMachine.add('ArmHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GripperControl',
'planning_failed': 'Retry_3',
'control_failed': 'Retry_3',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_left_arm',
'move_group': 'move_group_left_arm',
'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:1054 y:7
OperatableStateMachine.add('Retry_3',
WaitState(wait_time=0.5),
transitions={'done': 'ArmHome'},
autonomy={'done': Autonomy.Off})
# x:1216 y:123
OperatableStateMachine.add('GripperControl',
GripperControle(),
transitions={
'Active': 'GetAGVPose_2',
'Not_active': 'GantryHome_2',
'invalid_arm_id': 'failed'
},
autonomy={
'Active': Autonomy.Off,
'Not_active': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_right'})
# x:1436 y:124
OperatableStateMachine.add('GetAGVPose_2',
GetObjectPoseState(
object_frame='kit_tray_2',
ref_frame='world'),
transitions={
'continue': 'PreDropTray_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'agv_pose_right'})
# x:1428 y:232
OperatableStateMachine.add('PreDropTray_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputeDrop_2',
'planning_failed': 'Retry_4',
'control_failed': 'Retry_4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'PreDropright',
'move_group': 'move_group_gantry',
'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:1644 y:233
OperatableStateMachine.add('Retry_4',
WaitState(wait_time=0.5),
transitions={'done': 'PreDropTray_2'},
autonomy={'done': Autonomy.Off})
# x:1398 y:447
OperatableStateMachine.add('MoveToDrop_2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperOff_2',
'planning_failed': 'Retry_5',
'control_failed': 'Retry_5'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group':
'move_group_right_arm',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1637 y:452
OperatableStateMachine.add('Retry_5',
WaitState(wait_time=0.5),
transitions={'done': 'MoveToDrop_2'},
autonomy={'done': Autonomy.Off})
# x:1397 y:582
OperatableStateMachine.add('GripperOff_2',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'ArmHome_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_right'})
# x:1417 y:689
OperatableStateMachine.add(
'ArmHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GantryHome_2',
'planning_failed': 'Retry_6',
'control_failed': 'Retry_6',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_right_arm',
'move_group': 'move_group_right_arm',
'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:1634 y:689
OperatableStateMachine.add('Retry_6',
WaitState(wait_time=0.5),
transitions={'done': 'ArmHome_2'},
autonomy={'done': Autonomy.Off})
# x:53 y:627
OperatableStateMachine.add('Retry_7',
WaitState(wait_time=0.5),
transitions={'done': 'GantryHome'},
autonomy={'done': Autonomy.Off})
# x:51 y:128
OperatableStateMachine.add('GetAGVPose',
GetObjectPoseState(
object_frame='kit_tray_2',
ref_frame='world'),
transitions={
'continue': 'PreDropTray',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'agv_pose_left'})
# x:381 y:129
OperatableStateMachine.add(
'ComputeDrop',
ComputeGraspPartOffsetAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'MoveToDrop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_left_arm',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_left',
'pose': 'agv_pose_left',
'offset': 'drop_offset',
'rotation': 'rotation',
'part_pose': 'part_pose_left',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1388 y:336
OperatableStateMachine.add(
'ComputeDrop_2',
ComputeGraspPartOffsetAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'MoveToDrop_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_right_arm',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_right',
'pose': 'agv_pose_right',
'offset': 'drop_offset',
'rotation': 'rotation',
'part_pose': 'part_pose_right',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:42 y:393
OperatableStateMachine.add('ArmHome_3',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ArmHome_2_2',
'planning_failed': 'Retry_3_2',
'control_failed': 'Retry_3_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_left_arm',
'move_group': 'move_group_left_arm',
'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:229 y:401
OperatableStateMachine.add('Retry_3_2',
WaitState(wait_time=0.5),
transitions={'done': 'ArmHome_3'},
autonomy={'done': Autonomy.Off})
# x:43 y:250
OperatableStateMachine.add(
'ArmHome_2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetAGVPose',
'planning_failed': 'Retry_6_2',
'control_failed': 'Retry_6_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_right_arm',
'move_group': 'move_group_right_arm',
'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:229 y:269
OperatableStateMachine.add('Retry_6_2',
WaitState(wait_time=0.5),
transitions={'done': 'ArmHome_2_2'},
autonomy={'done': Autonomy.Off})
# x:985 y:660
OperatableStateMachine.add('GantryHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'Retry_7_2',
'control_failed': 'Retry_7_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_gantry',
'move_group': 'move_group_gantry',
'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:1006 y:777
OperatableStateMachine.add('Retry_7_2',
WaitState(wait_time=0.5),
transitions={'done': 'GantryHome_2'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:385 y:685, x:966 y:441
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.config_name_gantry = 'Gantry_Home'
_state_machine.userdata.move_group_gantry = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.move_group_right_arm = 'Right_Arm'
_state_machine.userdata.config_name_left_arm = 'Left_Home'
_state_machine.userdata.config_name_right_arm = 'Right_Home'
_state_machine.userdata.move_group_left_arm = 'Left_Arm'
_state_machine.userdata.config_name_PreSide = 'Gantry_Pre_Side'
_state_machine.userdata.config_name_PreGrasp = 'Gantry_PreGrasp_Right_bins_1'
_state_machine.userdata.camera_ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_1'
_state_machine.userdata.camera_frame = 'logical_camera_1_frame'
_state_machine.userdata.part_type = 'pulley_part_red'
_state_machine.userdata.offset = 0.085
_state_machine.userdata.rotation = 0
_state_machine.userdata.tool_link_right = 'right_ee_link'
_state_machine.userdata.arm_id = 'right'
_state_machine.userdata.config_name_PreDrop = 'Gantry_Predrop_AGV_1'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:164 y:146
OperatableStateMachine.add('Move_left_arm_home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move_right_arm_home',
'planning_failed': 'Retry',
'control_failed': 'Retry',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_left_arm',
'move_group': 'move_group_left_arm',
'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:201 y:38
OperatableStateMachine.add(
'Retry',
WaitState(wait_time=0.3),
transitions={'done': 'Move_left_arm_home'},
autonomy={'done': Autonomy.Off})
# x:332 y:148
OperatableStateMachine.add(
'Move_right_arm_home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move_gantry_home',
'planning_failed': 'Retry_2',
'control_failed': 'Retry_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_right_arm',
'move_group': 'move_group_right_arm',
'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:362 y:37
OperatableStateMachine.add(
'Retry_2',
WaitState(wait_time=0.3),
transitions={'done': 'Move_right_arm_home'},
autonomy={'done': Autonomy.Off})
# x:525 y:149
OperatableStateMachine.add('Move_gantry_home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreSide',
'planning_failed': 'Retry_3',
'control_failed': 'Retry_3',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_gantry',
'move_group': 'move_group_gantry',
'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:549 y:32
OperatableStateMachine.add(
'Retry_3',
WaitState(wait_time=0.3),
transitions={'done': 'Move_gantry_home'},
autonomy={'done': Autonomy.Off})
# x:745 y:152
OperatableStateMachine.add('PreSide',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DetectPart',
'planning_failed': 'Retry_4',
'control_failed': 'Retry_4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_PreSide',
'move_group': 'move_group_gantry',
'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:773 y:27
OperatableStateMachine.add('Retry_4',
WaitState(wait_time=0.3),
transitions={'done': 'PreSide'},
autonomy={'done': Autonomy.Off})
# x:1111 y:151
OperatableStateMachine.add('PreGrasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputePick',
'planning_failed': 'Retry_5',
'control_failed': 'Retry_5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_PreGrasp',
'move_group': 'move_group_gantry',
'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:1129 y:33
OperatableStateMachine.add('Retry_5',
WaitState(wait_time=0.3),
transitions={'done': 'PreGrasp'},
autonomy={'done': Autonomy.Off})
# x:1308 y:150
OperatableStateMachine.add(
'ComputePick',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Move_to_pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_right_arm',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_right',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:918 y:149
OperatableStateMachine.add(
'DetectPart',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'PreGrasp',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:1552 y:44
OperatableStateMachine.add('Retry_6',
WaitState(wait_time=0.3),
transitions={'done': 'Move_to_pick'},
autonomy={'done': Autonomy.Off})
# x:1501 y:148
OperatableStateMachine.add('Move_to_pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperOn',
'planning_failed': 'Retry_6',
'control_failed': 'GripperOn'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group':
'move_group_right_arm',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1510 y:247
OperatableStateMachine.add('GripperOn',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'Move_right_arm_home_2',
'failed': 'ComputePick',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1517 y:381
OperatableStateMachine.add(
'Move_right_arm_home_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreSide_2',
'planning_failed': 'Retry_7',
'control_failed': 'Retry_7',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_right_arm',
'move_group': 'move_group_right_arm',
'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:1729 y:398
OperatableStateMachine.add(
'Retry_7',
WaitState(wait_time=0.3),
transitions={'done': 'Move_right_arm_home_2'},
autonomy={'done': Autonomy.Off})
# x:1525 y:496
OperatableStateMachine.add('PreSide_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move_gantry_home_2',
'planning_failed': 'Retry_8',
'control_failed': 'Retry_8',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_PreSide',
'move_group': 'move_group_gantry',
'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:1523 y:593
OperatableStateMachine.add('Move_gantry_home_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreDrop_AGV1',
'planning_failed': 'Retry_9',
'control_failed': 'Retry_9',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_gantry',
'move_group': 'move_group_gantry',
'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:1730 y:498
OperatableStateMachine.add('Retry_8',
WaitState(wait_time=0.3),
transitions={'done': 'PreSide_2'},
autonomy={'done': Autonomy.Off})
# x:1551 y:733
OperatableStateMachine.add(
'Retry_9',
WaitState(wait_time=0.3),
transitions={'done': 'Move_gantry_home_2'},
autonomy={'done': Autonomy.Off})
# x:1334 y:727
OperatableStateMachine.add('Retry_10',
WaitState(wait_time=0.3),
transitions={'done': 'PreDrop_AGV1'},
autonomy={'done': Autonomy.Off})
# x:1305 y:592
OperatableStateMachine.add('PreDrop_AGV1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'Retry_10',
'control_failed': 'Retry_10',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_PreDrop',
'move_group': 'move_group_gantry',
'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'
})
return _state_machine
def create(self):
# x:692 y:77, x:764 y:355
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'part_type1', 'part_type2', 'offset1', 'offset2',
'camera_topic1', 'camera_topic2', 'camera_frame1',
'camera_frame2', 'config_name_Gantry_pick1',
'config_name_Gantry_pick2', 'config_name_PrePick1',
'config_name_PrePick2'
])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.part_type1 = ''
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.offset1 = ''
_state_machine.userdata.move_group_G = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic1 = ''
_state_machine.userdata.camera_frame1 = ''
_state_machine.userdata.tool_link_R = 'right_ee_link'
_state_machine.userdata.arm_id1 = 'left'
_state_machine.userdata.config_name_Gantry_pick1 = ''
_state_machine.userdata.null = 0
_state_machine.userdata.config_name_Left_home = 'Left_Home'
_state_machine.userdata.config_name_Right_home = 'Right_Home'
_state_machine.userdata.move_group_L = 'Left_Arm'
_state_machine.userdata.move_group_R = 'Right_Arm'
_state_machine.userdata.config_name_L = 'Links_PreDrop_Stelling'
_state_machine.userdata.config_name_R = 'Rechts_PreDrop_Stelling'
_state_machine.userdata.tool_link_L = 'left_ee_link'
_state_machine.userdata.rotation = 0
_state_machine.userdata.camera_ref_frame = 'world'
_state_machine.userdata.config_name_PrePick1 = ''
_state_machine.userdata.config_name_PreDrop = 'Links_PreDrop_AGV'
_state_machine.userdata.pose = []
_state_machine.userdata.config_name_Gantry_pick2 = ''
_state_machine.userdata.part_type2 = ''
_state_machine.userdata.camera_frame2 = ''
_state_machine.userdata.camera_topic2 = ''
_state_machine.userdata.offset2 = ''
_state_machine.userdata.arm_id2 = 'right'
_state_machine.userdata.config_name_PrePick2 = ''
_state_machine.userdata.config_name_GantrySafe = 'Gantry_Transportband_Home'
_state_machine.userdata.AtShelve = 0
_state_machine.userdata.true = 1
_state_machine.userdata.false = 0
_state_machine.userdata.Lshelve = 'Links_PreDrop_Stelling'
_state_machine.userdata.Rshelve = 'Rechts_PreDrop_Stelling'
_state_machine.userdata.empty = 'empty'
_state_machine.userdata.config_name_Gantry_AtShelve = 'Gantry_Stelling'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:939 y:13
OperatableStateMachine.add('LeftHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'RightHome',
'planning_failed': 'failed',
'control_failed': 'RightHome',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_Left_home',
'move_group': 'move_group_L',
'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:1150 y:13
OperatableStateMachine.add(
'RightHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GripperDisable_2',
'planning_failed': 'failed',
'control_failed': 'GripperDisable_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_Right_home',
'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:1650 y:396
OperatableStateMachine.add('CheckPartPoseBin',
DetectPartCameraAriacState(time_out=2),
transitions={
'continue': 'ComputePick',
'failed': 'failed',
'not_found': 'Retry'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic1',
'camera_frame': 'camera_frame1',
'part': 'part_type1',
'pose': 'pose'
})
# x:1660 y:495
OperatableStateMachine.add(
'ComputePick',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Pick',
'failed': 'Retry'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_L',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_L',
'pose': 'pose',
'offset': 'offset1',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1430 y:397
OperatableStateMachine.add('Retry',
WaitState(wait_time=0.2),
transitions={'done': 'PrePick'},
autonomy={'done': Autonomy.Off})
# x:1673 y:283
OperatableStateMachine.add(
'PrePick',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'CheckPartPoseBin',
'planning_failed': 'failed',
'control_failed': 'CheckPartPoseBin',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_PrePick1',
'move_group': 'move_group_L',
'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:1451 y:763
OperatableStateMachine.add('LeftHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'empty?',
'planning_failed': 'failed',
'control_failed': 'empty?',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_Left_home',
'move_group': 'move_group_L',
'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:1348 y:541
OperatableStateMachine.add('Pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperEnable',
'planning_failed': 'failed',
'control_failed': 'GripperEnable'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_L',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1679 y:758
OperatableStateMachine.add('PrePick_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'LeftHome_2',
'planning_failed': 'failed',
'control_failed': 'LeftHome_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_PrePick1',
'move_group': 'move_group_L',
'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:1657 y:659
OperatableStateMachine.add('GripperEnable',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'PrePick_2',
'failed': 'ComputePick',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id1'})
# x:475 y:757
OperatableStateMachine.add('Pregrasp_G_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PrePick_3',
'planning_failed': 'failed',
'control_failed': 'PrePick_3',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_Gantry_pick2',
'move_group': 'move_group_G',
'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:15 y:701
OperatableStateMachine.add('CheckPartPoseBin_2',
DetectPartCameraAriacState(time_out=2),
transitions={
'continue': 'ComputePick_2',
'failed': 'Retry_2',
'not_found': 'Retry_2'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic2',
'camera_frame': 'camera_frame2',
'part': 'part_type2',
'pose': 'pose2'
})
# x:13 y:536
OperatableStateMachine.add(
'ComputePick_2',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Pick_2',
'failed': 'Retry_2'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_R',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_R',
'pose': 'pose2',
'offset': 'offset2',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:271 y:602
OperatableStateMachine.add('Retry_2',
WaitState(wait_time=0.2),
transitions={'done': 'PrePick_3'},
autonomy={'done': Autonomy.Off})
# x:255 y:742
OperatableStateMachine.add(
'PrePick_3',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'CheckPartPoseBin_2',
'planning_failed': 'failed',
'control_failed': 'CheckPartPoseBin_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_PrePick2',
'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:456
OperatableStateMachine.add('Pick_2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperEnable_2',
'planning_failed': 'failed',
'control_failed': 'GripperEnable_2'
},
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:28 y:367
OperatableStateMachine.add('GripperEnable_2',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'PrePick_2_2',
'failed': 'ComputePick_2',
'invalid_arm_id': 'PrePick_2_2'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id2'})
# x:39 y:257
OperatableStateMachine.add('PrePick_2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'RightHome_2',
'planning_failed': 'failed',
'control_failed': 'RightHome_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_PrePick2',
'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:1568 y:9
OperatableStateMachine.add('GripperDisable',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'ShelveCheck1',
'failed': 'ShelveCheck1',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id1'})
# x:1359 y:10
OperatableStateMachine.add('GripperDisable_2',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'GripperDisable',
'failed': 'GripperDisable',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id2'})
# x:37 y:150
OperatableStateMachine.add(
'RightHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'SavePosShelve2_2',
'planning_failed': 'failed',
'control_failed': 'SavePosShelve2_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_Right_home',
'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:1657 y:92
OperatableStateMachine.add('ShelveCheck1',
EqualState(),
transitions={
'true': 'SetShelve1',
'false': 'Pregrasp_G'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'config_name_PrePick1',
'value_b': 'Lshelve'
})
# x:1441 y:206
OperatableStateMachine.add('SavePosShelve1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Pregrasp_G',
'planning_failed': 'failed',
'control_failed': 'Pregrasp_G',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_GantrySafe',
'move_group': 'move_group_G',
'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:1433 y:102
OperatableStateMachine.add('SetShelve1',
ReplaceState(),
transitions={'done': 'SavePosShelve1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'true',
'result': 'AtShelve'
})
# x:1044 y:755
OperatableStateMachine.add('AtShelve?',
EqualState(),
transitions={
'true': 'ShelveCheck3',
'false': 'ShelveCheck2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'AtShelve',
'value_b': 'true'
})
# x:770 y:765
OperatableStateMachine.add('ShelveCheck3',
EqualState(),
transitions={
'true': 'Pregrasp_G_2',
'false': 'SavePosShelve2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'config_name_PrePick2',
'value_b': 'Rshelve'
})
# x:692 y:623
OperatableStateMachine.add('SavePosShelve2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Pregrasp_G_2',
'planning_failed': 'failed',
'control_failed': 'Pregrasp_G_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_GantrySafe',
'move_group': 'move_group_G',
'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:354 y:107
OperatableStateMachine.add('SavePosShelve2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_GantrySafe',
'move_group': 'move_group_G',
'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:1237 y:753
OperatableStateMachine.add('empty?',
EqualState(),
transitions={
'true': 'SavePosShelve2_2',
'false': 'AtShelve?'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type2',
'value_b': 'empty'
})
# x:1671 y:179
OperatableStateMachine.add('Pregrasp_G',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PrePick',
'planning_failed': 'failed',
'control_failed': 'PrePick',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_Gantry_pick1',
'move_group': 'move_group_G',
'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:870 y:686
OperatableStateMachine.add('ShelveCheck2',
EqualState(),
transitions={
'true': 'SavePosShelve2',
'false': 'Pregrasp_G_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'config_name_PrePick2',
'value_b': 'Rshelve'
})
return _state_machine
def create(self):
# x:737 y:206, x:797 y:409
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'agv_id', 'offset2', 'offset1', 'pose_on_agv1', 'pose_on_agv2',
'part_type2'
])
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.move_group_G = 'Gantry'
_state_machine.userdata.move_group_L = 'Left_Arm'
_state_machine.userdata.move_group_R = 'Right_Arm'
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.offset1 = ''
_state_machine.userdata.offset2 = ''
_state_machine.userdata.config_name_GantryL_AGVL = 'Gantry_AGV_Links_L'
_state_machine.userdata.config_name_GantryL_AGVR = 'Gantry_AGV_Links_R'
_state_machine.userdata.config_name_GantryR_AGVL = 'Gantry_AGV_Rechts_L'
_state_machine.userdata.config_name_GantryR_AGVR = 'Gantry_AGV_Rechts_R'
_state_machine.userdata.config_name_GantryAGVL = ''
_state_machine.userdata.config_name_GantryAGVR = ''
_state_machine.userdata.agv1 = 'agv1'
_state_machine.userdata.agv2 = 'agv2'
_state_machine.userdata.config_name_PlaceAGVL = 'Links_PreDrop_AGV'
_state_machine.userdata.tool_link_R = 'right_ee_link'
_state_machine.userdata.tool_link_L = 'left_ee_link'
_state_machine.userdata.agv_pose = []
_state_machine.userdata.rotation = 0
_state_machine.userdata.pose_on_agv1 = []
_state_machine.userdata.arm_idL = 'left'
_state_machine.userdata.config_name_PlaceAGVR = 'Rechts_PreDrop_AGV'
_state_machine.userdata.arm_idR = 'right'
_state_machine.userdata.pose_on_agv2 = []
_state_machine.userdata.config_name_homeL = 'Left_Home'
_state_machine.userdata.config_name_homeR = 'Right_Home'
_state_machine.userdata.config_name_Gantry_home = 'Gantry_Transportband_Home'
_state_machine.userdata.part_type2 = ''
_state_machine.userdata.empty = 'empty'
_state_machine.userdata.kit_tray = 'kit_tray_1'
_state_machine.userdata.KT1 = 'kit_tray_1'
_state_machine.userdata.KT2 = 'kit_tray_2'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:98 y:59
OperatableStateMachine.add('AGV choice',
EqualState(),
transitions={
'true': 'SetPosL1',
'false': 'SetPosL2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv_id',
'value_b': 'agv2'
})
# x:340 y:29
OperatableStateMachine.add('SetPosL1',
ReplaceState(),
transitions={'done': 'SetPosR1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_GantryR_AGVL',
'result': 'config_name_GantryAGVL'
})
# x:534 y:29
OperatableStateMachine.add('SetPosR1',
ReplaceState(),
transitions={'done': 'MoveL to AGV'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_GantryR_AGVR',
'result': 'config_name_GantryAGVR'
})
# x:340 y:91
OperatableStateMachine.add('SetPosL2',
ReplaceState(),
transitions={'done': 'SetPosR2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_GantryL_AGVL',
'result': 'config_name_GantryAGVL'
})
# x:535 y:91
OperatableStateMachine.add('SetPosR2',
ReplaceState(),
transitions={'done': 'MoveL to AGV'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_GantryL_AGVR',
'result': 'config_name_GantryAGVR'
})
# x:1187 y:33
OperatableStateMachine.add('Move to PrePlace',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'AGV choice_3',
'planning_failed': 'failed',
'control_failed': 'AGV choice_3',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_PlaceAGVL',
'move_group': 'move_group_L',
'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:833 y:56
OperatableStateMachine.add(
'MoveL to AGV',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move to PrePlace',
'planning_failed': 'failed',
'control_failed': 'Move to PrePlace',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_GantryAGVL',
'move_group': 'move_group_G',
'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:1472 y:127
OperatableStateMachine.add('GetAGV1pose',
GetObjectPoseState(
object_frame='kit_tray_1',
ref_frame='world'),
transitions={
'continue': 'pose_on_AGV',
'failed': 'retry'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'agv_pose'})
# x:1604 y:295
OperatableStateMachine.add(
'pose_on_AGV',
ComputeDropPartOffsetGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'MoveToDrop1',
'failed': 'retry'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_L',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_L',
'part_pose': 'pose_on_agv1',
'pose': 'agv_pose',
'offset': 'offset1',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1219 y:277
OperatableStateMachine.add(
'retry',
WaitState(wait_time=0.5),
transitions={'done': 'Move to PrePlace'},
autonomy={'done': Autonomy.Off})
# x:1650 y:386
OperatableStateMachine.add('MoveToDrop1',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'DisableGripper',
'planning_failed': 'failed',
'control_failed': 'DisableGripper'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_L',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1658 y:491
OperatableStateMachine.add('DisableGripper',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'Move to PrePlace_2',
'failed': 'Move to PrePlace_2',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_idL'})
# x:1678 y:599
OperatableStateMachine.add('Move to PrePlace_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move L to home ',
'planning_failed': 'failed',
'control_failed': 'Move L to home ',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_PlaceAGVL',
'move_group': 'move_group_L',
'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:1294 y:690
OperatableStateMachine.add(
'MoveR to AGV',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move to PrePlace_3',
'planning_failed': 'failed',
'control_failed': 'Move to PrePlace_3',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_GantryAGVR',
'move_group': 'move_group_G',
'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:1066 y:683
OperatableStateMachine.add('Move to PrePlace_3',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'pose_on_AGV_2',
'planning_failed': 'failed',
'control_failed': 'pose_on_AGV_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_PlaceAGVR',
'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:491 y:669
OperatableStateMachine.add('GetAGV1pose_2',
GetObjectPoseState(
object_frame='kit_tray_1',
ref_frame='world'),
transitions={
'continue': 'pose_on_AGV_2',
'failed': 'retry_2'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose2'})
# x:14 y:717
OperatableStateMachine.add(
'pose_on_AGV_2',
ComputeDropPartOffsetGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'MoveToDrop2',
'failed': 'retry_2'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_R',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_R',
'part_pose': 'pose_on_agv2',
'pose': 'agv_pose',
'offset': 'offset2',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:586 y:481
OperatableStateMachine.add(
'retry_2',
WaitState(wait_time=0.5),
transitions={'done': 'Move to PrePlace_3'},
autonomy={'done': Autonomy.Off})
# x:43 y:604
OperatableStateMachine.add('MoveToDrop2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'DisableGripper_2',
'planning_failed': 'failed',
'control_failed': 'DisableGripper_2'
},
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:48 y:494
OperatableStateMachine.add('DisableGripper_2',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'Move to PrePlace_2_2',
'failed': 'Move to PrePlace_2_2',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_idR'})
# x:61 y:378
OperatableStateMachine.add('Move to PrePlace_2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move R to home ',
'planning_failed': 'failed',
'control_failed': 'Move R to home ',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_PlaceAGVR',
'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:1684 y:711
OperatableStateMachine.add('Move L to home ',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'CheckSecondPart',
'planning_failed': 'failed',
'control_failed': 'CheckSecondPart',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_homeL',
'move_group': 'move_group_L',
'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:65 y:279
OperatableStateMachine.add('Move R to home ',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GantryHome',
'planning_failed': 'failed',
'control_failed': 'GantryHome',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_homeR',
'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:335 y:205
OperatableStateMachine.add('GantryHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_Gantry_home',
'move_group': 'move_group_G',
'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:1479 y:750
OperatableStateMachine.add('CheckSecondPart',
EqualState(),
transitions={
'true': 'GantryHome',
'false': 'MoveR to AGV'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type2',
'value_b': 'empty'
})
# x:668 y:546
OperatableStateMachine.add('GetAGV2pose_2',
GetObjectPoseState(
object_frame='kit_tray_2',
ref_frame='world'),
transitions={
'continue': 'pose_on_AGV_2',
'failed': 'retry_2'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose2'})
# x:749 y:650
OperatableStateMachine.add('AGV choice_2',
EqualState(),
transitions={
'true': 'GetAGV2pose_2',
'false': 'GetAGV1pose_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv_id',
'value_b': 'agv2'
})
# x:1462 y:28
OperatableStateMachine.add('AGV choice_3',
EqualState(),
transitions={
'true': 'GetAGV2pose',
'false': 'GetAGV1pose'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv_id',
'value_b': 'agv2'
})
# x:1666 y:199
OperatableStateMachine.add('GetAGV2pose',
GetObjectPoseState(
object_frame='kit_tray_2',
ref_frame='world'),
transitions={
'continue': 'pose_on_AGV',
'failed': 'retry'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'agv_pose'})
return _state_machine