def create(self):
# x:30 y:365
_state_machine = OperatableStateMachine(outcomes=['finished'])
_state_machine.userdata.GetalA = 10
_state_machine.userdata.GetalB = 5
_state_machine.userdata.GetalC = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Add',
AddNumericState(),
transitions={'done': 'Result'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'GetalA',
'value_b': 'GetalB',
'result': 'GetalC'
})
# x:211 y:41
OperatableStateMachine.add('Result',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'GetalC'})
return _state_machine
def create(self):
# x:11 y:218
_state_machine = OperatableStateMachine(outcomes=['finished'])
_state_machine.userdata.number_of_interations = 5
_state_machine.userdata.iteration = 0
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:134 y:46
OperatableStateMachine.add('Add',
AddNumericState(),
transitions={'done': 'Compare'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'one_value',
'value_b': 'iteration',
'result': 'iteration'
})
# x:348 y:202
OperatableStateMachine.add('Message',
MessageState(),
transitions={'continue': 'Add'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'iteration'})
# x:345 y:46
OperatableStateMachine.add('Compare',
GreaterNumericState(),
transitions={
'true': 'finished',
'false': 'Message'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'iteration',
'value_b': 'number_of_interations'
})
return _state_machine
def create(self):
# x:926 y:151, x:511 y:227
_state_machine = OperatableStateMachine(
outcomes=['finished', 'fail'],
input_keys=['Shipments', 'NumberOfShipments'])
_state_machine.userdata.Shipments = []
_state_machine.userdata.NumberOfShipments = 0
_state_machine.userdata.Products = []
_state_machine.userdata.NumberOfProducts = 0
_state_machine.userdata.AgvID = ''
_state_machine.userdata.ShipmentIndex = 1
_state_machine.userdata.ShipmentType = ''
_state_machine.userdata.ShipmentIterator = 0
_state_machine.userdata.OneValue = 1
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:43 y:24
OperatableStateMachine.add('GetProducts',
GetProductsFromShipmentState(),
transitions={
'continue': 'ShipmenTypeMessage',
'invalid_index': 'fail'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments': 'Shipments',
'index': 'ShipmentIterator',
'shipment_type': 'ShipmentType',
'agv_id': 'AgvID',
'products': 'Products',
'number_of_products':
'NumberOfProducts'
})
# x:740 y:34
OperatableStateMachine.add(
'IncrementShipmentsIterator',
AddNumericState(),
transitions={'done': 'CompareShepmentsIterator'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'ShipmentIterator',
'value_b': 'OneValue',
'result': 'ShipmentIterator'
})
# x:740 y:120
OperatableStateMachine.add('CompareShepmentsIterator',
EqualState(),
transitions={
'true': 'finished',
'false': 'GetProducts'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'ShipmentIterator',
'value_b': 'NumberOfShipments'
})
# x:564 y:30
OperatableStateMachine.add(
'get_products',
self.use_behavior(get_productsSM, 'get_products'),
transitions={
'finished': 'IncrementShipmentsIterator',
'fail': 'fail'
},
autonomy={
'finished': Autonomy.Inherit,
'fail': Autonomy.Inherit
},
remapping={
'Products': 'Products',
'NumberOfProducts': 'NumberOfProducts'
})
# x:252 y:25
OperatableStateMachine.add(
'ShipmenTypeMessage',
MessageState(),
transitions={'continue': 'AgvIdMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ShipmentType'})
# x:416 y:28
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'get_products'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'AgvID'})
return _state_machine
def create(self):
# x:537 y:364, x:433 y:362, x:430 y:252
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed', 'no_products_belt'],
output_keys=['part_type', 'pose'])
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_6'
_state_machine.userdata.camera_frame = 'logical_camera_6_frame'
_state_machine.userdata.powerOff = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.pose = []
_state_machine.userdata.retries = 0
_state_machine.userdata.plus = 1
_state_machine.userdata.zero = 0
_state_machine.userdata.condition = 15
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:77 y:33
OperatableStateMachine.add(
'ZeroTics',
ReplaceState(),
transitions={'done': 'DetectFirstProduct'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'retries'
})
# x:200 y:131
OperatableStateMachine.add('WaitRetry',
WaitState(wait_time=0.5),
transitions={'done': 'CountCameraTics'},
autonomy={'done': Autonomy.Off})
# x:505 y:34
OperatableStateMachine.add('TurnConveyorOff',
SetConveyorbeltPowerState(),
transitions={
'continue':
'DetectFirstProductRetry',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'powerOff'})
# x:708 y:56
OperatableStateMachine.add(
'DetectFirstProductRetry',
DetectFirstPartCameraAriacState(
part_list=['piston_rod_part_red', 'gasket_part_blue'],
time_out=0.5),
transitions={
'continue': 'finished',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:242 y:249
OperatableStateMachine.add('CountCameraTics',
AddNumericState(),
transitions={'done': 'CheckCondition'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'retries',
'value_b': 'plus',
'result': 'retries'
})
# x:276 y:34
OperatableStateMachine.add(
'DetectFirstProduct',
DetectFirstPartCameraAriacState(
part_list=['piston_rod_part_red', 'gasket_part_blue'],
time_out=0.5),
transitions={
'continue': 'TurnConveyorOff',
'failed': 'WaitRetry',
'not_found': 'WaitRetry'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:336 y:118
OperatableStateMachine.add('CheckCondition',
EqualState(),
transitions={
'true': 'no_products_belt',
'false': 'DetectFirstProduct'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'retries',
'value_b': 'condition'
})
return _state_machine
def create(self):
# x:867 y:12, x:483 y:355
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.order_id = ''
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.products = []
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.increment = 1
_state_machine.userdata.Null = 0
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.old_order_id = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:49 y:117
OperatableStateMachine.add('startassignment',
StartAssignment(),
transitions={'continue': 'get_order'},
autonomy={'continue': Autonomy.Off})
# x:207 y:119
OperatableStateMachine.add('get_order',
GetOrderState(),
transitions={'continue': 'TestLastOrder'},
autonomy={'continue': Autonomy.Off},
remapping={'order_id': 'order_id', 'shipments': 'shipments', 'number_of_shipments': 'number_of_shipments'})
# x:410 y:121
OperatableStateMachine.add('getProductsfromshipment',
GetProductsFromShipmentState(),
transitions={'continue': 'ShipmentID', 'invalid_index': 'failed'},
autonomy={'continue': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'shipments': 'shipments', 'index': 'shipment_index', 'shipment_type': 'shipment_type', 'agv_id': 'agv_id', 'products': 'products', 'number_of_products': 'number_of_products'})
# x:848 y:122
OperatableStateMachine.add('transport_part_form_bin_to_agv_state',
self.use_behavior(transport_part_form_bin_to_agv_stateSM, 'transport_part_form_bin_to_agv_state'),
transitions={'finished': 'IncreaseProductIndex', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'part_type': 'part_type', 'agv_id': 'agv_id', 'part_pose': 'part_pose', 'pose': 'pose'})
# x:649 y:120
OperatableStateMachine.add('GetPartinfo',
GetPartFromProductsState(),
transitions={'continue': 'transport_part_form_bin_to_agv_state', 'invalid_index': 'failed'},
autonomy={'continue': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'products': 'products', 'index': 'product_index', 'type': 'part_type', 'pose': 'part_pose'})
# x:928 y:241
OperatableStateMachine.add('IncreaseProductIndex',
AddNumericState(),
transitions={'done': 'CheckProduct_index'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'product_index', 'value_b': 'increment', 'result': 'product_index'})
# x:934 y:323
OperatableStateMachine.add('CheckProduct_index',
EqualState(),
transitions={'true': 'SETnull', 'false': 'GetPartinfo'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'product_index', 'value_b': 'number_of_products'})
# x:770 y:514
OperatableStateMachine.add('IncreaseShipmentIndex',
AddNumericState(),
transitions={'done': 'Checkshipmentindex'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'shipment_index', 'value_b': 'increment', 'result': 'shipment_index'})
# x:396 y:499
OperatableStateMachine.add('Checkshipmentindex',
EqualState(),
transitions={'true': 'get_order', 'false': 'notify_shipment_ready'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'number_of_shipments', 'value_b': 'shipment_index'})
# x:990 y:530
OperatableStateMachine.add('SETnull',
ReplaceState(),
transitions={'done': 'IncreaseShipmentIndex'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'Null', 'result': 'product_index'})
# x:721 y:355
OperatableStateMachine.add('notify_shipment_ready',
self.use_behavior(notify_shipment_readySM, 'notify_shipment_ready'),
transitions={'finished': 'getProductsfromshipment', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:388 y:13
OperatableStateMachine.add('TestLastOrder',
EqualState(),
transitions={'true': 'finished', 'false': 'RememberOldOrder'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'order_id', 'value_b': 'old_order_id'})
# x:632 y:44
OperatableStateMachine.add('RememberOldOrder',
ReplaceState(),
transitions={'done': 'getProductsfromshipment'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'order_id', 'result': 'old_order_id'})
# x:40 y:265
OperatableStateMachine.add('ShipmentID',
MessageState(),
transitions={'continue': 'GetPartinfo'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'shipment_type'})
return _state_machine
def create(self):
# x:509 y:35, x:204 y:373
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
_state_machine.userdata.order_id = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.old_order_id = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:65 y:95
OperatableStateMachine.add('start',
StartAssignment(),
transitions={'continue': 'getorder'},
autonomy={'continue': Autonomy.Off})
# x:243 y:92
OperatableStateMachine.add(
'getorder',
GetOrderState(),
transitions={'continue': 'compare_last_order'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
# x:465 y:94
OperatableStateMachine.add('get_products_from_shipments',
GetProductsFromShipmentState(),
transitions={
'continue':
'get_part_from_products',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipment_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:689 y:90
OperatableStateMachine.add(
'get_part_from_products',
GetPartFromProductsState(),
transitions={
'continue': 'transport_part_form_bin_to_agv_state',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'product_index',
'type': 'part_type',
'pose': 'part_pose'
})
# x:986 y:193
OperatableStateMachine.add('increment_product_index',
AddNumericState(),
transitions={'done': 'einde_products'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'one_value',
'result': 'product_index'
})
# x:978 y:283
OperatableStateMachine.add('einde_products',
EqualState(),
transitions={
'true': 'reset_product index',
'false': 'get_part_from_products'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:972 y:480
OperatableStateMachine.add('increment_shipment_index',
AddNumericState(),
transitions={'done': 'einde_shipments'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'one_value',
'result': 'shipment_index'
})
# x:971 y:579
OperatableStateMachine.add('einde_shipments',
EqualState(),
transitions={
'true': 'notify_shipment_ready',
'false':
'get_products_from_shipments'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:978 y:384
OperatableStateMachine.add(
'reset_product index',
ReplaceState(),
transitions={'done': 'increment_shipment_index'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'product_index'
})
# x:572 y:362
OperatableStateMachine.add('notify_shipment_ready',
self.use_behavior(
notify_shipment_readySM,
'notify_shipment_ready'),
transitions={
'finished':
'get_products_from_shipments',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:959 y:86
OperatableStateMachine.add(
'transport_part_form_bin_to_agv_state',
self.use_behavior(transport_part_form_bin_to_agv_stateSM,
'transport_part_form_bin_to_agv_state'),
transitions={
'finished': 'increment_product_index',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'agv_id': 'agv_id',
'pose_on_agv': 'part_pose'
})
# x:266 y:2
OperatableStateMachine.add('compare_last_order',
EqualState(),
transitions={
'true': 'finished',
'false': 'rememberoldorder'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'order_id',
'value_b': 'old_order_id'
})
# x:584 y:4
OperatableStateMachine.add(
'rememberoldorder',
ReplaceState(),
transitions={'done': 'get_products_from_shipments'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'order_id',
'result': 'old_order_id'
})
return _state_machine
def create(self):
# x:884 y:573, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group = ''
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.arm_id = 'Left_Arm'
_state_machine.userdata.offset = 0.021
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_stock1'
_state_machine.userdata.camera_frame = 'logical_camera_stock1_frame'
_state_machine.userdata.home = 'Gantry_Home'
_state_machine.userdata.part = 'piston_rod_part_red'
_state_machine.userdata.full_home = 'Full_Home'
_state_machine.userdata.clearance = 0.05
_state_machine.userdata.bin = 'Gantry_Bin_Red'
_state_machine.userdata.bin_pose = []
_state_machine.userdata.binOffset = -1
_state_machine.userdata.one = 1
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:102 y:30
OperatableStateMachine.add('Move_Bin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Get_Object_Place',
'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': 'bin',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:312 y:27
OperatableStateMachine.add('Compute_Place',
ComputeDropPart(),
transitions={
'continue': 'Move_To_Place',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'binOffset': 'binOffset',
'bin_pose': 'bin_pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:698 y:26
OperatableStateMachine.add('Gripper_Off',
GripperControl(enable=False),
transitions={
'continue': 'Add_Clearance',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:842 y:232
OperatableStateMachine.add('Move_Clear',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Move_Home',
'planning_failed': 'Move_Home',
'control_failed': 'Move_Clear'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:844 y:24
OperatableStateMachine.add('Add_Clearance',
AddNumericState(),
transitions={'done': 'Compute_Place_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'clearance',
'value_b': 'offset',
'result': 'offset'
})
# x:846 y:330
OperatableStateMachine.add('Move_Home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'AddPart',
'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': 'full_home',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:482 y:20
OperatableStateMachine.add('Move_To_Place',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_Off',
'planning_failed': 'failed',
'control_failed': 'Gripper_Off'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:68 y:266
OperatableStateMachine.add('Get_Object_Place',
GetObjectPoseState(
object_frame='bin5_frame',
ref_frame='world'),
transitions={
'continue': 'Offset',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'bin_pose'})
# x:854 y:117
OperatableStateMachine.add('Compute_Place_2',
ComputeDropPart(),
transitions={
'continue': 'Move_Clear',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'binOffset': 'binOffset',
'bin_pose': 'bin_pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:220 y:154
OperatableStateMachine.add('Offset',
part_offsetCalc(),
transitions={
'succes': 'Compute_Place',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part',
'part_offset': 'offset'
})
# x:815 y:435
OperatableStateMachine.add('AddPart',
AddNumericState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'binOffset',
'value_b': 'one',
'result': 'binOffset'
})
return _state_machine
def create(self):
# x:1680 y:94, x:415 y:533
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.move_groupR = 'Right_Arm'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.ARM_idMAIN = ''
_state_machine.userdata.move_groupL = 'Left_Arm'
_state_machine.userdata.tool_link_right = 'right_ee_link'
_state_machine.userdata.tool_link_left = 'left_ee_link'
_state_machine.userdata.joint_names = ''
_state_machine.userdata.joint_values = ''
_state_machine.userdata.ConveyorPartoffset = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.arm_idR = 'right_arm'
_state_machine.userdata.arm_idL = 'left_arm'
_state_machine.userdata.Ja_value = 'Ja'
_state_machine.userdata.Nee_value = 'Nee'
_state_machine.userdata.BothArmsFull = 'Nee'
_state_machine.userdata.config_nameLeftPregrasp = 'Left_PreGrasp'
_state_machine.userdata.config_nameRightPregrasp = 'Right_PreGrasp'
_state_machine.userdata.config_nameGantryROT = 'Gantry_SafeHall1'
_state_machine.userdata.move_groupGantry = 'Gantry'
_state_machine.userdata.Round_nrMAIN = 0
_state_machine.userdata.RoundTargetMAIN = 4
_state_machine.userdata.PartTYPE = ''
_state_machine.userdata.RightArmItem = ''
_state_machine.userdata.LeftArmItem = ''
_state_machine.userdata.ONE = 1
_state_machine.userdata.USEarmID = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Startass',
StartAssignment(),
transitions={'continue': 'ADDround'},
autonomy={'continue': Autonomy.Off})
# x:562 y:111
OperatableStateMachine.add('Conveyor_State',
self.use_behavior(
Conveyor_StateSM, 'Conveyor_State'),
transitions={
'finished': 'BeltPickUP',
'failed': 'WaitFailed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'ARMidMAIN': 'ARMidMAIN',
'PartPose': 'PartPose',
'PartOffset': 'PartOffset',
'PartTYPE': 'PartTYPE',
'exactparttype': 'exactparttype'
})
# x:1542 y:89
OperatableStateMachine.add('stopass',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:349 y:41
OperatableStateMachine.add('Armattachedchecker',
self.use_behavior(
ArmattachedcheckerSM,
'Armattachedchecker'),
transitions={
'finished': 'BothFULL?',
'failed': 'WaitFailed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'ARMidMAIN': 'ARMidMAIN',
'BothArmsFull': 'BothArmsFull',
'BothArmsEMPTY': 'BothArmsEMPTY'
})
# x:561 y:45
OperatableStateMachine.add('BothFULL?',
EqualState(),
transitions={
'true': 'ToRotationStation',
'false': 'Conveyor_State'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'BothArmsFull',
'value_b': 'Ja_value'
})
# x:179 y:41
OperatableStateMachine.add('StartUP',
self.use_behavior(StartUPSM, 'StartUP'),
transitions={
'finished': 'Armattachedchecker',
'failed': 'WaitFailed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:560 y:198
OperatableStateMachine.add('BeltPickUP',
self.use_behavior(
BeltPickUPSM, 'BeltPickUP'),
transitions={
'finished': 'WaitNewRound',
'failed': 'WaitFailed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'ARMidMAIN': 'ARMidMAIN',
'PartPose': 'PartPose',
'PartOffset': 'PartOffset',
'exactparttype': 'exactparttype',
'PartTYPE': 'PartTYPE',
'RightArmItem': 'RightArmItem',
'LeftArmItem': 'LeftArmItem'
})
# x:757 y:46
OperatableStateMachine.add(
'ToRotationStation',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GoToPreDrop',
'planning_failed': 'TransfertoDropFailed',
'control_failed': 'TransfertoDropFailed',
'param_error': 'TransfertoDropFailed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_nameGantryROT',
'move_group': 'move_groupGantry',
'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:203 y:204
OperatableStateMachine.add('WaitNewRound',
WaitState(wait_time=0.25),
transitions={'done': 'StartUP'},
autonomy={'done': Autonomy.Off})
# x:376 y:394
OperatableStateMachine.add('WaitFailed',
WaitState(wait_time=2),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:749 y:354
OperatableStateMachine.add('TransfertoDropFailed',
WaitState(wait_time=2),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:934 y:41
OperatableStateMachine.add('GoToPreDrop',
self.use_behavior(
GoToPreDropSM, 'GoToPreDrop'),
transitions={
'finished': 'DropItemsInBIN',
'failed': 'TransfertoDropFailed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'RightArmItem': 'RightArmItem',
'LeftArmItem': 'LeftArmItem',
'GantryLocation': 'GantryLocation',
'USEarmID': 'USEarmID'
})
# x:1323 y:153
OperatableStateMachine.add('RoundCheck',
EqualState(),
transitions={
'true': 'stopass',
'false': 'ADDround'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'Round_nrMAIN',
'value_b': 'RoundTargetMAIN'
})
# x:21 y:348
OperatableStateMachine.add('ADDround',
AddNumericState(),
transitions={'done': 'StartUP'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'Round_nrMAIN',
'value_b': 'ONE',
'result': 'Round_nrMAIN'
})
# x:1116 y:42
OperatableStateMachine.add('DropItemsInBIN',
self.use_behavior(
DropItemsInBINSM, 'DropItemsInBIN'),
transitions={
'Next arm': 'GoToPreDrop',
'failed': 'TransfertoDropFailed',
'BothArmsEmpty': 'RoundCheck',
'BinVol': 'LogBinVol'
},
autonomy={
'Next arm': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'BothArmsEmpty': Autonomy.Inherit,
'BinVol': Autonomy.Inherit
},
remapping={
'USEarmID': 'USEarmID',
'GantryLocation': 'GantryLocation',
'RightArmItem': 'RightArmItem',
'LeftArmItem': 'LeftArmItem'
})
# x:1323 y:46
OperatableStateMachine.add('LogBinVol',
LogState(text="Een van de bins is vol",
severity=Logger.REPORT_HINT),
transitions={'done': 'stopass'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:291 y:423, x:814 y:225
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['Shipments', 'NumberOfShipments'])
_state_machine.userdata.Shipments = []
_state_machine.userdata.NumberOfShipments = 0
_state_machine.userdata.Products = []
_state_machine.userdata.NumberOfProducts = 0
_state_machine.userdata.AgvId = ''
_state_machine.userdata.ShipmentIndex = 0
_state_machine.userdata.ShipmentType = ''
_state_machine.userdata.ShipmentIterator = 0
_state_machine.userdata.OneValue = 1
_state_machine.userdata.agv_id = ''
_state_machine.userdata.agv_ready_state = "ready_to_deliver"
_state_machine.userdata.agv_state = ''
_state_machine.userdata.Zero = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:55 y:39
OperatableStateMachine.add('GetProducts',
GetProductsFromShipmentState(),
transitions={
'continue': 'number of products',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments': 'Shipments',
'index': 'ShipmentIterator',
'shipment_type': 'ShipmentType',
'agv_id': 'agv_id',
'products': 'Products',
'number_of_products':
'NumberOfProducts'
})
# x:600 y:38
OperatableStateMachine.add('Get_Products',
self.use_behavior(
Get_ProductsSM, 'Get_Products'),
transitions={
'finished': 'NotifyShipmentReady',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'Products': 'Products',
'NumberOfProducts':
'NumberOfProducts',
'agv_id': 'agv_id'
})
# x:870 y:36
OperatableStateMachine.add('NotifyShipmentReady',
NotifyShipmentReadyState(),
transitions={
'continue': 'agv_busy',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'agv_id': 'agv_id',
'shipment_type': 'ShipmentType',
'success': 'success',
'message': 'message'
})
# x:1131 y:38
OperatableStateMachine.add('agv_busy',
WaitState(wait_time=8),
transitions={'done': 'Shipment nr.'},
autonomy={'done': Autonomy.Off})
# x:1145 y:384
OperatableStateMachine.add('ShipmentIterator',
AddNumericState(),
transitions={'done': 'Shipment nr._2'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'ShipmentIterator',
'value_b': 'OneValue',
'result': 'ShipmentIterator'
})
# x:680 y:378
OperatableStateMachine.add('CompareIterator',
EqualState(),
transitions={
'true': 'ResetIterator',
'false': 'GetProducts'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'NumberOfShipments',
'value_b': 'ShipmentIterator'
})
# x:399 y:24
OperatableStateMachine.add(
'number of products',
MessageState(),
transitions={'continue': 'Get_Products'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'NumberOfProducts'})
# x:1171 y:283
OperatableStateMachine.add(
'Shipment nr.',
MessageState(),
transitions={'continue': 'ShipmentIterator'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ShipmentIterator'})
# x:915 y:383
OperatableStateMachine.add(
'Shipment nr._2',
MessageState(),
transitions={'continue': 'CompareIterator'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ShipmentIterator'})
# x:1346 y:166
OperatableStateMachine.add('AGVstatus',
EqualState(),
transitions={
'true': 'Shipment nr.',
'false': 'agv_busy'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv_state',
'value_b': 'agv_ready_state'
})
# x:1378 y:49
OperatableStateMachine.add('getAGVstatus',
GetAgvStatusState(),
transitions={
'continue': 'AGVstatus',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'agv_id': 'agv_id',
'agv_state': 'agv_state'
})
# x:436 y:410
OperatableStateMachine.add('ResetIterator',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Zero',
'result': 'ShipmentIterator'
})
return _state_machine
def create(self):
# x:27 y:469, x:242 y:360
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
output_keys=['part_type', 'pose_on_agv'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.config_name = ''
_state_machine.userdata.move_group_g = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.tool_link = ''
_state_machine.userdata.pose_on_agv = ''
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group = ''
_state_machine.userdata.arm_id = ''
_state_machine.userdata.part_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.one = 1
_state_machine.userdata.product_itt = 0
_state_machine.userdata.shipments_index = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:64 y:31
OperatableStateMachine.add(
'Start_assignment',
StartAssignment(),
transitions={'continue': 'move_home_belt_nvd'},
autonomy={'continue': Autonomy.Off})
# x:431 y:28
OperatableStateMachine.add(
'get_orders',
GetOrderState(),
transitions={'continue': 'Get shipments'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
# x:615 y:25
OperatableStateMachine.add('Get shipments',
GetProductsFromShipmentState(),
transitions={
'continue': 'get part',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipments_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:833 y:35
OperatableStateMachine.add('get part',
GetPartFromProductsState(),
transitions={
'continue': 'message part main prg',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'part_index',
'type': 'part_type',
'pose': 'pose_on_agv'
})
# x:1263 y:258
OperatableStateMachine.add('Order_picking',
self.use_behavior(
Order_pickingSM, 'Order_picking'),
transitions={
'finished': 'add_itterator order',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'pose_on_agv': 'pose_on_agv',
'pose_on_agv_r': 'pose_on_agv_r',
'pose_on_agv_l': 'pose_on_agv_l'
})
# x:1148 y:353
OperatableStateMachine.add(
'add_itterator order',
AddNumericState(),
transitions={'done': 'part index message'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'one',
'value_b': 'part_index',
'result': 'part_index'
})
# x:914 y:501
OperatableStateMachine.add('Check_product_amount',
EqualState(),
transitions={
'true': 'move_to_agv',
'false': 'gripper check'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_products',
'value_b': 'part_index'
})
# x:1053 y:12
OperatableStateMachine.add(
'message part main prg',
MessageState(),
transitions={'continue': 'choice point'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:1154 y:437
OperatableStateMachine.add(
'part index message',
MessageState(),
transitions={'continue': 'Check_product_amount'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_index'})
# x:1277 y:24
OperatableStateMachine.add('choice point',
choiceoflocation(time_out=0.5),
transitions={
'bingr0': 'Order_picking',
'bingr1': 'Order_picking',
'bingr2': 'Order_picking',
'bingr3': 'Order_picking',
'bingr4':
'shelve pick_behavior_nvd',
'bingr5':
'shelve pick_behavior_nvd',
'failed': 'failed'
},
autonomy={
'bingr0': Autonomy.Off,
'bingr1': Autonomy.Off,
'bingr2': Autonomy.Off,
'bingr3': Autonomy.Off,
'bingr4': Autonomy.Off,
'bingr5': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'part_type': 'part_type'})
# x:999 y:213
OperatableStateMachine.add('shelve pick_behavior_nvd',
self.use_behavior(
shelvepick_behavior_nvdSM,
'shelve pick_behavior_nvd'),
transitions={
'finished': 'add_itterator order',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'pose_on_agv': 'pose_on_agv',
'part_type': 'part_type',
'pose_on_agv_l': 'pose_on_agv_l',
'pose_on_agv_r': 'pose_on_agv_r'
})
# x:235 y:9
OperatableStateMachine.add('move_home_belt_nvd',
self.use_behavior(
move_home_belt_nvdSM,
'move_home_belt_nvd'),
transitions={
'finished': 'get_orders',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:896 y:411
OperatableStateMachine.add('gripper check',
GripperActiveCheck(),
transitions={
'Left': 'Get shipments',
'Right': 'Get shipments',
'failed': 'failed',
'Full': 'move_to_agv'
},
autonomy={
'Left': Autonomy.Off,
'Right': Autonomy.Off,
'failed': Autonomy.Off,
'Full': Autonomy.Off
},
remapping={
'arm_id': 'arm_id',
'tool_link': 'tool_link',
'move_group': 'move_group'
})
# x:635 y:505
OperatableStateMachine.add('move_to_agv',
self.use_behavior(
move_to_agvSM, 'move_to_agv'),
transitions={
'finished':
'Check_product_amount_2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'agv_id': 'agv_id',
'pose_on_agv_l': 'pose_on_agv_l',
'pose_on_agv_r': 'pose_on_agv_r'
})
# x:438 y:593
OperatableStateMachine.add('Check_product_amount_2',
EqualState(),
transitions={
'true': 'finished',
'false': 'Get shipments'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_products',
'value_b': 'part_index'
})
return _state_machine
def create(self):
# x:587 y:648, x:611 y:249
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['Products', 'NumberOfProducts', 'agv_id'])
_state_machine.userdata.ProductIterator = 0
_state_machine.userdata.OneValue = 1
_state_machine.userdata.ProductType = ''
_state_machine.userdata.ProductPose = []
_state_machine.userdata.Products = []
_state_machine.userdata.NumberOfProducts = 0
_state_machine.userdata.MaterialsLocationList = []
_state_machine.userdata.MaterialLocation = ''
_state_machine.userdata.MaterialLocationIndex = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.part_type1 = 'empty'
_state_machine.userdata.part_type2 = 'empty'
_state_machine.userdata.empty = 'empty'
_state_machine.userdata.done = 'false'
_state_machine.userdata.true = 'true'
_state_machine.userdata.false = 'false'
_state_machine.userdata.pose_on_agv1 = []
_state_machine.userdata.pose_on_agv2 = []
_state_machine.userdata.Zero = 0
_state_machine.userdata.Minus = -1
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:84 y:253
OperatableStateMachine.add('SetCountlimit',
AddNumericState(),
transitions={'done': 'GetProduct'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'NumberOfProducts',
'value_b': 'Minus',
'result': 'NumberOfProducts'
})
# x:892 y:34
OperatableStateMachine.add('GetMaterialLocation',
GetItemFromListState(),
transitions={
'done': 'product iterator_2',
'invalid_index': 'failed'
},
autonomy={
'done': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'list': 'MaterialsLocationList',
'index': 'MaterialLocationIndex',
'item': 'MaterialLocation'
})
# x:1161 y:108
OperatableStateMachine.add('Orderverwerking',
self.use_behavior(
OrderverwerkingSM,
'Orderverwerking'),
transitions={
'finished': 'ResetPart1',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type1': 'part_type1',
'part_type2': 'part_type2',
'pose_on_agv1': 'pose_on_agv1',
'agv_id': 'agv_id',
'pose_on_agv2': 'pose_on_agv2'
})
# x:399 y:673
OperatableStateMachine.add('ProductITerator',
AddNumericState(),
transitions={'done': 'GetProduct'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'ProductIterator',
'value_b': 'OneValue',
'result': 'ProductIterator'
})
# x:672 y:673
OperatableStateMachine.add('CompareIterator',
EqualState(),
transitions={
'true': 'check arm',
'false': 'ProductITerator'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'NumberOfProducts',
'value_b': 'ProductIterator'
})
# x:585 y:37
OperatableStateMachine.add(
'GetMaterialsLocation',
GetMaterialLocationsState(),
transitions={'continue': 'GetMaterialLocation'},
autonomy={'continue': Autonomy.Off},
remapping={
'part': 'ProductType',
'material_locations': 'MaterialsLocationList'
})
# x:1391 y:708
OperatableStateMachine.add('SetPart1',
ReplaceState(),
transitions={'done': 'CompareIterator'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'ProductType',
'result': 'part_type1'
})
# x:1327 y:31
OperatableStateMachine.add('set arm1',
EqualState(),
transitions={
'true': 'SetPose1',
'false': 'set arm2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type1',
'value_b': 'empty'
})
# x:671 y:461
OperatableStateMachine.add('SetPart2',
ReplaceState(),
transitions={'done': 'GetProduct'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'ProductType',
'result': 'part_type2'
})
# x:1289 y:268
OperatableStateMachine.add('set arm2',
EqualState(),
transitions={
'true': 'SetPose2',
'false': 'Orderverwerking'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type2',
'value_b': 'empty'
})
# x:1012 y:262
OperatableStateMachine.add('ResetPart1',
ReplaceState(),
transitions={'done': 'ResetPart2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'empty',
'result': 'part_type1'
})
# x:1010 y:360
OperatableStateMachine.add('ResetPart2',
ReplaceState(),
transitions={'done': 'ResetPose1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'empty',
'result': 'part_type2'
})
# x:715 y:573
OperatableStateMachine.add('check arm',
EqualState(),
transitions={
'true': 'ResetIterator',
'false': 'done'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type1',
'value_b': 'empty'
})
# x:509 y:361
OperatableStateMachine.add('done?',
EqualState(),
transitions={
'true': 'ResetIterator',
'false': 'ProductITerator'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'done',
'value_b': 'true'
})
# x:1155 y:596
OperatableStateMachine.add('done',
ReplaceState(),
transitions={'done': 'Orderverwerking'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'true',
'result': 'done'
})
# x:979 y:471
OperatableStateMachine.add('SetPose2',
ReplaceState(),
transitions={'done': 'SetPart2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'pose_on_agv',
'result': 'pose_on_agv2'
})
# x:1454 y:327
OperatableStateMachine.add('SetPose1',
ReplaceState(),
transitions={'done': 'SetPart1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'pose_on_agv',
'result': 'pose_on_agv1'
})
# x:748 y:373
OperatableStateMachine.add('ResetPose1',
ReplaceState(),
transitions={'done': 'ResetPose2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Zero',
'result': 'pose_on_agv1'
})
# x:754 y:269
OperatableStateMachine.add('ResetPose2',
ReplaceState(),
transitions={'done': 'done?'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Zero',
'result': 'pose_on_agv2'
})
# x:283 y:59
OperatableStateMachine.add(
'product iterator',
MessageState(),
transitions={'continue': 'GetMaterialsLocation'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ProductIterator'})
# x:1134 y:23
OperatableStateMachine.add(
'product iterator_2',
MessageState(),
transitions={'continue': 'set arm1'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ProductIterator'})
# x:138 y:480
OperatableStateMachine.add('GetProduct',
GetPartFromProductsState(),
transitions={
'continue': 'product iterator',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'Products',
'index': 'ProductIterator',
'type': 'ProductType',
'pose': 'pose_on_agv'
})
# x:502 y:551
OperatableStateMachine.add('ResetIterator',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Zero',
'result': 'ProductIterator'
})
return _state_machine
def create(self):
# x:719 y:341, x:826 y:25
_state_machine = OperatableStateMachine(
outcomes=['finished', 'fail'],
input_keys=['Products', 'NumberOfProducts'])
_state_machine.userdata.ProductIterator = 0
_state_machine.userdata.OneValue = 1
_state_machine.userdata.ProductType = ''
_state_machine.userdata.ProductPose = []
_state_machine.userdata.Products = []
_state_machine.userdata.NumberOfProducts = 0
_state_machine.userdata.MaterialsLocationList = []
_state_machine.userdata.MaterialLocation = ''
_state_machine.userdata.MaterailLocationIndex = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:356 y:121
OperatableStateMachine.add('GetProduct',
GetPartFromProductsState(),
transitions={
'continue': 'ProductTypeMessage',
'invalid_index': 'fail'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'Products',
'index': 'ProductIterator',
'type': 'ProductType',
'pose': 'ProductPose'
})
# x:817 y:258
OperatableStateMachine.add(
'IncrementProductIterator',
AddNumericState(),
transitions={'done': 'CompareProductIterator'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'ProductIterator',
'value_b': 'OneValue',
'result': 'ProductIterator'
})
# x:625 y:256
OperatableStateMachine.add('CompareProductIterator',
EqualState(),
transitions={
'true': 'finished',
'false': 'GetProduct'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'ProductIterator',
'value_b': 'NumberOfProducts'
})
# x:569 y:121
OperatableStateMachine.add(
'ProductTypeMessage',
MessageState(),
transitions={'continue': 'ProductPoseMassage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ProductType'})
# x:728 y:120
OperatableStateMachine.add(
'ProductPoseMassage',
MessageState(),
transitions={'continue': 'GetMaterialsLocation'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ProductPose'})
# x:877 y:120
OperatableStateMachine.add(
'GetMaterialsLocation',
GetMaterialLocationsState(),
transitions={'continue': 'MaterialsLocationListMessage'},
autonomy={'continue': Autonomy.Off},
remapping={
'part': 'ProductType',
'material_locations': 'MaterialsLocationList'
})
# x:1046 y:119
OperatableStateMachine.add(
'MaterialsLocationListMessage',
MessageState(),
transitions={'continue': 'GerMaterailLocation'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'MaterialsLocationList'})
# x:1226 y:120
OperatableStateMachine.add('GerMaterailLocation',
GetItemFromListState(),
transitions={
'done': 'MaterailLocationMessage',
'invalid_index': 'fail'
},
autonomy={
'done': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'list': 'MaterialsLocationList',
'index': 'MaterailLocationIndex',
'item': 'MaterialLocation'
})
# x:1406 y:124
OperatableStateMachine.add(
'MaterailLocationMessage',
MessageState(),
transitions={'continue': 'IncrementProductIterator'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'MaterialLocation'})
return _state_machine
def create(self):
# x:80 y:338, x:691 y:334
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['part_type_right', 'part_type_left'])
_state_machine.userdata.config_name_right_bin1 = 'rightArmBin1'
_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.joint_values = []
_state_machine.userdata.joint_names = ''
_state_machine.userdata.part_type_right = ''
_state_machine.userdata.part_type_left = ''
_state_machine.userdata.config_name_right_bin2 = 'rightArmBin2'
_state_machine.userdata.move_group_right_arm = 'Right_Arm'
_state_machine.userdata.move_group_left_arm = 'Left_Arm'
_state_machine.userdata.tool_link_right = 'right_ee_link'
_state_machine.userdata.bin1_offset = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.poseBin1 = []
_state_machine.userdata.tool_link_left = 'left_ee_link'
_state_machine.userdata.arm1 = 'Right_Arm'
_state_machine.userdata.arm2 = 'Left_Arm'
_state_machine.userdata.config_name_right_home = 'Right_Home'
_state_machine.userdata.config_name_left_home = 'Left_Home'
_state_machine.userdata.config_name_left_bin1 = 'leftArmBin1'
_state_machine.userdata.config_name_left_bin2 = 'leftArmBin2'
_state_machine.userdata.poseBin2 = []
_state_machine.userdata.iterator = 0.17
_state_machine.userdata.offset = 0
_state_machine.userdata.bin2_offset = 0
_state_machine.userdata.config_name_right_left = 'gantryPosPlaceRightLeft'
_state_machine.userdata.config_name_left_right = 'gantryPosPlaceLeftRight'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:86 y:86
OperatableStateMachine.add(
'inspect_arm_part_type_right',
self.use_behavior(inspect_arm_part_type_rightSM,
'inspect_arm_part_type_right'),
transitions={
'gasket_part': 'MoveToGasketBin',
'no_part': 'inspect_arm_part_type_left',
'piston_rod_part': 'MoveToPistonBin'
},
autonomy={
'gasket_part': Autonomy.Inherit,
'no_part': Autonomy.Inherit,
'piston_rod_part': Autonomy.Inherit
},
remapping={
'part_type_right': 'part_type_right',
'part_type_left': 'part_type_left'
})
# x:347 y:38
OperatableStateMachine.add('MoveToPistonBin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetPoseBin1',
'planning_failed': 'failed',
'control_failed': 'GetPoseBin1',
'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_left',
'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:348 y:161
OperatableStateMachine.add('MoveToGasketBin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetPoseBin2',
'planning_failed': 'failed',
'control_failed': 'GetPoseBin2',
'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_right',
'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:675 y:36
OperatableStateMachine.add(
'ComputeDropPartPiston',
ComputeDropAriacState(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': 'MoveToPlacePiston',
'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': 'poseBin1',
'offset': 'bin1_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:675 y:161
OperatableStateMachine.add(
'ComputeDropPartGasket',
ComputeDropAriacState(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': 'MoveToPlaceGasket',
'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': 'poseBin2',
'offset': 'bin2_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:860 y:37
OperatableStateMachine.add('MoveToPlacePiston',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'DisableGripperPiston',
'planning_failed':
'ComputeDropPartPiston',
'control_failed':
'DisableGripperPiston'
},
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:869 y:158
OperatableStateMachine.add('MoveToPlaceGasket',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'DisableGripperGasket',
'planning_failed':
'ComputeDropPartGasket',
'control_failed':
'DisableGripperGasket'
},
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:1080 y:38
OperatableStateMachine.add('DisableGripperPiston',
GripperControl(enable=False),
transitions={
'continue': 'MoveRightHome',
'failed': 'failed',
'invalid_id': 'MoveToPlacePiston'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm1'})
# x:1086 y:154
OperatableStateMachine.add('DisableGripperGasket',
GripperControl(enable=False),
transitions={
'continue': 'MoveRightHome_2',
'failed': 'failed',
'invalid_id': 'MoveToPlaceGasket'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm1'})
# x:1243 y:36
OperatableStateMachine.add(
'MoveRightHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'AddOffsetBin1',
'planning_failed': 'failed',
'control_failed': 'AddOffsetBin1',
'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_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:1251 y:150
OperatableStateMachine.add(
'MoveRightHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'AddOffsetBin2',
'planning_failed': 'failed',
'control_failed': 'AddOffsetBin2',
'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_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:1405 y:510
OperatableStateMachine.add(
'inspect_arm_part_type_left',
self.use_behavior(inspect_arm_part_type_leftSM,
'inspect_arm_part_type_left'),
transitions={
'gasket_part': 'MoveToGasketBinLeft',
'no_part': 'finished',
'piston_rod_part': 'MoveToPistonBinLeft'
},
autonomy={
'gasket_part': Autonomy.Inherit,
'no_part': Autonomy.Inherit,
'piston_rod_part': Autonomy.Inherit
},
remapping={
'part_type_right': 'part_type_right',
'part_type_left': 'part_type_left'
})
# x:1188 y:444
OperatableStateMachine.add('MoveToPistonBinLeft',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetPoseBin1Left',
'planning_failed': 'failed',
'control_failed': 'GetPoseBin1Left',
'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_right',
'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:1197 y:594
OperatableStateMachine.add('MoveToGasketBinLeft',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetPoseBin2_2',
'planning_failed': 'failed',
'control_failed': 'GetPoseBin2_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_left',
'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:792 y:449
OperatableStateMachine.add(
'ComputeDropPartPistonLeft',
ComputeDropAriacState(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': 'MoveToPlacePistonLeft',
'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': 'poseBin1',
'offset': 'bin1_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:810 y:591
OperatableStateMachine.add(
'ComputeDropPartGasketLeft',
ComputeDropAriacState(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': 'MoveToPlaceGasketLeft',
'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': 'poseBin2',
'offset': 'bin2_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:585 y:448
OperatableStateMachine.add('MoveToPlacePistonLeft',
MoveitToJointsDynAriacState(),
transitions={
'reached':
'DisableGripperPistonLeft',
'planning_failed':
'ComputeDropPartPistonLeft',
'control_failed':
'DisableGripperPistonLeft'
},
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:581 y:590
OperatableStateMachine.add('MoveToPlaceGasketLeft',
MoveitToJointsDynAriacState(),
transitions={
'reached':
'DisableGripperGasketLeft',
'planning_failed':
'ComputeDropPartGasketLeft',
'control_failed':
'DisableGripperGasketLeft'
},
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:393 y:446
OperatableStateMachine.add('DisableGripperPistonLeft',
GripperControl(enable=False),
transitions={
'continue': 'MoveLeftHome',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm2'})
# x:390 y:589
OperatableStateMachine.add('DisableGripperGasketLeft',
GripperControl(enable=False),
transitions={
'continue': 'MoveLeftHome_2',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm2'})
# x:225 y:590
OperatableStateMachine.add('MoveLeftHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'AddOffsetBin2_2',
'planning_failed': 'failed',
'control_failed': 'AddOffsetBin2_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_home',
'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:219 y:444
OperatableStateMachine.add('MoveLeftHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'AddOffsetBin1_2',
'planning_failed': 'failed',
'control_failed': 'AddOffsetBin1_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_home',
'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:512 y:36
OperatableStateMachine.add('GetPoseBin1',
GetObjectPoseState(
object_frame='bin1_frame',
ref_frame='world'),
transitions={
'continue': 'ComputeDropPartPiston',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'poseBin1'})
# x:1407 y:36
OperatableStateMachine.add(
'AddOffsetBin1',
AddNumericState(),
transitions={'done': 'inspect_arm_part_type_left'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'bin1_offset',
'value_b': 'iterator',
'result': 'bin1_offset'
})
# x:516 y:160
OperatableStateMachine.add('GetPoseBin2',
GetObjectPoseState(
object_frame='bin2_frame',
ref_frame='world'),
transitions={
'continue': 'ComputeDropPartGasket',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'poseBin2'})
# x:1412 y:148
OperatableStateMachine.add(
'AddOffsetBin2',
AddNumericState(),
transitions={'done': 'inspect_arm_part_type_left'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'bin2_offset',
'value_b': 'iterator',
'result': 'bin2_offset'
})
# x:1010 y:446
OperatableStateMachine.add(
'GetPoseBin1Left',
GetObjectPoseState(object_frame='bin1_frame',
ref_frame='world'),
transitions={
'continue': 'ComputeDropPartPistonLeft',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'poseBin1'})
# x:1011 y:591
OperatableStateMachine.add(
'GetPoseBin2_2',
GetObjectPoseState(object_frame='bin2_frame',
ref_frame='world'),
transitions={
'continue': 'ComputeDropPartGasketLeft',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'poseBin2'})
# x:34 y:439
OperatableStateMachine.add('AddOffsetBin1_2',
AddNumericState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'bin1_offset',
'value_b': 'iterator',
'result': 'bin1_offset'
})
# x:39 y:583
OperatableStateMachine.add('AddOffsetBin2_2',
AddNumericState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'bin2_offset',
'value_b': 'iterator',
'result': 'bin2_offset'
})
return _state_machine
def create(self):
# x:1249 y:486, x:614 y:299, x:1274 y:293, x:1759 y:363
_state_machine = OperatableStateMachine(outcomes=['Next arm', 'failed', 'BothArmsEmpty', 'BinVol'], input_keys=['USEarmID', 'GantryLocation', 'RightArmItem', 'LeftArmItem'], output_keys=['RightArmItem', 'LeftArmItem'])
_state_machine.userdata.arm_idL = 'left_arm'
_state_machine.userdata.arm_idR = 'right_arm'
_state_machine.userdata.USEarmID = ''
_state_machine.userdata.move_groupR = 'Right_Arm'
_state_machine.userdata.move_groupL = 'Left_Arm'
_state_machine.userdata.tool_linkL = 'left_ee_link'
_state_machine.userdata.tool_linkR = 'right_ee_link'
_state_machine.userdata.DropPose = []
_state_machine.userdata.offsetXPiston = 0
_state_machine.userdata.offsetPiston = 0
_state_machine.userdata.offsetYPiston = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.PistonOffset = 0.22
_state_machine.userdata.GasketOffset = 0.15
_state_machine.userdata.offsetGasket = 0
_state_machine.userdata.offsetXGasket = -0.1
_state_machine.userdata.offsetYGasket = 0
_state_machine.userdata.GasketLocation = 'GasketLoc'
_state_machine.userdata.PistonLocation = 'PistonLoc'
_state_machine.userdata.GantryLocation = ''
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.Niks = 'Niks'
_state_machine.userdata.RightArmItem = ''
_state_machine.userdata.LeftArmItem = ''
_state_machine.userdata.SideXoffset = 0
_state_machine.userdata.SideYoffset = 0
_state_machine.userdata.SideZoffset = 0
_state_machine.userdata.GasketRow = 0
_state_machine.userdata.PistonRow = 0
_state_machine.userdata.RowTarget = 4
_state_machine.userdata.GasketRound = 0
_state_machine.userdata.PistonRound = 0
_state_machine.userdata.RoundTarget = 12
_state_machine.userdata.Zero_Value = 0
_state_machine.userdata.One_value = 1
_state_machine.userdata.move_groupG = 'Gantry'
_state_machine.userdata.config_nameSH3 = 'Gantry_SafeHall3'
_state_machine.userdata.robot_name = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:47 y:31
OperatableStateMachine.add('GasketLocation',
EqualState(),
transitions={'true': 'GetGasketBinPosition', 'false': 'PistonLocation'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'GasketLocation', 'value_b': 'GantryLocation'})
# x:216 y:33
OperatableStateMachine.add('GetGasketBinPosition',
GetObjectPoseState(object_frame='bin12_frame', ref_frame='world'),
transitions={'continue': 'SetGasketBinXOffset', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'pose': 'DropPose'})
# x:208 y:544
OperatableStateMachine.add('GetPistonBinPosition',
GetObjectPoseState(object_frame='bin1_frame', ref_frame='world'),
transitions={'continue': 'SetPistonBinYOffset', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'pose': 'DropPose'})
# x:1363 y:346
OperatableStateMachine.add('checkLeftArmAttached',
CheckGripperattached(),
transitions={'True': 'Next arm', 'False': 'BothArmsEmpty', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_idL'})
# x:1028 y:38
OperatableStateMachine.add('GripperUITRechts',
VacuumGripperControlState(enable=False),
transitions={'continue': 'ResetRightArmItem', 'failed': 'GoToDropRight', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_idR'})
# x:848 y:38
OperatableStateMachine.add('GoToDropRight',
MoveitToJointsDynAriacState(),
transitions={'reached': 'GripperUITRechts', 'planning_failed': 'GripperUITRechts', 'control_failed': 'GripperUITRechts'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_groupR', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1563 y:393
OperatableStateMachine.add('StartUP',
self.use_behavior(StartUPSM, 'StartUP'),
transitions={'finished': 'checkRightArmAttached', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:1203 y:37
OperatableStateMachine.add('ResetRightArmItem',
ReplaceState(),
transitions={'done': 'AddX2offsetGasket'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'Niks', 'result': 'RightArmItem'})
# x:1029 y:100
OperatableStateMachine.add('GripperUITLinks',
VacuumGripperControlState(enable=False),
transitions={'continue': 'ResetLeftArmItem', 'failed': 'GoToDropLeft', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_idL'})
# x:44 y:542
OperatableStateMachine.add('PistonLocation',
EqualState(),
transitions={'true': 'GetPistonBinPosition', 'false': 'failed'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'PistonLocation', 'value_b': 'GantryLocation'})
# x:848 y:98
OperatableStateMachine.add('GoToDropLeft',
MoveitToJointsDynAriacState(),
transitions={'reached': 'GripperUITLinks', 'planning_failed': 'GripperUITLinks', 'control_failed': 'GripperUITLinks'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_groupL', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1203 y:100
OperatableStateMachine.add('ResetLeftArmItem',
ReplaceState(),
transitions={'done': 'AddX2offsetGasket'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'Niks', 'result': 'LeftArmItem'})
# x:1359 y:423
OperatableStateMachine.add('checkRightArmAttached',
CheckGripperattached(),
transitions={'True': 'Next arm', 'False': 'checkLeftArmAttached', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_idR'})
# x:708 y:35
OperatableStateMachine.add('ComputeRightDrop',
ComputeBeltDrop(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': 'GoToDropRight', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'move_group': 'move_groupR', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_linkR', 'pose': 'DropPose', 'offset': 'SideZoffset', 'SideYoffset': 'SideYoffset', 'SideXoffset': 'SideXoffset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:706 y:99
OperatableStateMachine.add('ComputeLeftDrop',
ComputeBeltDrop(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': 'GoToDropLeft', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'move_group': 'move_groupL', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_linkL', 'pose': 'DropPose', 'offset': 'SideZoffset', 'SideYoffset': 'SideYoffset', 'SideXoffset': 'SideXoffset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:365 y:34
OperatableStateMachine.add('SetGasketBinXOffset',
ReplaceState(),
transitions={'done': 'SetGasketBinYOffset'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'offsetXGasket', 'result': 'SideXoffset'})
# x:362 y:99
OperatableStateMachine.add('SetGasketBinYOffset',
ReplaceState(),
transitions={'done': 'RechterARM?2'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'offsetYGasket', 'result': 'SideYoffset'})
# x:350 y:606
OperatableStateMachine.add('SetPistonBinXOffset',
ReplaceState(),
transitions={'done': 'RechterARM?'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'offsetXPiston', 'result': 'SideXoffset'})
# x:352 y:544
OperatableStateMachine.add('SetPistonBinYOffset',
ReplaceState(),
transitions={'done': 'SetPistonBinXOffset'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'offsetYPiston', 'result': 'SideYoffset'})
# x:1378 y:35
OperatableStateMachine.add('AddX2offsetGasket',
AddNumericState(),
transitions={'done': 'CheckGasketRows'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'offsetXGasket', 'value_b': 'GasketOffset', 'result': 'offsetXGasket'})
# x:1378 y:98
OperatableStateMachine.add('CheckGasketRows',
EqualState(),
transitions={'true': 'ResetGasketRow', 'false': 'AddGasketRound'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'GasketRound', 'value_b': 'RowTarget'})
# x:1376 y:158
OperatableStateMachine.add('ResetGasketRow',
ReplaceState(),
transitions={'done': 'AddYoffsetGasket'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'Zero_Value', 'result': 'offsetXGasket'})
# x:1373 y:220
OperatableStateMachine.add('AddYoffsetGasket',
AddNumericState(),
transitions={'done': 'AddGasketRound'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'GasketOffset', 'value_b': 'offsetYGasket', 'result': 'offsetYGasket'})
# x:1551 y:37
OperatableStateMachine.add('AddGasketRound',
AddNumericState(),
transitions={'done': 'CheckGasketRound'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'One_value', 'value_b': 'GasketRound', 'result': 'GasketRound'})
# x:1551 y:102
OperatableStateMachine.add('CheckGasketRound',
EqualState(),
transitions={'true': 'ResetGasketRound', 'false': 'BackToSH3'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'GasketRound', 'value_b': 'RoundTarget'})
# x:1717 y:103
OperatableStateMachine.add('ResetGasketRound',
ReplaceState(),
transitions={'done': 'BinVol'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'Zero_Value', 'result': 'offsetYGasket'})
# x:534 y:33
OperatableStateMachine.add('RechterARM?2',
EqualState(),
transitions={'true': 'ComputeRightDrop', 'false': 'ComputeLeftDrop'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'USEarmID', 'value_b': 'arm_idR'})
# x:518 y:544
OperatableStateMachine.add('RechterARM?',
EqualState(),
transitions={'true': 'ComputeRightDrop_2', 'false': 'ComputeLeftDrop_2'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'USEarmID', 'value_b': 'arm_idR'})
# x:689 y:544
OperatableStateMachine.add('ComputeRightDrop_2',
ComputeBeltDrop(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': 'GoToDropRight_2', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'move_group': 'move_groupR', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_linkR', 'pose': 'DropPose', 'offset': 'SideZoffset', 'SideYoffset': 'SideYoffset', 'SideXoffset': 'SideXoffset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:688 y:609
OperatableStateMachine.add('ComputeLeftDrop_2',
ComputeBeltDrop(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': 'GoToDropLeft_2', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'move_group': 'move_groupL', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_linkL', 'pose': 'DropPose', 'offset': 'SideZoffset', 'SideYoffset': 'SideYoffset', 'SideXoffset': 'SideXoffset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:836 y:545
OperatableStateMachine.add('GoToDropRight_2',
MoveitToJointsDynAriacState(),
transitions={'reached': 'GripperUITRechts_2', 'planning_failed': 'GripperUITRechts_2', 'control_failed': 'GripperUITRechts_2'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_groupR', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:834 y:607
OperatableStateMachine.add('GoToDropLeft_2',
MoveitToJointsDynAriacState(),
transitions={'reached': 'GripperUITLinks_2', 'planning_failed': 'GripperUITLinks_2', 'control_failed': 'GripperUITLinks_2'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_groupL', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1008 y:547
OperatableStateMachine.add('GripperUITRechts_2',
VacuumGripperControlState(enable=False),
transitions={'continue': 'ResetRightArmItem_2', 'failed': 'GoToDropRight_2', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_idR'})
# x:1008 y:606
OperatableStateMachine.add('GripperUITLinks_2',
VacuumGripperControlState(enable=False),
transitions={'continue': 'ResetLeftArmItem_2', 'failed': 'GoToDropLeft_2', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_idL'})
# x:1173 y:549
OperatableStateMachine.add('ResetRightArmItem_2',
ReplaceState(),
transitions={'done': 'AddX2offsetPiston'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'Niks', 'result': 'RightArmItem'})
# x:1173 y:607
OperatableStateMachine.add('ResetLeftArmItem_2',
ReplaceState(),
transitions={'done': 'AddX2offsetPiston'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'Niks', 'result': 'LeftArmItem'})
# x:1343 y:550
OperatableStateMachine.add('AddX2offsetPiston',
AddNumericState(),
transitions={'done': 'CheckPistonRows'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'offsetXPiston', 'value_b': 'PistonOffset', 'result': 'offsetXPiston'})
# x:1517 y:602
OperatableStateMachine.add('CheckPistonRound',
EqualState(),
transitions={'true': 'ResetPistonRound', 'false': 'StartUP'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'PistonRound', 'value_b': 'RoundTarget'})
# x:1343 y:615
OperatableStateMachine.add('CheckPistonRows',
EqualState(),
transitions={'true': 'ResetPistonRow', 'false': 'AddPistonRound'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'PistonRound', 'value_b': 'RowTarget'})
# x:1342 y:676
OperatableStateMachine.add('ResetPistonRow',
ReplaceState(),
transitions={'done': 'AddYoffsetPiston'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'Zero_Value', 'result': 'offsetXPiston'})
# x:1340 y:735
OperatableStateMachine.add('AddYoffsetPiston',
AddNumericState(),
transitions={'done': 'AddPistonRound'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'PistonOffset', 'value_b': 'offsetYPiston', 'result': 'offsetYPiston'})
# x:1514 y:669
OperatableStateMachine.add('AddPistonRound',
AddNumericState(),
transitions={'done': 'CheckPistonRound'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'One_value', 'value_b': 'PistonRound', 'result': 'PistonRound'})
# x:1688 y:602
OperatableStateMachine.add('ResetPistonRound',
ReplaceState(),
transitions={'done': 'BinVol'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'Zero_Value', 'result': 'offsetYPiston'})
# x:1559 y:228
OperatableStateMachine.add('BackToSH3',
SrdfStateToMoveitAriac(),
transitions={'reached': 'StartUP', '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_nameSH3', 'move_group': 'move_groupG', '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:1648 y:487, x:394 y:242
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.config_name_gantry_Pre_Conv_LA = 'Gantry_PreGrasp_Conv_LA'
_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.poweroff = 0
_state_machine.userdata.Partsaantal = 0
_state_machine.userdata.Partsvast = 3
_state_machine.userdata.Plus1 = 1
_state_machine.userdata.poweron = 100
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_4'
_state_machine.userdata.camera_frame = 'logical_camera_4_frame'
_state_machine.userdata.part = ''
_state_machine.userdata.pose = ''
_state_machine.userdata.move_group_right_arm = 'Right_Arm'
_state_machine.userdata.move_group_left_arm = 'Left_Arm'
_state_machine.userdata.arm_idL = 'left'
_state_machine.userdata.arm_idR = 'right'
_state_machine.userdata.config_name_Left_Home = ''
_state_machine.userdata.config_name_Right_Home = ''
_state_machine.userdata.config_name_gantry_Pre_Conv_RA = 'Gantry_PreGrasp_Conv_RA'
_state_machine.userdata.config_name_RA_pre = 'config_name_RA_pre'
_state_machine.userdata.config_name_LA_pre = 'config_name_LA_pre'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'Start',
StartAssignment(),
transitions={'continue': 'Move_to_Home'},
autonomy={'continue': Autonomy.Off})
# x:213 y:41
OperatableStateMachine.add('Move_to_Home',
self.use_behavior(
Move_to_HomeSM, 'Move_to_Home'),
transitions={
'finished': 'Oppakken',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'arm_idL':
'arm_idL',
'arm_idR':
'arm_idR',
'config_name_PreSide':
'config_name_PreSide',
'config_name_Right_Home':
'config_name_Right_Home',
'config_name_Left_Home':
'config_name_Left_Home',
'config_name_gantry':
'config_name_gantry'
})
# x:977 y:473
OperatableStateMachine.add('3 parts?',
EqualState(),
transitions={
'true': 'finished',
'false': 'Start conveyer'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'Partsvast',
'value_b': 'Partsaantal'
})
# x:1233 y:370
OperatableStateMachine.add('increase',
AddNumericState(),
transitions={'done': '3 parts?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'Plus1',
'value_b': 'Partsaantal',
'result': 'Partsaantal'
})
# x:601 y:399
OperatableStateMachine.add('Start conveyer',
SetConveyorbeltPowerState(),
transitions={
'continue': 'Oppakken',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'poweron'})
# x:823 y:35
OperatableStateMachine.add(
'Oppakken',
self.use_behavior(OppakkenSM, 'Oppakken'),
transitions={
'finished': 'Droppen',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part': 'part',
'pose': 'pose',
'move_group_prefix': 'move_group_prefix',
'move_group_right_arm': 'move_group_right_arm',
'move_group_left_arm': 'move_group_left_arm',
'action_topic': 'action_topic',
'camera_frame': 'camera_frame',
'camera_topic': 'camera_topic',
'poweroff': 'poweroff',
'ref_frame': 'ref_frame',
'move_group_gantry': 'move_group_gantry',
'robot_name': 'robot_name',
'config_name_Left_Home': 'config_name_Left_Home',
'config_name_Right_Home': 'config_name_Right_Home',
'arm_idL': 'arm_idL',
'arm_idR': 'arm_idR',
'config_name_gantry_Pre_Conv_RA':
'config_name_gantry_Pre_Conv_RA',
'config_name_gantry_Pre_Conv_LA':
'config_name_gantry_Pre_Conv_LA',
'config_name_RA_pre': 'config_name_RA_pre',
'config_name_LA_pre': 'config_name_LA_pre',
'offset_var_left': 'offset_var_left',
'offset_var_right': 'offset_var_right',
'offset_gasket': 'offset_gasket',
'offset_piston': 'offset_piston'
})
# x:1314 y:156
OperatableStateMachine.add('Droppen',
self.use_behavior(DroppenSM, 'Droppen'),
transitions={
'finished': 'increase',
'failed': 'failed',
'end': 'finished'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'end': Autonomy.Inherit
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group_right_arm':
'move_group_right_arm',
'move_group_left_arm':
'move_group_left_arm',
'action_topic':
'action_topic',
'camera_frame':
'camera_frame',
'camera_topic':
'camera_topic',
'ref_frame':
'ref_frame',
'move_group_gantry':
'move_group_gantry',
'robot_name':
'robot_name',
'offset_var_left':
'offset_var_left',
'offset_var_right':
'offset_var_right',
'offset_gasket':
'offset_gasket',
'offset_piston':
'offset_piston',
'arm_idL':
'arm_idL',
'arm_idR':
'arm_idR',
'config_name_Right_Home':
'config_name_Right_Home',
'config_name_Left_Home':
'config_name_Left_Home',
'config_name_RA_pre':
'config_name_RA_pre',
'config_name_LA_pre':
'config_name_LA_pre'
})
return _state_machine
def create(self):
# x:346 y:592, x:899 y:56
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['offsetx', 'offsety'], output_keys=['offsetx', 'offsety'])
_state_machine.userdata.offsetx = 0
_state_machine.userdata.offsety = 0
_state_machine.userdata.opschuifx = 0.2
_state_machine.userdata.opschuify = 0.2
_state_machine.userdata.een = 1
_state_machine.userdata.nul = 0
_state_machine.userdata.tellerx = 0
_state_machine.userdata.tellery = 0
_state_machine.userdata.twee = 2
_state_machine.userdata.message = 'BinisVol!'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:121 y:40
OperatableStateMachine.add('CheckY',
EqualState(),
transitions={'true': 'ResetY', 'false': 'AddY'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'tellery', 'value_b': 'twee'})
# x:103 y:280
OperatableStateMachine.add('AddTellerY',
AddNumericState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'tellery', 'value_b': 'een', 'result': 'tellery'})
# x:339 y:34
OperatableStateMachine.add('ResetY',
ReplaceState(),
transitions={'done': 'CheckX'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'tellery', 'result': 'nul'})
# x:369 y:276
OperatableStateMachine.add('AddX',
AddNumericState(),
transitions={'done': 'AddTellerX'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'offsetx', 'value_b': 'opschuifx', 'result': 'offsetx'})
# x:383 y:409
OperatableStateMachine.add('AddTellerX',
AddNumericState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'tellerx', 'value_b': 'een', 'result': 'tellerx'})
# x:340 y:138
OperatableStateMachine.add('CheckX',
EqualState(),
transitions={'true': 'ResetX', 'false': 'AddX'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'tellerx', 'value_b': 'twee'})
# x:576 y:131
OperatableStateMachine.add('ResetX',
ReplaceState(),
transitions={'done': 'ResetX'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'tellerx', 'result': 'nul'})
# x:776 y:130
OperatableStateMachine.add('BinisVol',
MessageState(),
transitions={'continue': 'failed'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'message'})
# x:114 y:170
OperatableStateMachine.add('AddY',
AddNumericState(),
transitions={'done': 'AddTellerY'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'offsety', 'value_b': 'opschuify', 'result': 'offsety'})
return _state_machine
def create(self):
# x:821 y:56, x:1214 y:358
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.order_id = ''
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.old_order_id = ''
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.material_locations = []
_state_machine.userdata.bin_index = 0
_state_machine.userdata.bin_id = ''
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
_state_machine.userdata.arm_right = 'right_arm'
_state_machine.userdata.arm_left = 'left_arm'
_state_machine.userdata.product_location1 = ''
_state_machine.userdata.product_location2 = ''
_state_machine.userdata.home_rightarm_id = 'Right_PreGrasp'
_state_machine.userdata.move_group_right = 'Right_Arm'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.home_leftarm_id = 'Left_PreGrasp'
_state_machine.userdata.move_group_left = 'Left_Arm'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:57 y:48
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:218 y:48
OperatableStateMachine.add('GetOrder',
GetOrderState(),
transitions={'continue': 'Test_LastOrder'},
autonomy={'continue': Autonomy.Off},
remapping={'order_id': 'order_id', 'shipments': 'shipments', 'number_of_shipments': 'number_of_shipments'})
# x:422 y:47
OperatableStateMachine.add('Test_LastOrder',
EqualState(),
transitions={'true': 'EndAssignment', 'false': 'Remember_OldOrder'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'order_id', 'value_b': 'old_order_id'})
# x:614 y:124
OperatableStateMachine.add('Remember_OldOrder',
ReplaceState(),
transitions={'done': 'GetShipment'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'order_id', 'result': 'old_order_id'})
# x:622 y:47
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:801 y:119
OperatableStateMachine.add('GetShipment',
GetProductsFromShipmentState(),
transitions={'continue': 'GetPart', 'invalid_index': 'failed'},
autonomy={'continue': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'shipments': 'shipments', 'index': 'shipment_index', 'shipment_type': 'shipment_type', 'agv_id': 'agv_id', 'products': 'products', 'number_of_products': 'number_of_products'})
# x:1023 y:119
OperatableStateMachine.add('GetPart',
GetPartFromProductsState(),
transitions={'continue': 'MaterialLocation', 'invalid_index': 'failed'},
autonomy={'continue': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'products': 'products', 'index': 'product_index', 'type': 'part_type', 'pose': 'part_pose'})
# x:1217 y:116
OperatableStateMachine.add('MaterialLocation',
GetMaterialLocationsState(),
transitions={'continue': 'GetBin'},
autonomy={'continue': Autonomy.Off},
remapping={'part': 'part_type', 'material_locations': 'material_locations'})
# x:1403 y:114
OperatableStateMachine.add('GetBin',
GetItemFromListState(),
transitions={'done': 'CheckAttachedRight', 'invalid_index': 'failed'},
autonomy={'done': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'list': 'material_locations', 'index': 'bin_index', 'item': 'bin_id'})
# x:1361 y:507
OperatableStateMachine.add('IncrementProductIndex',
AddNumericState(),
transitions={'done': 'EndProduct'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'product_index', 'value_b': 'one_value', 'result': 'product_index'})
# x:1360 y:585
OperatableStateMachine.add('EndProduct',
EqualState(),
transitions={'true': 'HomepositieRightArm', 'false': 'CheckAttachedRight2'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'product_index', 'value_b': 'number_of_products'})
# x:95 y:565
OperatableStateMachine.add('ResetProductIndex',
ReplaceState(),
transitions={'done': 'IncrementShipmentIndex'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'zero_value', 'result': 'product_index'})
# x:98 y:469
OperatableStateMachine.add('IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'Notify_Shipment_Ready'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'shipment_index', 'value_b': 'one_value', 'result': 'shipment_index'})
# x:89 y:277
OperatableStateMachine.add('EndShipment',
EqualState(),
transitions={'true': 'ResetShipmentIndex', 'false': 'GetShipment'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'shipment_index', 'value_b': 'number_of_shipments'})
# x:87 y:378
OperatableStateMachine.add('Notify_Shipment_Ready',
self.use_behavior(Notify_Shipment_ReadySM, 'Notify_Shipment_Ready'),
transitions={'finished': 'EndShipment', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'shipment_type': 'shipment_type'})
# x:1605 y:181
OperatableStateMachine.add('Transport_part_from_bin_to_agv',
self.use_behavior(Transport_part_from_bin_to_agvSM, 'Transport_part_from_bin_to_agv'),
transitions={'finished': 'SetVariablePart1', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'part_type': 'part_type', 'part_pose': 'part_pose', 'bin_id': 'bin_id'})
# x:95 y:183
OperatableStateMachine.add('ResetShipmentIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'zero_value', 'result': 'shipment_index'})
# x:1354 y:248
OperatableStateMachine.add('CheckAttachedRight',
CheckGripperattached(),
transitions={'True': 'CheckAttachedLeft', 'False': 'Transport_part_from_bin_to_agv', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_right'})
# x:1356 y:335
OperatableStateMachine.add('CheckAttachedLeft',
CheckGripperattached(),
transitions={'True': 'IncrementProductIndex', 'False': 'Transport_part_from_bin_to_agv_LeftArm', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_left'})
# x:1572 y:418
OperatableStateMachine.add('Transport_part_from_bin_to_agv_LeftArm',
self.use_behavior(Transport_part_from_bin_to_agv_LeftArmSM, 'Transport_part_from_bin_to_agv_LeftArm'),
transitions={'finished': 'SetVariablePart2', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'part_type': 'part_type', 'part_pose': 'part_pose', 'bin_id': 'bin_id'})
# x:1654 y:275
OperatableStateMachine.add('SetVariablePart1',
ReplaceState(),
transitions={'done': 'IncrementProductIndex'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'part_pose', 'result': 'product_location1'})
# x:1651 y:503
OperatableStateMachine.add('SetVariablePart2',
ReplaceState(),
transitions={'done': 'IncrementProductIndex'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'part_pose', 'result': 'product_location2'})
# x:997 y:510
OperatableStateMachine.add('CheckAttachedRight2',
CheckGripperattached(),
transitions={'True': 'CheckAttachedLeft2', 'False': 'GetPart', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_right'})
# x:803 y:520
OperatableStateMachine.add('CheckAttachedLeft2',
CheckGripperattached(),
transitions={'True': 'PlaceOnAgv', 'False': 'GetPart', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_left'})
# x:533 y:450
OperatableStateMachine.add('PlaceOnAgv',
self.use_behavior(PlaceOnAgvSM, 'PlaceOnAgv'),
transitions={'finished': 'GetPart', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'part_pose_right': 'product_location1', 'part_pose_left': 'product_location2'})
# x:1371 y:670
OperatableStateMachine.add('HomepositieRightArm',
SrdfStateToMoveitAriac(),
transitions={'reached': 'HomepositieLeftArm', 'planning_failed': 'Retry_1', 'control_failed': 'Retry_1', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'home_rightarm_id', 'move_group': 'move_group_right', '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:1182 y:655
OperatableStateMachine.add('HomepositieLeftArm',
SrdfStateToMoveitAriac(),
transitions={'reached': 'PlaceOnAgv_2', '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': 'home_leftarm_id', 'move_group': 'move_group_left', '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:1558 y:671
OperatableStateMachine.add('Retry_1',
WaitState(wait_time=3),
transitions={'done': 'HomepositieRightArm'},
autonomy={'done': Autonomy.Off})
# x:1226 y:733
OperatableStateMachine.add('Retry_2',
WaitState(wait_time=3),
transitions={'done': 'HomepositieLeftArm'},
autonomy={'done': Autonomy.Off})
# x:832 y:626
OperatableStateMachine.add('PlaceOnAgv_2',
self.use_behavior(PlaceOnAgvSM, 'PlaceOnAgv_2'),
transitions={'finished': 'ResetProductIndex', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'part_pose_right': 'product_location1', 'part_pose_left': 'product_location2'})
return _state_machine
def create(self):
# x:579 y:679, x:313 y:185, x:323 y:666
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed', 'false'],
input_keys=[
'number_of_products', 'product_index', 'bin_id', 'part_type',
'part_pose', 'agv_id'
],
output_keys=['product_index'])
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.one_value = 1
_state_machine.userdata.bin_id = ''
_state_machine.userdata.zero_value = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.agv_id = ''
_state_machine.userdata.arm_id_right = 'right'
_state_machine.userdata.arm_id_left = 'left'
_state_machine.userdata.part_pose_right = []
_state_machine.userdata.part_pose_left = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:49 y:41
OperatableStateMachine.add('GripperControlLeft',
GripperControle(),
transitions={
'Active': 'GripperControlRight',
'Not_active': 'SelectorStateLeft',
'invalid_arm_id': 'failed'
},
autonomy={
'Active': Autonomy.Off,
'Not_active': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_left'})
# x:50 y:185
OperatableStateMachine.add('SelectorStateLeft',
self.use_behavior(
SelectorStateLeftSM,
'SelectorStateLeft'),
transitions={
'finished': 'IncrementProductIndex',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'bin_id': 'bin_id',
'part_type': 'part_type',
'arm_id_left': 'arm_id_left',
'part_pose': 'part_pose',
'part_pose_left': 'part_pose_left'
})
# x:516 y:37
OperatableStateMachine.add('GripperControlRight',
GripperControle(),
transitions={
'Active': 'AgvSelector',
'Not_active': 'SelectorStateRight',
'invalid_arm_id': 'failed'
},
autonomy={
'Active': Autonomy.Off,
'Not_active': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_right'})
# x:510 y:189
OperatableStateMachine.add(
'SelectorStateRight',
self.use_behavior(SelectorStateRightSM, 'SelectorStateRight'),
transitions={
'finished': 'IncrementProductIndex_2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'bin_id': 'bin_id',
'part_type': 'part_type',
'arm_id_right': 'arm_id_right',
'part_pose': 'part_pose',
'part_pose_right': 'part_pose_right'
})
# x:49 y:302
OperatableStateMachine.add('IncrementProductIndex',
AddNumericState(),
transitions={'done': 'EndProduct'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'one_value',
'result': 'product_index'
})
# x:49 y:398
OperatableStateMachine.add('EndProduct',
EqualState(),
transitions={
'true': 'AgvSelector',
'false': 'false'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:507 y:303
OperatableStateMachine.add('IncrementProductIndex_2',
AddNumericState(),
transitions={'done': 'EndProduct_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'one_value',
'result': 'product_index'
})
# x:508 y:396
OperatableStateMachine.add('EndProduct_2',
EqualState(),
transitions={
'true': 'AgvSelector',
'false': 'false'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:509 y:478
OperatableStateMachine.add('ResetProductIndex',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'product_index'
})
# x:271 y:473
OperatableStateMachine.add('EndProduct_3',
EqualState(),
transitions={
'true': 'ResetProductIndex',
'false': 'false'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:267 y:347
OperatableStateMachine.add('AgvSelector',
self.use_behavior(
AgvSelectorSM, 'AgvSelector'),
transitions={
'finished': 'EndProduct_3',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_pose_right':
'part_pose_right',
'part_pose_left': 'part_pose_left',
'agv_id': 'agv_id',
'arm_id_right': 'arm_id_right',
'arm_id_left': 'arm_id_left'
})
return _state_machine
def create(self):
# x:667 y:540, x:729 y:205
_state_machine = OperatableStateMachine(outcomes=['finished', 'fail'])
_state_machine.userdata.shipments = []
_state_machine.userdata.part_type = ''
_state_machine.userdata.material_locations = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.order_id = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.pose = []
_state_machine.userdata.product_index = 0
_state_machine.userdata.shipment_index = 1
_state_machine.userdata.add_one = 1
_state_machine.userdata.part_pose = []
_state_machine.userdata.zero = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Start',
StartAssignment(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:905 y:34
OperatableStateMachine.add('GetProductPose',
GetPartFromProductsState(),
transitions={
'continue': 'message_3',
'invalid_index': 'fail'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'product_index',
'type': 'part_type',
'pose': 'part_pose'
})
# x:1091 y:123
OperatableStateMachine.add('TransportPartToAgv',
self.use_behavior(
TransportPartToAgvSM,
'TransportPartToAgv'),
transitions={
'finished': 'IncrementProductIndex',
'failed': 'fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'agv_id': 'agv_id',
'part_type': 'part_type',
'part_pose': 'part_pose'
})
# x:1095 y:229
OperatableStateMachine.add('IncrementProductIndex',
AddNumericState(),
transitions={'done': 'ShipmentReady?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'add_one',
'result': 'product_index'
})
# x:1092 y:486
OperatableStateMachine.add(
'IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'notify_shipment_ready'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'add_one',
'result': 'shipment_index'
})
# x:1094 y:329
OperatableStateMachine.add('ShipmentReady?',
EqualState(),
transitions={
'true': 'ResetProductIndex',
'false': 'GetProductPose'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:474 y:461
OperatableStateMachine.add('OrderReady?',
EqualState(),
transitions={
'true': 'ResetShiptIndex',
'false': 'GetProductsShipment'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:857 y:483
OperatableStateMachine.add('notify_shipment_ready',
self.use_behavior(
notify_shipment_readySM,
'notify_shipment_ready'),
transitions={
'finished': 'OrderReady?',
'failed': 'fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:402 y:33
OperatableStateMachine.add(
'message',
MessageLogger(),
transitions={'continue': 'GetProductsShipment'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'order_id'})
# x:766 y:40
OperatableStateMachine.add(
'message_2',
MessageLogger(),
transitions={'continue': 'GetProductPose'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'shipment_type'})
# x:1090 y:36
OperatableStateMachine.add(
'message_3',
MessageLogger(),
transitions={'continue': 'TransportPartToAgv'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:560 y:34
OperatableStateMachine.add('GetProductsShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'message_2',
'invalid_index': 'fail'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipment_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:203 y:32
OperatableStateMachine.add('GetOrder',
GetOrderState(),
transitions={'continue': 'message'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id':
'order_id',
'shipments':
'shipments',
'number_of_shipments':
'number_of_shipments'
})
# x:1090 y:407
OperatableStateMachine.add(
'ResetProductIndex',
ReplaceState(),
transitions={'done': 'IncrementShipmentIndex'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'product_index'
})
# x:338 y:318
OperatableStateMachine.add('ResetShiptIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'shipment_index'
})
return _state_machine
def create(self):
# x:1067 y:518, x:672 y:189
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['shipment_index', 'part_index'],
output_keys=[
'part_type_L', 'part_type_R', 'agv_id', 'part_pose_L',
'part_pose_R', 'shipment_index', 'part_index'
])
_state_machine.userdata.order_id = ''
_state_machine.userdata.shipments = ''
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.part_index = 0
_state_machine.userdata.part_type_R = ''
_state_machine.userdata.part_pose_L = []
_state_machine.userdata.part_type_L = []
_state_machine.userdata.part_pose_R = []
_state_machine.userdata.add_one = 1
_state_machine.userdata.reset = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.none = 'None'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:253 y:450
OperatableStateMachine.add('ShipmentReady?',
EqualState(),
transitions={
'true': 'ResetPartIndex',
'false': 'GetRPartPose'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_products',
'value_b': 'part_index'
})
# x:442 y:38
OperatableStateMachine.add('GetShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'GetRPartPose',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipment_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:658 y:37
OperatableStateMachine.add('GetRPartPose',
GetPartFromProductsState(),
transitions={
'continue': 'IncrementPartIndex',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'part_index',
'type': 'part_type_R',
'pose': 'part_pose_R'
})
# x:853 y:181
OperatableStateMachine.add('GetLPartPose',
GetPartFromProductsState(),
transitions={
'continue': 'IncrementPartIndex_2',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'part_index',
'type': 'part_type_L',
'pose': 'part_pose_L'
})
# x:1046 y:185
OperatableStateMachine.add('IncrementShipIndex',
AddNumericState(),
transitions={'done': 'CheckAGV'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'add_one',
'result': 'shipment_index'
})
# x:262 y:171
OperatableStateMachine.add('OrderReady?',
EqualState(),
transitions={
'true': 'ResetShipmentIndex',
'false': 'GetShipment'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_shipments',
'value_b': 'shipment_index'
})
# x:853 y:253
OperatableStateMachine.add(
'IncrementPartIndex_2',
AddNumericState(),
transitions={'done': 'ShipmentReady2?2'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'part_index',
'value_b': 'add_one',
'result': 'part_index'
})
# x:853 y:111
OperatableStateMachine.add('ShipmentReady2?',
EqualState(),
transitions={
'true': 'None',
'false': 'GetLPartPose'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_products',
'value_b': 'part_index'
})
# x:249 y:387
OperatableStateMachine.add('ResetPartIndex',
ReplaceState(),
transitions={'done': 'ShipIndex0'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'reset',
'result': 'part_index'
})
# x:260 y:107
OperatableStateMachine.add('ResetShipmentIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'reset',
'result': 'shipment_index'
})
# x:261 y:38
OperatableStateMachine.add('GetOrder',
GetOrderState(),
transitions={'continue': 'GetShipment'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id':
'order_id',
'shipments':
'shipments',
'number_of_shipments':
'number_of_shipments'
})
# x:1044 y:110
OperatableStateMachine.add(
'None',
ReplaceState(),
transitions={'done': 'IncrementShipIndex'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'none',
'result': 'part_type_L'
})
# x:850 y:36
OperatableStateMachine.add('IncrementPartIndex',
AddNumericState(),
transitions={'done': 'ShipmentReady2?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'part_index',
'value_b': 'add_one',
'result': 'part_index'
})
# x:1041 y:257
OperatableStateMachine.add('ShipmentReady2?2',
EqualState(),
transitions={
'true': 'IncrementShipIndex',
'false': 'CheckAGV'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_products',
'value_b': 'part_index'
})
# x:215 y:313
OperatableStateMachine.add('ShipIndex0',
EqualState(),
transitions={
'true': 'OrderReady?',
'false': 'SendShipment'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'reset'
})
# x:1159 y:330
OperatableStateMachine.add('CheckAGV',
self.use_behavior(
CheckAGVSM, 'CheckAGV'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'agv_id': 'agv_id'})
# x:325 y:244
OperatableStateMachine.add('SendShipment',
NotifyShipmentReadyState(),
transitions={
'continue': 'OrderReady?',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'agv_id': 'agv_id',
'shipment_type': 'shipment_type',
'success': 'success',
'message': 'message'
})
return _state_machine
def create(self):
# x:1144 y:563, x:642 y:465
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group = ''
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.arm_id = 'Right_Arm'
_state_machine.userdata.pose = []
_state_machine.userdata.offset = 0.020
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_belt'
_state_machine.userdata.camera_frame = 'logical_camera_belt_frame'
_state_machine.userdata.home = 'Gantry_Home'
_state_machine.userdata.part = 'gasket_part_blue'
_state_machine.userdata.belt = 'Gantry_Belt_Right'
_state_machine.userdata.full_home = 'Full_Home_Blue'
_state_machine.userdata.clearance = 0.05
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:29 y:50
OperatableStateMachine.add(
'Detect_Blue_Part',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Move_Belt',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part',
'pose': 'pose'
})
# x:240 y:46
OperatableStateMachine.add('Compute_Pick',
ComputeGraspAriacState(),
transitions={
'continue': 'Move_to_Pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:454 y:50
OperatableStateMachine.add('Move_to_Pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_On',
'planning_failed': 'failed',
'control_failed': 'Gripper_On'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:671 y:56
OperatableStateMachine.add('Gripper_On',
GripperControl(enable=True),
transitions={
'continue': 'Move_Belt_2',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:838 y:52
OperatableStateMachine.add('Move_Belt_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Add_Clearance',
'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': 'belt',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:50 y:152
OperatableStateMachine.add('Move_Belt',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Offset',
'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': 'belt',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1046 y:358
OperatableStateMachine.add('Left_Arm_Home',
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': 'full_home',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1041 y:144
OperatableStateMachine.add('Compute_Grasp',
ComputeGraspAriacState(),
transitions={
'continue': 'Move_Clearance',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1013 y:50
OperatableStateMachine.add('Add_Clearance',
AddNumericState(),
transitions={'done': 'Compute_Grasp'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'clearance',
'value_b': 'offset',
'result': 'offset'
})
# x:1039 y:226
OperatableStateMachine.add('Move_Clearance',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Left_Arm_Home',
'planning_failed': 'Left_Arm_Home',
'control_failed': 'Move_Clearance'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:195 y:108
OperatableStateMachine.add('Offset',
part_offsetCalc(),
transitions={
'succes': 'Compute_Pick',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part',
'part_offset': 'offset'
})
return _state_machine
def create(self):
# x:144 y:698, x:147 y:461
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.order_id = ''
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
_state_machine.userdata.shipment_type = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:190 y:41
OperatableStateMachine.add(
'GetOrder',
GetOrderState(),
transitions={'continue': 'GetProductsFromShipment'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
# x:409 y:40
OperatableStateMachine.add('GetProductsFromShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'GetPartFromProduct',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipment_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:670 y:38
OperatableStateMachine.add(
'GetPartFromProduct',
GetPartFromProductsState(),
transitions={
'continue': 'transport_part_form_bin_to_agv_state',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'product_index',
'type': 'part_type',
'pose': 'part_pose'
})
# x:1211 y:34
OperatableStateMachine.add('IncrementProductIndex',
AddNumericState(),
transitions={'done': 'EndProducts'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'one_value',
'result': 'product_index'
})
# x:1213 y:164
OperatableStateMachine.add('EndProducts',
EqualState(),
transitions={
'true': 'ResetProductIndex',
'false': 'GetPartFromProduct'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:1214 y:282
OperatableStateMachine.add(
'ResetProductIndex',
ReplaceState(),
transitions={'done': 'IncrementShipmentIndex'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'product_index'
})
# x:1214 y:413
OperatableStateMachine.add('IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'EindShipments'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'one_value',
'result': 'shipment_index'
})
# x:1215 y:541
OperatableStateMachine.add('EindShipments',
EqualState(),
transitions={
'true': 'ResetShipmentIndex',
'false': 'notify_shipment_ready'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:1000 y:541
OperatableStateMachine.add('ResetShipmentIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'shipment_index'
})
# x:1211 y:782
OperatableStateMachine.add('notify_shipment_ready',
self.use_behavior(
notify_shipment_readySM,
'notify_shipment_ready'),
transitions={
'finished':
'GetProductsFromShipment',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:911 y:38
OperatableStateMachine.add(
'transport_part_form_bin_to_agv_state',
self.use_behavior(transport_part_form_bin_to_agv_stateSM,
'transport_part_form_bin_to_agv_state'),
transitions={
'finished': 'IncrementProductIndex',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
return _state_machine
def create(self):
# x:650 y:25, x:929 y:416
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.products = []
_state_machine.userdata.product_index = 0
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.material_locations = []
_state_machine.userdata.bin_index = 0
_state_machine.userdata.order_id = ''
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.bin_id = ''
_state_machine.userdata.old_order_id = ''
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:45 y:112
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:211 y:112
OperatableStateMachine.add('GetOrder',
GetOrderState(),
transitions={'continue': 'ConveyorOff'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id':
'order_id',
'shipments':
'shipments',
'number_of_shipments':
'number_of_shipments'
})
# x:424 y:113
OperatableStateMachine.add('TestLastOrder',
EqualState(),
transitions={
'true': 'EndAssignment',
'false': 'RememberOldOrder'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'order_id',
'value_b': 'old_order_id'
})
# x:650 y:114
OperatableStateMachine.add('RememberOldOrder',
ReplaceState(),
transitions={'done': 'GetShipment'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'order_id',
'result': 'old_order_id'
})
# x:869 y:114
OperatableStateMachine.add('GetShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'GetPart',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipment_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:1122 y:115
OperatableStateMachine.add('GetPart',
GetPartFromProductsState(),
transitions={
'continue': 'MaterialLocation',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'product_index',
'type': 'part_type',
'pose': 'part_pose'
})
# x:1361 y:113
OperatableStateMachine.add('MaterialLocation',
GetMaterialLocationsState(),
transitions={'continue': 'GetBin'},
autonomy={'continue': Autonomy.Off},
remapping={
'part':
'part_type',
'material_locations':
'material_locations'
})
# x:1361 y:235
OperatableStateMachine.add('GetBin',
GetItemFromListState(),
transitions={
'done': 'AGV_id',
'invalid_index': 'failed'
},
autonomy={
'done': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'list': 'material_locations',
'index': 'bin_index',
'item': 'bin_id'
})
# x:759 y:683
OperatableStateMachine.add('IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'EndShipment'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'one_value',
'result': 'shipment_index'
})
# x:536 y:683
OperatableStateMachine.add('EndShipment',
EqualState(),
transitions={
'true': 'Notify_shipment_ready',
'false': 'GetShipment'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:446 y:6
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:206 y:680
OperatableStateMachine.add('Notify_shipment_ready',
self.use_behavior(
Notify_shipment_readySM,
'Notify_shipment_ready'),
transitions={
'finished': 'ResetShipmentIndex',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'shipment_type': 'shipment_type',
'agv_id': 'agv_id'
})
# x:1357 y:675
OperatableStateMachine.add(
'GripperChoice',
self.use_behavior(GripperChoiceSM, 'GripperChoice'),
transitions={
'finished': 'IncrementShipmentIndex',
'failed': 'failed',
'false': 'GetPart'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'false': Autonomy.Inherit
},
remapping={
'number_of_products': 'number_of_products',
'product_index': 'product_index',
'bin_id': 'bin_id',
'part_type': 'part_type',
'part_pose': 'part_pose',
'agv_id': 'agv_id'
})
# x:194 y:377
OperatableStateMachine.add('ResetShipmentIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'shipment_index'
})
# x:216 y:19
OperatableStateMachine.add('ConveyorOff',
SetConveyorbeltPowerState(),
transitions={
'continue': 'TestLastOrder',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'zero_value'})
# x:1384 y:446
OperatableStateMachine.add(
'AGV_id',
MessageState(),
transitions={'continue': 'GripperChoice'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
return _state_machine
def create(self):
# x:27 y:469, x:242 y:360
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.config_name = ''
_state_machine.userdata.move_group_g = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.tool_link = ''
_state_machine.userdata.pose = ''
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group = ''
_state_machine.userdata.arm_id = ''
_state_machine.userdata.part_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.one = 1
_state_machine.userdata.product_itt = 0
_state_machine.userdata.shipments_index = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:64 y:31
OperatableStateMachine.add(
'Start_assignment',
StartAssignment(),
transitions={'continue': 'move_home_belt'},
autonomy={'continue': Autonomy.Off})
# x:431 y:28
OperatableStateMachine.add(
'get_orders',
GetOrderState(),
transitions={'continue': 'Get shipments'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
# x:615 y:25
OperatableStateMachine.add('Get shipments',
GetProductsFromShipmentState(),
transitions={
'continue': 'get part',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipments_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:833 y:35
OperatableStateMachine.add('get part',
GetPartFromProductsState(),
transitions={
'continue': 'message part main prg',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'part_index',
'type': 'part_type',
'pose': 'pose'
})
# x:261 y:28
OperatableStateMachine.add('move_home_belt',
self.use_behavior(
move_home_beltSM, 'move_home_belt'),
transitions={
'finished': 'get_orders',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:1153 y:72
OperatableStateMachine.add('Order_picking',
self.use_behavior(
Order_pickingSM, 'Order_picking'),
transitions={
'finished': 'add_itterator order',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'part_type': 'part_type'})
# x:1038 y:147
OperatableStateMachine.add(
'add_itterator order',
AddNumericState(),
transitions={'done': 'part index message'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'one',
'value_b': 'part_index',
'result': 'part_index'
})
# x:1034 y:328
OperatableStateMachine.add('Check_product_amount',
EqualState(),
transitions={
'true': 'move_home_belt',
'false': 'Get shipments'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_products',
'value_b': 'part_index'
})
# x:1053 y:12
OperatableStateMachine.add(
'message part main prg',
MessageState(),
transitions={'continue': 'Order_picking'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:1141 y:221
OperatableStateMachine.add(
'part index message',
MessageState(),
transitions={'continue': 'Check_product_amount'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_index'})
return _state_machine
def create(self):
# x:642 y:425, x:506 y:337
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
output_keys=['part_type', 'agv_id', 'pose_on_agv'])
_state_machine.userdata.order_id = ' '
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.camera_ref_frame = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.camera_topic = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:59 y:52
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:225 y:52
OperatableStateMachine.add(
'GetOrder',
GetOrderState(),
transitions={'continue': 'GetProductsFromShipment'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
# x:442 y:53
OperatableStateMachine.add('GetProductsFromShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'GetPartFromProduct',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipment_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:675 y:55
OperatableStateMachine.add(
'GetPartFromProduct',
GetPartFromProductsState(),
transitions={
'continue': 'transport_part_form_bin_to_agv_state',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'product_index',
'type': 'part_type',
'pose': 'pose_on_agv'
})
# x:958 y:176
OperatableStateMachine.add(
'IncrementProductIndex',
AddNumericState(),
transitions={'done': 'EindeProductenLijst'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'one_value',
'result': 'product_index'
})
# x:963 y:273
OperatableStateMachine.add('EindeProductenLijst',
EqualState(),
transitions={
'true': 'IncrementShipmentIndex',
'false': 'GetPartFromProduct'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:964 y:363
OperatableStateMachine.add('IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'EindeShipment'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'one_value',
'result': 'shipment_index'
})
# x:967 y:449
OperatableStateMachine.add('EindeShipment',
EqualState(),
transitions={
'true': 'GetOrder',
'false': 'GetProductsFromShipment'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:917 y:45
OperatableStateMachine.add(
'transport_part_form_bin_to_agv_state',
self.use_behavior(transport_part_form_bin_to_agv_stateSM,
'transport_part_form_bin_to_agv_state'),
transitions={
'finished': 'IncrementProductIndex',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'agv_id': 'agv_id',
'pose_on_agv': 'pose_on_agv',
'camera_ref_frame': 'camera_ref_frame',
'camera_frame': 'camera_frame',
'camera_topic': 'camera_topic'
})
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:1428 y:298, x:1110 y:127
_state_machine = OperatableStateMachine(
outcomes=['finished', 'fail'],
input_keys=['Shipments', 'NumberOfShipments'])
_state_machine.userdata.Shipments = []
_state_machine.userdata.NumberOfShipments = 0
_state_machine.userdata.Products = []
_state_machine.userdata.NumberOfProducts = 0
_state_machine.userdata.AgvID = ''
_state_machine.userdata.ShipmentIndex = 1
_state_machine.userdata.ShipmentType = ''
_state_machine.userdata.ShipmentIterator = 0
_state_machine.userdata.OneValue = 1
_state_machine.userdata.agv_id = ''
_state_machine.userdata.agv_ready_state = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:43 y:24
OperatableStateMachine.add('GetProducts',
GetProductsFromShipmentState(),
transitions={
'continue': 'get_products',
'invalid_index': 'fail'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments': 'Shipments',
'index': 'ShipmentIterator',
'shipment_type': 'ShipmentType',
'agv_id': 'agv_id',
'products': 'Products',
'number_of_products':
'NumberOfProducts'
})
# x:1475 y:229
OperatableStateMachine.add(
'IncrementShipmentsIterator',
AddNumericState(),
transitions={'done': 'CompareShepmentsIterator'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'ShipmentIterator',
'value_b': 'OneValue',
'result': 'ShipmentIterator'
})
# x:1218 y:234
OperatableStateMachine.add('CompareShepmentsIterator',
EqualState(),
transitions={
'true': 'finished',
'false': 'GetProducts'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'ShipmentIterator',
'value_b': 'NumberOfShipments'
})
# x:730 y:26
OperatableStateMachine.add('get_products',
self.use_behavior(
get_productsSM, 'get_products'),
transitions={
'finished': 'NotifyShipmentReady',
'fail': 'fail'
},
autonomy={
'finished': Autonomy.Inherit,
'fail': Autonomy.Inherit
},
remapping={
'Products': 'Products',
'NumberOfProducts':
'NumberOfProducts',
'agv_id': 'agv_id'
})
# x:1354 y:16
OperatableStateMachine.add('GetAgvState',
GetAgvStatusState(),
transitions={
'continue': 'AgvReady',
'fail': 'fail'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'agv_id': 'agv_id',
'agv_state': 'agv_state'
})
# x:1228 y:125
OperatableStateMachine.add('AgvReady',
EqualState(),
transitions={
'true':
'IncrementShipmentsIterator',
'false': 'Wait'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv_state',
'value_b': 'agv_ready_state'
})
# x:1177 y:23
OperatableStateMachine.add('Wait',
WaitState(wait_time=1),
transitions={'done': 'GetAgvState'},
autonomy={'done': Autonomy.Off})
# x:959 y:20
OperatableStateMachine.add('NotifyShipmentReady',
NotifyShipmentReadyState(),
transitions={
'continue': 'Wait',
'fail': 'fail'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'agv_id': 'agv_id',
'shipment_type': 'ShipmentType',
'success': 'success',
'message': 'message'
})
return _state_machine
def create(self):
# x:1630 y:562, x:1479 y:95
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['arm'])
_state_machine.userdata.arm_id = 'arm1'
_state_machine.userdata.power = 100
_state_machine.userdata.NoPower = 0
_state_machine.userdata.part = ''
_state_machine.userdata.ref_frame1 = 'arm1_linear_arm_actuator'
_state_machine.userdata.Camera1_topic = '/ariac/Camera_Converyor_Links'
_state_machine.userdata.camera_frame1 = 'Camera_Converyor_Links_frame'
_state_machine.userdata.part_type = ''
_state_machine.userdata.pose = []
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = '/ariac/arm1'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.arm = ''
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.Disk_rotation = 0
_state_machine.userdata.Disk_offset = 0.035
_state_machine.userdata.config_name_R1PreBin3 = 'R1PreBin3'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.config_name_R1PreConveyor = 'R1PreConveyor'
_state_machine.userdata.Round_Nr = 1
_state_machine.userdata.Round_target = 6
_state_machine.userdata.Zero = 0
_state_machine.userdata.ONE = 1
_state_machine.userdata.SideYoffset = 0
_state_machine.userdata.SideXoffset = 0
_state_machine.userdata.PlusOffset = 0.20
_state_machine.userdata.config_name_R1HOME = 'R1Home'
_state_machine.userdata.Row_count = 3
_state_machine.userdata.offset = 0.040
_state_machine.userdata.rotation = 0
_state_machine.userdata.BIN3_pose = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:49 y:25
OperatableStateMachine.add('MoveToHOME',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GripperUIT_2',
'planning_failed': 'RetryHOME',
'control_failed': 'RetryHOME',
'param_error': 'WAIT_failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_R1HOME',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1262 y:559
OperatableStateMachine.add(
'WAIT_completed',
WaitState(wait_time=2),
transitions={'done': 'SluitConveyorAF_2'},
autonomy={'done': Autonomy.Off})
# x:202 y:91
OperatableStateMachine.add('Stopconveyor_2',
SetConveyorbeltPowerState(),
transitions={
'continue': 'MoveToPreConveyor',
'fail': 'WAIT_failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'NoPower'})
# x:1106 y:35
OperatableStateMachine.add('WAIT_failed',
WaitState(wait_time=5),
transitions={'done': 'SluitConveyorAF'},
autonomy={'done': Autonomy.Off})
# x:563 y:30
OperatableStateMachine.add('WAIT_check',
WaitState(wait_time=1),
transitions={'done': 'CheckforDisk'},
autonomy={'done': Autonomy.Off})
# x:372 y:91
OperatableStateMachine.add(
'CheckforDisk',
DetectFirstPartCameraAriacState(part_list=['disk_part'],
time_out=2),
transitions={
'continue': 'Stopconveyor_2',
'failed': 'WAIT_failed',
'not_found': 'WAIT_check'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame1',
'camera_topic': 'Camera1_topic',
'camera_frame': 'camera_frame1',
'part': 'part',
'pose': 'pose'
})
# x:200 y:162
OperatableStateMachine.add(
'Computebeltpick',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'MoveToConveyorPart',
'failed': 'WAIT_failed_pick'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'Disk_offset',
'rotation': 'Disk_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:371 y:166
OperatableStateMachine.add('MoveToConveyorPart',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'WAIT_gripperoke',
'planning_failed':
'WAIT_failed_pick',
'control_failed': 'WAIT_failed_drop'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1106 y:97
OperatableStateMachine.add('WAIT_failed_pick',
WaitState(wait_time=5),
transitions={'done': 'SluitConveyorAF'},
autonomy={'done': Autonomy.Off})
# x:201 y:299
OperatableStateMachine.add('MoveToBin3',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetBin3Pose',
'planning_failed': 'RetryBin3',
'control_failed': 'RetryBin3',
'param_error': 'WAIT_failed_pick'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_R1PreBin3',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:51 y:163
OperatableStateMachine.add('Gripperaan',
UseGripper(enable=True),
transitions={
'continue': 'Computebeltpick',
'failed': 'WAIT_failed_pick',
'invalid_arm': 'WAIT_failed_pick'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:370 y:243
OperatableStateMachine.add(
'WAIT_gripperoke',
WaitState(wait_time=0.5),
transitions={'done': 'MoveToPreConveyor_2'},
autonomy={'done': Autonomy.Off})
# x:51 y:96
OperatableStateMachine.add(
'MoveToPreConveyor',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Gripperaan',
'planning_failed': 'RetryPreConveyor',
'control_failed': 'RetryPreConveyor',
'param_error': 'WAIT_failed_pick'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_R1PreConveyor',
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out': 'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out': 'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:22 y:707
OperatableStateMachine.add(
'RetryPreConveyor',
WaitState(wait_time=1),
transitions={'done': 'MoveToPreConveyor'},
autonomy={'done': Autonomy.Off})
# x:261 y:768
OperatableStateMachine.add('RetryBin3',
WaitState(wait_time=1),
transitions={'done': 'MoveToBin3'},
autonomy={'done': Autonomy.Off})
# x:200 y:231
OperatableStateMachine.add(
'MoveToPreConveyor_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveToBin3',
'planning_failed': 'RetryPreConveyor_2',
'control_failed': 'RetryPreConveyor_2',
'param_error': 'WAIT_failed_pick'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_R1PreConveyor',
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out': 'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out': 'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:20 y:769
OperatableStateMachine.add(
'RetryPreConveyor_2',
WaitState(wait_time=1),
transitions={'done': 'MoveToPreConveyor_2'},
autonomy={'done': Autonomy.Off})
# x:356 y:566
OperatableStateMachine.add('GripperUIT',
UseGripper(enable=False),
transitions={
'continue': 'MoveToBin3_2',
'failed': 'WAIT_failed_pick',
'invalid_arm': 'WAIT_failed_pick'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1033 y:518
OperatableStateMachine.add('AddRound',
AddNumericState(),
transitions={'done': 'CheckRound'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'Round_Nr',
'value_b': 'ONE',
'result': 'Round_Nr'
})
# x:1032 y:584
OperatableStateMachine.add('CheckRound',
EqualState(),
transitions={
'true': 'ResetRound',
'false': 'StartConveryor'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'Round_Nr',
'value_b': 'Round_target'
})
# x:1031 y:654
OperatableStateMachine.add('ResetRound',
ReplaceState(),
transitions={'done': 'WAIT_completed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Round_Nr',
'result': 'Zero'
})
# x:360 y:382
OperatableStateMachine.add(
'ComputeDrop',
ComputeBeltDrop(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'ShowDropPose',
'failed': 'WAIT_failed_drop'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'BIN3_pose',
'offset': 'offset',
'SideYoffset': 'SideYoffset',
'SideXoffset': 'SideXoffset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:356 y:507
OperatableStateMachine.add('MoveToDROP',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperUIT',
'planning_failed': 'RetryDROP',
'control_failed': 'RetryDROP'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1106 y:159
OperatableStateMachine.add('WAIT_failed_drop',
WaitState(wait_time=5),
transitions={'done': 'SluitConveyorAF'},
autonomy={'done': Autonomy.Off})
# x:360 y:768
OperatableStateMachine.add('RetryDROP',
WaitState(wait_time=1),
transitions={'done': 'MoveToDROP'},
autonomy={'done': Autonomy.Off})
# x:743 y:522
OperatableStateMachine.add('AddYOffset',
AddNumericState(),
transitions={'done': 'CheckRow'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'SideYoffset',
'value_b': 'PlusOffset',
'result': 'SideYoffset'
})
# x:742 y:579
OperatableStateMachine.add('CheckRow',
EqualState(),
transitions={
'true': 'ResetRow',
'false': 'AddRound'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'Row_count',
'value_b': 'Round_Nr'
})
# x:741 y:647
OperatableStateMachine.add('ResetRow',
ReplaceState(),
transitions={'done': 'AddXOffset'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Zero',
'result': 'SideYoffset'
})
# x:739 y:711
OperatableStateMachine.add('AddXOffset',
AddNumericState(),
transitions={'done': 'AddRound'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'SideXoffset',
'value_b': 'PlusOffset',
'result': 'SideXoffset'
})
# x:156 y:768
OperatableStateMachine.add(
'RetryDropBin',
WaitState(wait_time=1),
transitions={'done': 'MoveToConveyorPart'},
autonomy={'done': Autonomy.Off})
# x:22 y:645
OperatableStateMachine.add('RetryHOME',
WaitState(wait_time=1),
transitions={'done': 'MoveToHOME'},
autonomy={'done': Autonomy.Off})
# x:371 y:28
OperatableStateMachine.add('StartConveryor',
SetConveyorbeltPowerState(),
transitions={
'continue': 'WAIT_check',
'fail': 'WAIT_failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'power'})
# x:203 y:26
OperatableStateMachine.add('GripperUIT_2',
UseGripper(enable=False),
transitions={
'continue': 'StartConveryor',
'failed': 'WAIT_failed',
'invalid_arm': 'WAIT_failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1300 y:89
OperatableStateMachine.add('SluitConveyorAF',
SetConveyorbeltPowerState(),
transitions={
'continue': 'failed',
'fail': 'WAIT_failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'NoPower'})
# x:1396 y:560
OperatableStateMachine.add('SluitConveyorAF_2',
SetConveyorbeltPowerState(),
transitions={
'continue': 'finished',
'fail': 'WAIT_completed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'NoPower'})
# x:368 y:311
OperatableStateMachine.add(
'GetBin3Pose',
GetObjectPoseState(object_frame='kit_tray_2',
ref_frame='arm1_linear_arm_actuator'),
transitions={
'continue': 'ComputeDrop',
'failed': 'WAIT_failed_drop'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'BIN3_pose'})
# x:361 y:443
OperatableStateMachine.add('ShowDropPose',
MessageState(),
transitions={'continue': 'MoveToDROP'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'BIN3_pose'})
# x:536 y:562
OperatableStateMachine.add(
'MoveToBin3_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'AddYOffset',
'planning_failed': 'WAIT_failed_drop',
'control_failed': 'WAIT_failed_drop',
'param_error': 'WAIT_failed_drop'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_R1PreBin3',
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out': 'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out': 'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
return _state_machine