class AbortState(smach.State):
def __init__(self, duration=1.0):
smach.State.__init__(
self,
outcomes=['succeeded'], #, 'failed'],
input_keys=['move_group']) # suctionState needs that one
self.suctionStateOff = SuctionState(state='off')
# ==========================================================
def execute(self, userdata):
# wait for the specified duration or until we are asked to preempt
rospy.loginfo("In the AbortState execution...")
self.suctionStateOff.execute(userdata)
# = SuctionState(state='off')
# if self.preempt_requested():
# else:
# return 'succeeded'
return 'succeeded'
def __init__(self, duration=1.0):
smach.State.__init__(
self,
outcomes=['succeeded'], #, 'failed'],
input_keys=['move_group']) # suctionState needs that one
self.suctionStateOff = SuctionState(state='off')
def execute(self, userdata):
# try to call the service
# !! warning, this could potentially block forever
try:
self.suctionState = SuctionState()
velocity_scale = Float32()
velocity_scale.data = self.velocity_scaling_factor
max_attempts = Int32()
max_attempts.data = self.max_attempts
planning_time = Float32()
planning_time.data = self.planning_time
#
# if self.movegroup is None:
# rospy.logerr("Parameter movegroup is not specified. Aborting robot motion.")
# return 'aborted'
# goal_frame_id = userdata['goal_frame_id']
goal_pose_array = userdata['goal_pose_array']
pre_grasp_pose_array = userdata['pre_grasp_pose_array']
move_group_name_array = userdata['move_group_name_array']
# rospy.loginfo('Trying to move robot\'s %s. Goal frame is %s' %
# (self.movegroup, goal_frame_id))
# rospy.loginfo('goal pose: %s' % goal_pose)
# print self.tf.lookupTransform(goal_frame_id, '/base', 0)
# movegroup=String()
# movegroup.data=self.movegroup
for i in range(len(goal_pose_array.poses)):
grasp_pose_stamped = PoseStamped()
grasp_pose_stamped.pose = goal_pose_array.poses[i]
grasp_pose_stamped.header.frame_id = goal_pose_array.header.frame_id
grasp_pose_stamped.header.stamp = rospy.Time.now()
pre_grasp_pose_stamped = PoseStamped()
pre_grasp_pose_stamped.pose = pre_grasp_pose_array.poses[i]
pre_grasp_pose_stamped.header.frame_id = goal_pose_array.header.frame_id
pre_grasp_pose_stamped.header.stamp = rospy.Time.now()
movegroup = String()
movegroup.data = move_group_name_array[i]
rospy.loginfo("[GraspState] Trying to move robot\'s '%s'." %
movegroup.data)
# print '\n--------------------The pose is:'
# print grasp_pose_stamped.pose
# print '---------------------\n'
# turn the valve to the correct sucker
self.activateValve(movegroup)
# TERRIBLE HACK TO HELP WITH THE WRONG SUCKER GRABBING ONTO THE WRONG ITEM
rospy.sleep(1)
# turn the pump on!
self.suctionState.activatePump()
# Use Standard Path
print 'Attempting to move to pre grasp and grasp pose: ' #, pre_grasp_pose_stamped
# Plan in joint space
# move_to_pre_grasp_pose_resp = self.service.call(movegroup, pre_grasp_pose_stamped, velocity_scale, max_attempts,planning_time)
# Plan in cartesian space
cartesian_pose_array = PoseArray()
cartesian_pose_array.header.frame_id = pre_grasp_pose_stamped.header.frame_id
cartesian_pose_array.header.stamp = rospy.Time.now()
cartesian_pose_array.poses.append(pre_grasp_pose_stamped.pose)
cartesian_pose_array.poses.append(grasp_pose_stamped.pose)
movegroup_cartesian = deepcopy(movegroup)
movegroup_cartesian.data = movegroup_cartesian.data + '_cartesian'
velocity_scale_grasp = Float32()
velocity_scale_grasp.data = 1.0
min_path_completion = Float32()
min_path_completion.data = 0.25
self.execute_unfill_bins(
) # Turn off the shelf collision model
move_in_resp = self.cartesian_service.call(
movegroup_cartesian, cartesian_pose_array,
velocity_scale_grasp, min_path_completion, max_attempts,
planning_time)
# if(move_to_pre_grasp_pose_resp.success.data == True):
# print 'Attempting to grasp object at pose: ' #, grasp_pose_stamped
# # response = self.service.call(movegroup, grasp_pose_stamped, velocity_scale, max_attempts,planning_time)
#
#
#
# # Use Cartesian Path - need to add in check for 100% complete though
# cartesian_pose_array = PoseArray()
# cartesian_pose_array.header.frame_id = grasp_pose_stamped.header.frame_id
# cartesian_pose_array.header.stamp = rospy.Time.now()
# cartesian_pose_array.poses.append(grasp_pose_stamped.pose)
# movegroup_cartesian = deepcopy(movegroup)
# movegroup_cartesian.data = movegroup_cartesian.data + '_cartesian'
# velocity_scale_grasp = Float32()
# velocity_scale_grasp.data = 1.0
# # move_in_resp = self.cartesian_service.call(movegroup_cartesian, cartesian_pose_array, velocity_scale,min_path_completion)
# move_in_resp = self.cartesian_service.call(movegroup_cartesian, cartesian_pose_array, velocity_scale_grasp,min_path_completion)
if move_in_resp.success.data == True:
# Fill in goal_pose with the pick pose that worked so that next up relative move works
userdata['goal_pose'] = grasp_pose_stamped
userdata['goal_pose_array'] = []
userdata[
'move_group'] = movegroup.data ## To keep consistent with main state maching interface
return 'succeeded'
else:
self.suctionState.deactivatePump()
except Exception, e:
print e
rospy.logerr(
'Service call to move_robot_pose failed. Reason: \n %s' % e)
return 'aborted'
class GraspObjectState(smach.State):
def __init__(self, movegroup=None, velocity_scale=1.0):
smach.State.__init__(
self,
outcomes=['succeeded', 'aborted', 'failed'],
input_keys=[
'next_item_to_pick', 'goal_frame_id', 'goal_pose_array',
'pre_grasp_pose_array', 'move_group_name_array'
],
output_keys=['goal_pose', 'goal_pose_array', 'move_group'])
# if movegroup is None:
# rospy.logwarn("Parameter movegroup not specified. GraspObjectState instance will not execute.")
# self.movegroup = movegroup
self.velocity_scaling_factor = velocity_scale
self.max_attempts = 1
self.planning_time = 0.5
# wait for the service to appear
rospy.loginfo('Waiting for service move_robot_pose to come online ...')
try:
rospy.wait_for_service('/moveit_lib/move_robot_pose', timeout=0.1)
except:
rospy.logerr('Service %s not available. Restart and try again.' %
'/moveit_lib/move_robot_pose')
# rospy.signal_shutdown('')
# create a proxy to that service
self.service = rospy.ServiceProxy('/moveit_lib/move_robot_pose',
moveit_lib.srv.move_robot_pose)
self.cartesian_service = rospy.ServiceProxy(
'/moveit_lib/move_robot_pose_array',
moveit_lib.srv.move_robot_pose_array)
self.named_service = rospy.ServiceProxy(
'/moveit_lib/move_robot_named', moveit_lib.srv.move_robot_named)
self.tf_listener = tf.TransformListener()
self.pumpPublisher = rospy.Publisher(
'/robot/end_effector/right_gripper/command',
EndEffectorCommand,
queue_size=1)
self.valvePublisher = rospy.Publisher(
'/robot/end_effector/left_gripper/command',
EndEffectorCommand,
queue_size=1)
# ==========================================================
def execute(self, userdata):
# try to call the service
# !! warning, this could potentially block forever
try:
self.suctionState = SuctionState()
velocity_scale = Float32()
velocity_scale.data = self.velocity_scaling_factor
max_attempts = Int32()
max_attempts.data = self.max_attempts
planning_time = Float32()
planning_time.data = self.planning_time
#
# if self.movegroup is None:
# rospy.logerr("Parameter movegroup is not specified. Aborting robot motion.")
# return 'aborted'
# goal_frame_id = userdata['goal_frame_id']
goal_pose_array = userdata['goal_pose_array']
pre_grasp_pose_array = userdata['pre_grasp_pose_array']
move_group_name_array = userdata['move_group_name_array']
# rospy.loginfo('Trying to move robot\'s %s. Goal frame is %s' %
# (self.movegroup, goal_frame_id))
# rospy.loginfo('goal pose: %s' % goal_pose)
# print self.tf.lookupTransform(goal_frame_id, '/base', 0)
# movegroup=String()
# movegroup.data=self.movegroup
for i in range(len(goal_pose_array.poses)):
grasp_pose_stamped = PoseStamped()
grasp_pose_stamped.pose = goal_pose_array.poses[i]
grasp_pose_stamped.header.frame_id = goal_pose_array.header.frame_id
grasp_pose_stamped.header.stamp = rospy.Time.now()
pre_grasp_pose_stamped = PoseStamped()
pre_grasp_pose_stamped.pose = pre_grasp_pose_array.poses[i]
pre_grasp_pose_stamped.header.frame_id = goal_pose_array.header.frame_id
pre_grasp_pose_stamped.header.stamp = rospy.Time.now()
movegroup = String()
movegroup.data = move_group_name_array[i]
rospy.loginfo("[GraspState] Trying to move robot\'s '%s'." %
movegroup.data)
# print '\n--------------------The pose is:'
# print grasp_pose_stamped.pose
# print '---------------------\n'
# turn the valve to the correct sucker
self.activateValve(movegroup)
# TERRIBLE HACK TO HELP WITH THE WRONG SUCKER GRABBING ONTO THE WRONG ITEM
rospy.sleep(1)
# turn the pump on!
self.suctionState.activatePump()
# Use Standard Path
print 'Attempting to move to pre grasp and grasp pose: ' #, pre_grasp_pose_stamped
# Plan in joint space
# move_to_pre_grasp_pose_resp = self.service.call(movegroup, pre_grasp_pose_stamped, velocity_scale, max_attempts,planning_time)
# Plan in cartesian space
cartesian_pose_array = PoseArray()
cartesian_pose_array.header.frame_id = pre_grasp_pose_stamped.header.frame_id
cartesian_pose_array.header.stamp = rospy.Time.now()
cartesian_pose_array.poses.append(pre_grasp_pose_stamped.pose)
cartesian_pose_array.poses.append(grasp_pose_stamped.pose)
movegroup_cartesian = deepcopy(movegroup)
movegroup_cartesian.data = movegroup_cartesian.data + '_cartesian'
velocity_scale_grasp = Float32()
velocity_scale_grasp.data = 1.0
min_path_completion = Float32()
min_path_completion.data = 0.25
self.execute_unfill_bins(
) # Turn off the shelf collision model
move_in_resp = self.cartesian_service.call(
movegroup_cartesian, cartesian_pose_array,
velocity_scale_grasp, min_path_completion, max_attempts,
planning_time)
# if(move_to_pre_grasp_pose_resp.success.data == True):
# print 'Attempting to grasp object at pose: ' #, grasp_pose_stamped
# # response = self.service.call(movegroup, grasp_pose_stamped, velocity_scale, max_attempts,planning_time)
#
#
#
# # Use Cartesian Path - need to add in check for 100% complete though
# cartesian_pose_array = PoseArray()
# cartesian_pose_array.header.frame_id = grasp_pose_stamped.header.frame_id
# cartesian_pose_array.header.stamp = rospy.Time.now()
# cartesian_pose_array.poses.append(grasp_pose_stamped.pose)
# movegroup_cartesian = deepcopy(movegroup)
# movegroup_cartesian.data = movegroup_cartesian.data + '_cartesian'
# velocity_scale_grasp = Float32()
# velocity_scale_grasp.data = 1.0
# # move_in_resp = self.cartesian_service.call(movegroup_cartesian, cartesian_pose_array, velocity_scale,min_path_completion)
# move_in_resp = self.cartesian_service.call(movegroup_cartesian, cartesian_pose_array, velocity_scale_grasp,min_path_completion)
if move_in_resp.success.data == True:
# Fill in goal_pose with the pick pose that worked so that next up relative move works
userdata['goal_pose'] = grasp_pose_stamped
userdata['goal_pose_array'] = []
userdata[
'move_group'] = movegroup.data ## To keep consistent with main state maching interface
return 'succeeded'
else:
self.suctionState.deactivatePump()
except Exception, e:
print e
rospy.logerr(
'Service call to move_robot_pose failed. Reason: \n %s' % e)
return 'aborted'
# if move_in_resp.success.data == True:
# return 'succeeded'
return 'failed'
# move the object on top of the tote to drop it
sm.add('move_to_drop_pose', MoveRobotToNamedPose(movegroup='left_arm',
goal_pose_name='left_tote'),
transitions={'succeeded':'deactivate_suction',
'aborted': 'move_to_neutral_1',
'failed': 'move_to_neutral_1'})
sm.add('move_to_neutral_1', MoveRobotToNamedPose(movegroup='left_arm',
goal_pose_name='left_neutral'),
transitions={'succeeded':'deactivate_suction',
'failed': 'temp_remove_object_collision'})
sm.add('temp_remove_object_collision', UpdateCollisionState(action='detach'),
transitions={'succeeded':'abort_state'})
sm.add('deactivate_suction', SuctionState(state='off', movegroup=None),
transitions={'succeeded':'remove_object_collision', 'failed':'abort_state'})
sm.add('remove_object_collision', UpdateCollisionState(action='detach'),
transitions={'succeeded':'remove_current_pick'})
sm.add('remove_current_pick', PopItemState(),
transitions={'succeeded':'move_to_neutral'})
sm.add('move_to_neutral', MoveRobotToNamedPose(movegroup='left_arm',
goal_pose_name='left_neutral'),
transitions={'succeeded':'decide_next_pick',
'failed': 'abort_state'})
sm_grasp.add('turn_lip_off_failed',
ToggleBinFillersAndTote(action='lip_off'),
transitions={
'succeeded': 'track_number_of_grasp_attempts',
'failed': 'turn_lip_off_failed'
})
sm_grasp.add('check_pressure_sensor',
CheckPressureSensorState(),
transitions={
'succeeded': 'move_object_back_out',
'failed': 'pressure_failed_suction_off'
})
sm_grasp.add('pressure_failed_suction_off',
SuctionState(state='off', movegroup=None),
transitions={
'succeeded': 'move_back_out_no_object',
'failed': 'track_number_of_grasp_attempts'
})
sm_grasp.add('track_number_of_grasp_attempts',
TrackItemGraspAttemptsState(),
transitions={
'try_this_item_again': 'perception',
'move_on_to_next_item': 'abort_next_object'
})
sm_move_tote = smach.StateMachine(
outcomes=['repeat', 'perception', 'finished_next_object'],
input_keys=['next_item_to_pick', 'move_group'])