def InspectAfterPicking():
# Create a SMACH state machine
InspectAfterPicking = ppv.StateMachine(outcomes=['TIMED_OUT', 'SUCCEEDED'])
InspectAfterPicking.userdata.sm_x = []
InspectAfterPicking.userdata.sm_y = []
InspectAfterPicking.userdata.sm_z = []
InspectAfterPicking.userdata.sm_color = []
InspectAfterPicking.userdata.sm_counter = 0
try:
# Open the container
with InspectAfterPicking:
ppv.StateMachine.add('VIEW',
ppv.View(),
transitions={
'success': 'SUCCEEDED',
'failed': 'VIEW',
'timed_out': 'TIMED_OUT'
},
remapping={
'color': 'sm_color',
'counter': 'sm_counter'
})
# Execute SMACH plan
outcome_inspect = InspectAfterPicking.execute()
return outcome_inspect
except ppv.rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
def ClaimNewOnion():
# Create a SMACH state machine
ClaimNewOnion = ppv.StateMachine(
outcomes=['TIMED_OUT', 'SUCCEEDED', 'SORT COMPLETE'])
ClaimNewOnion.userdata.sm_x = []
ClaimNewOnion.userdata.sm_y = []
ClaimNewOnion.userdata.sm_z = []
ClaimNewOnion.userdata.sm_color = []
ClaimNewOnion.userdata.sm_counter = 0
try:
# Open the container
with ClaimNewOnion:
# Add states to the container
ppv.StateMachine.add('GETINFO',
ppv.Get_info_w_check(),
transitions={
'updated': 'CLAIM',
'not_updated': 'GETINFO',
'timed_out': 'TIMED_OUT',
'completed': 'SORT COMPLETE'
},
remapping={
'x': 'sm_x',
'y': 'sm_y',
'z': 'sm_z',
'color': 'sm_color',
'counter': 'sm_counter'
})
ppv.StateMachine.add('CLAIM',
ppv.Claim(),
transitions={
'updated': 'SUCCEEDED',
'not_updated': 'CLAIM',
'timed_out': 'TIMED_OUT',
'not_found': 'GETINFO',
'move': 'GETINFO',
'completed': 'SORT COMPLETE'
},
remapping={
'x': 'sm_x',
'y': 'sm_y',
'z': 'sm_z',
'color': 'sm_color',
'counter': 'sm_counter'
})
# Execute SMACH plan
outcome_claim = ClaimNewOnion.execute()
# print '\nClaim outcome is: ', outcome_claim
# ppv.rospy.sleep(100)
return outcome_claim
except ppv.rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
def PlaceOnConveyor():
# Create a SMACH state machine
PlaceOnConveyor = ppv.StateMachine(outcomes=['TIMED_OUT', 'SUCCEEDED'])
PlaceOnConveyor.userdata.sm_x = []
PlaceOnConveyor.userdata.sm_y = []
PlaceOnConveyor.userdata.sm_z = []
PlaceOnConveyor.userdata.sm_color = []
PlaceOnConveyor.userdata.sm_counter = 0
try:
# Open the container
with PlaceOnConveyor:
ppv.StateMachine.add('PLACEONCONVEYOR',
ppv.Placeonconveyor(),
transitions={
'success': 'DETACH',
'failed': 'PLACEONCONVEYOR',
'timed_out': 'TIMED_OUT'
},
remapping={
'color': 'sm_color',
'counter': 'sm_counter'
})
ppv.StateMachine.add('DETACH',
ppv.Detach_object(),
transitions={
'success': 'SUCCEEDED',
'failed': 'DETACH',
'timed_out': 'TIMED_OUT'
},
remapping={'counter': 'sm_counter'})
# ppv.StateMachine.add('LIFTUP', ppv.Liftup(),
# transitions={'success': 'SUCCEEDED',
# 'failed':'LIFTUP',
# 'timed_out': 'TIMED_OUT'},
# remapping={'counter':'sm_counter'})
# Execute SMACH plan
outcome_place = PlaceOnConveyor.execute()
return outcome_place
except ppv.rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
def PlaceInBin():
# Create a SMACH state machine
PlaceInBin = ppv.StateMachine(outcomes=['TIMED_OUT', 'SUCCEEDED'])
PlaceInBin.userdata.sm_x = []
PlaceInBin.userdata.sm_y = []
PlaceInBin.userdata.sm_z = []
PlaceInBin.userdata.sm_color = []
PlaceInBin.userdata.sm_counter = 0
try:
# Open the container
with PlaceInBin:
ppv.StateMachine.add('PLACEINBIN',
ppv.Placeinbin(),
transitions={
'success': 'DETACH',
'failed': 'PLACEINBIN',
'timed_out': 'TIMED_OUT'
},
remapping={
'color': 'sm_color',
'counter': 'sm_counter'
})
ppv.StateMachine.add('DETACH',
ppv.Detach_object(),
transitions={
'success': 'SUCCEEDED',
'failed': 'DETACH',
'timed_out': 'TIMED_OUT'
},
remapping={'counter': 'sm_counter'})
# Execute SMACH plan
outcome_place = PlaceInBin.execute()
return outcome_place
except ppv.rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
def Pick():
# Create a SMACH state machine
Pick = ppv.StateMachine(outcomes=['TIMED_OUT', 'SUCCEEDED', 'FAILED'])
Pick.userdata.sm_x = []
Pick.userdata.sm_y = []
Pick.userdata.sm_z = []
Pick.userdata.sm_color = []
Pick.userdata.sm_counter = 0
try:
# Open the container
with Pick:
ppv.StateMachine.add('APPROACH',
ppv.Approach(),
transitions={
'success': 'PICK',
'failed': 'APPROACH',
'timed_out': 'TIMED_OUT'
},
remapping={
'x': 'sm_x',
'y': 'sm_y',
'z': 'sm_z',
'color': 'sm_color',
'counter': 'sm_counter'
})
ppv.StateMachine.add('PICK',
ppv.Dipdown(),
transitions={
'success': 'GRASP',
'failed': 'PICK',
'timed_out': 'TIMED_OUT'
},
remapping={
'x': 'sm_x',
'y': 'sm_y',
'z': 'sm_z',
'color': 'sm_color',
'counter': 'sm_counter'
})
ppv.StateMachine.add('GRASP',
ppv.Grasp_object(),
transitions={
'success': 'LIFTUP',
'failed': 'GRASP',
'timed_out': 'TIMED_OUT'
},
remapping={
'x': 'sm_x',
'y': 'sm_y',
'z': 'sm_z',
'color': 'sm_color',
'counter': 'sm_counter'
})
ppv.StateMachine.add('LIFTUP',
ppv.Liftup(),
transitions={
'success': 'SUCCEEDED',
'failed': 'LIFTUP',
'no_grasp': 'FAILED',
'timed_out': 'TIMED_OUT'
},
remapping={'counter': 'sm_counter'})
# Execute SMACH plan
outcome_pick = Pick.execute()
return outcome_pick
except ppv.rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
