def execute(self, userdata):
suction.set_suck_level(0)
suction.pump_off()
gripper.open_gripper()
rospy.sleep(0.5)
userdata.data['move_objects_mode'] = False
userdata.data['move_objects_between_bins_mode'] = None
rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT')
curr = t.current_robot_pose('global_xyz_link', 'realsense_endpoint')
# Don't hit the top bar.
if curr.position.y >= TOOL_CHANGE_SAFE_Y:
curr.position.y = TOOL_CHANGE_SAFE_Y
# Don't hit the down bars.
if curr.position.y >= TOOL_CHANGE_DANGEROUS_Y:
# We might hit the front frame so move away from it.
if curr.position.x >= TOOL_CHANGE_X_MAX:
curr.position.x = TOOL_CHANGE_X_MAX
if curr.position.x <= TOOL_CHANGE_X_MIN:
curr.position.x = TOOL_CHANGE_X_MIN
res = m.move_to_global(curr.position.x,
min(curr.position.y, TOOL_CHANGE_SAFE_Y),
TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense')
if not res:
return 'failed'
rospy.loginfo('CHANGE TO NEUTRAL WRIST NEUTRAL')
res = m.move_to_named_pose('wrist_only', 'neutral')
if not res:
return 'failed'
return 'succeeded'
def execute(self, userdata):
first_attamept = True
label = userdata.data['item_to_pick']['label']
suck_level = item_meta[label].get('suction_pressure', 3)
set_suck_level(suck_level)
USE_ANGLED = False
position_limits = POSITION_LIMITS[
userdata.data['picking_from']]['sucker']
# Get the tote weight before the pick
scales_topic = userdata.data['picking_from'] + '_scales/weight'
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_type'] != 'grip_suck':
# If we've just tried sucking, then don't re-weigh
try:
weight = rospy.wait_for_message(scales_topic, Int16, 2.0)
userdata.data['weight_before'] = weight.data
except:
rospy.logerr('Unable to read weight from %s' % scales_topic)
userdata.data['weight_before'] = -1
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_point_type'] == 'rgb_centroid':
rospy.logerr(
"Aborting RGB Centroid pick because there aren't any scales."
)
return 'suck_failed'
for object_global in userdata.data['item_to_pick']['grasp_poses']:
USE_ANGLED = decide_angled(object_global, position_limits)
object_global.position.x += 0.015
object_global.position.y -= 0.03
if USE_ANGLED:
rospy.logerr('ANGLED PICK')
# Make sure we're inside the tote.
edge_offset = 0.0
object_global.position.x = max(
object_global.position.x,
position_limits['x_min'] - edge_offset)
object_global.position.x = min(
object_global.position.x,
position_limits['x_max'] + edge_offset)
object_global.position.y = max(
object_global.position.y,
position_limits['y_min'] - edge_offset)
object_global.position.y = min(
object_global.position.y,
position_limits['y_max'] + edge_offset)
object_global.position.z -= 0.02 # The endpoint is half way through the suction cup, so move up slightly
#t.publish_pose_as_transform(object_global, 'global_xyz_link', 'ORIGINAL', seconds=2)
translation = [
object_global.position.x, object_global.position.y,
object_global.position.z
]
qm = object_global.orientation
q = (qm.x, qm.y, qm.z, qm.w)
R = tft.quaternion_matrix(q)
P = tft.projection_matrix([0, 0, 0], [0, 0, 1]) # XY plane
pose_vec = R[:, 2] # Z portion of matrix.
proj_vec = np.dot(
P,
pose_vec)[:3] # Projected onto XY plane as a unit vector.
proj_vec_unit = tft.unit_vector(proj_vec)
y_unit = [0, 1, 0] # Unit vector in the y-direction.
yaw_angle = np.arccos(np.dot(
proj_vec_unit, y_unit)) # Angle to the grasp around Z.
pitch_angle = np.arcsin(
tft.vector_norm(proj_vec)) # Angle to tilt the suction cup
if pitch_angle > 1.2:
pitch_angle = 1.2
# Only get absolute angle from above. Check the direction of the vector.
if R[0, 2] > 0:
yaw_angle = -1 * yaw_angle
# Create a quaternion with the right angles (note rotations are applied in the rotated frame in zyx order)
q = tft.quaternion_from_euler(yaw_angle, 0, -1 * pitch_angle,
'rzyx')
if yaw_angle > 1.571 or yaw_angle < -1.571:
# Rotate by 180 to normalise to the sucker workspace.
q2 = tft.quaternion_from_euler(0, 0, 3.1415)
q = tft.quaternion_multiply(q, q2)
e = list(tft.euler_from_quaternion(q))
if e[2] > 1.5: # Stop gimbal lock and weird joint configurations.
e[2] = 1.5
if e[2] < -1.5:
e[2] = -1.5
q = tft.quaternion_from_euler(e[0], e[1], e[2])
object_global.orientation.x = q[0]
object_global.orientation.y = q[1]
object_global.orientation.z = q[2]
object_global.orientation.w = q[3]
R = tft.quaternion_matrix(q)
t.publish_tf_quaterion_as_transform(translation,
q,
'global_xyz_link',
'MOVE_HERE',
seconds=0.5)
# Create a pre-grasp pose away from the object.
T = tft.translation_matrix(translation)
T2 = tft.translation_matrix((0, 0, -0.05))
F = tft.concatenate_matrices(T, R, T2)
pre_grasp_t = tft.translation_from_matrix(F)
pre_grasp_q = tft.quaternion_from_matrix(F)
#t.publish_tf_quaterion_as_transform(pre_grasp_t, pre_grasp_q, 'global_xyz_link', 'PRE', seconds=5.0)
grasp_orientation = Quaternion()
grasp_orientation.x = q[0]
grasp_orientation.y = q[1]
grasp_orientation.z = q[2]
grasp_orientation.w = q[3]
rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT')
if first_attamept:
# Move the whole wrist and then select the sucker.
res = m.move_to_global(
min(object_global.position.x, REALSENSE_MAX_GLOBAL_X),
max(
min(object_global.position.y,
SUCKER_MAX_Y_STRAIGHT) - 0.02,
SUCKER_MIN_GLOBAL_Y + 0.02) -
SUCKER_REALSENSE_Y_OFFSET,
TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense')
if not res:
return 'failed'
rospy.loginfo('SELECTING SUCKER')
res = m.move_to_named_pose('wrist_only', 'sucker')
if not res:
return 'failed'
first_attamept = False
else:
# Straighten up the sucker from the last attempt and move into position
straight_q = Quaternion()
straight_q.x = 0
straight_q.y = 0
straight_q.z = 0
straight_q.w = 1
res = m.move_to_global(
min(object_global.position.x, SUCKER_MAX_GLOBAL_X),
max(
min(object_global.position.y,
SUCKER_MAX_Y_STRAIGHT) - 0.02,
SUCKER_MIN_GLOBAL_Y + 0.02),
TOOL_CHANGE_REALSENSE_GLOBAL_Z + 0.35,
'sucker',
orientation=straight_q)
if not res:
return 'failed'
rospy.loginfo("MOVE TO PRE-GRASP")
res = m.move_to_global(pre_grasp_t[0],
pre_grasp_t[1],
min(pre_grasp_t[2],
SUCKER_MAX_GLOBAL_Z),
'sucker',
orientation=grasp_orientation,
velo_scale=0.4)
if not res:
return 'failed'
rospy.loginfo("PUMP ON, MOVING TO OBJECT")
suction.pump_on()
(res, stopped,
bail) = m.move_to_global_monitor_weight_and_suction(
object_global.position.x,
object_global.position.y,
min(object_global.position.z, SUCKER_MAX_GLOBAL_Z),
'sucker',
scales_topic,
orientation=grasp_orientation,
velo_scale=0.3)
if not res:
return 'failed'
else:
# Straight Down Suck
rospy.logerr('STRAIGHT PICK')
# Make sure we're inside the tote.
edge_offset = 0.0
object_global.position.x = max(
object_global.position.x,
position_limits['x_min'] - edge_offset)
object_global.position.x = min(
object_global.position.x,
position_limits['x_max'] + edge_offset)
object_global.position.y = max(
object_global.position.y,
position_limits['y_min'] - edge_offset)
object_global.position.y = min(
object_global.position.y,
position_limits['y_max'] + edge_offset)
object_global.position.z = min(object_global.position.z,
position_limits['z_max'])
t.publish_pose_as_transform(object_global,
'global_xyz_link',
'MOVE_HERE',
seconds=0.5)
rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT')
# We only need to select the sucker on the first attempt.
# Can cause it to break becasue the controllers don't switch properly
# if there's no actual movment and we change controllers too quickly. Not sure why, something to do with timing.
if first_attamept:
res = m.move_to_global(
object_global.position.x,
max(
min(object_global.position.y,
SUCKER_MAX_Y_STRAIGHT),
SUCKER_MIN_Y_STRAIGHT) - SUCKER_REALSENSE_Y_OFFSET,
TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense')
if not res:
return 'failed'
rospy.loginfo('SELECTING SUCKER')
res = m.move_to_named_pose('wrist_only', 'sucker')
if not res:
return 'failed'
first_attamept = False
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_type'] == 'grip_suck':
# Weird async movement bug.
rospy.sleep(1.0)
else:
straight_q = Quaternion()
straight_q.x = 0
straight_q.y = 0
straight_q.z = 0
straight_q.w = 1
res = m.move_to_global(
object_global.position.x,
max(
min(object_global.position.y,
SUCKER_MAX_Y_STRAIGHT), SUCKER_MIN_Y_STRAIGHT),
TOOL_CHANGE_REALSENSE_GLOBAL_Z + 0.35,
'sucker',
orientation=straight_q)
if not res:
return 'failed'
# Correct the rotation if required.
pca_yaw = userdata.data['item_to_pick']['pca_yaw']
q = (pca_yaw.x, pca_yaw.y, pca_yaw.z, pca_yaw.w)
e = list(tft.euler_from_quaternion(q))
if e[2] > 1.57:
e[2] -= 3.14
if e[2] < -1.57:
e[2] += 3.14
if e[2] > 1.5: # Stop gimbal lock and weird joint configurations.
e[2] = 1.5
if e[2] < -1.5:
e[2] = -1.5
q = tft.quaternion_from_euler(0, 0, e[2])
pca_yaw.x = q[0]
pca_yaw.y = q[1]
pca_yaw.z = q[2]
pca_yaw.w = q[3]
rospy.loginfo("PUMP ON, MOVING TO OBJECT")
suction.pump_on()
v_scale = 0.25
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_point_type'] == 'rgb_centroid':
v_scale = 0.1
(res, stopped,
bail) = m.move_to_global_monitor_weight_and_suction(
object_global.position.x,
min(object_global.position.y, SUCKER_MAX_Y_STRAIGHT),
min(object_global.position.z, SUCKER_MAX_GLOBAL_Z),
'sucker',
scales_topic,
orientation=pca_yaw,
velo_scale=v_scale)
if not res:
return 'failed'
if bail:
m.move_relative(0, 0, -0.02, 'sucker_endpoint', 'sucker')
rospy.sleep(0.5)
elif not suction.check_pressure_sensor():
# If it's not already on, give it a sec.
rospy.sleep(1.0)
suction_state = suction.check_pressure_sensor(
) #query pressure sensor for state, has the object been sucked correctly
rospy.loginfo("PRESSURE SENSOR STATUS: %s" % suction_state)
if not suction_state and not bail:
set_suck_level(3)
# Just move down a touch and double check
move_amount = 0.02
if USE_ANGLED:
move_amount = 0.035
if object_global.position.z < (SUCKER_MAX_GLOBAL_Z):
move_amount = min(
move_amount,
SUCKER_MAX_GLOBAL_Z - object_global.position.z)
res = m.move_relative(0,
0,
move_amount,
'sucker_endpoint',
'sucker',
plan_time=0.5)
else:
res = True
if res:
rospy.sleep(1.0)
suction_state = suction.check_pressure_sensor()
if suction_state:
#userdata.data['last_failed'] = None
return 'succeeded'
else:
suction.pump_off()
# Run out of grasp poses.
suction.pump_off()
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_type'] == 'suck_grip':
return 'try_gripper'
userdata.data['last_failed'][userdata.data['item_to_pick']
['label']] = 0
return 'suck_failed'
def execute(self, userdata):
# Only do it once.
move_objects_mode = userdata.data['move_objects_mode']
userdata.data['move_objects_mode'] = False
rospy.loginfo('LIFTING OBJECT WITH SUCKER')
curr = t.current_robot_pose('global_xyz_link', 'sucker_endpoint')
# Update current pose with new orientation.
curr.position.x = min(curr.position.x, SUCKER_MAX_GLOBAL_X - 0.01)
curr.position.y = min(curr.position.y, SUCKER_MAX_Y_STRAIGHT)
curr.position.y = max(curr.position.y, SUCKER_MIN_GLOBAL_Y + 0.01)
suck_height = curr.position.z
curr.position.z = SUCKER_LIFT_HEIGHT
#t.publish_pose_as_transform(curr, 'global_xyz_link', 'MOVE HERE', 1.0)
co = curr.orientation
q = [co.x, co.y, co.z, co.w]
e = tft.euler_from_quaternion(q)
q = tft.quaternion_from_euler(0, 0, e[2])
qm = gmsg.Quaternion()
qm.x = 0
qm.y = 0
qm.z = 0
qm.w = 1
v_scale = 0.3
if item_meta[userdata.data['item_to_pick']['label']]['weight'] > 0.5:
# Heavy Item
v_scale = 0.15
res = m.move_to_global(curr.position.x, curr.position.y, curr.position.z, 'sucker', orientation=qm, velo_scale=v_scale)
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
if not suction.check_pressure_sensor():
userdata.data['last_failed'][userdata.data['item_to_pick']['label']] = 0
return 'suck_failed'
scales_topic = userdata.data['picking_from'] + '_scales/weight'
if move_objects_mode and userdata.data['move_objects_between_bins_mode'] is None:
mid_x, mid_y = POSITION_LIMITS[userdata.data['picking_from']]['centre']
if curr.position.x > mid_x:
curr.position.x -= 0.15
else:
curr.position.x += 0.15
if curr.position.y > mid_y:
curr.position.y -= 0.2
else:
curr.position.y += 0.2
res, _, _ = m.move_to_global_monitor_weight(curr.position.x, curr.position.y, suck_height - 0.13, 'sucker', scales_topic, velo_scale=0.2)
return 'suck_failed'
if userdata.data['weight_before'] < 0:
rospy.logerr('No pre-pick weight, ignoring weight check')
return 'succeeded'
try:
weight = rospy.wait_for_message(scales_topic, Int16)
except:
rospy.logerr('Failed to get weight from %s, ignoring weight check')
return 'succeeded'
userdata.data['weight_after'] = weight.data
dw = userdata.data['weight_before'] - weight.data
if item_meta[userdata.data['item_to_pick']['label']].get('weight')*1000.0 < 5:
# We can't do a weight check.
return 'succeeded'
if userdata.data['weight_failures'].get(userdata.data['item_to_pick']['label'], 0) > 3 and (len(userdata.data['visible_objects']) < 6 or userdata.data.get('i_think_im_done')):
# We've failed weight check on this item heaps of times and we're almost finished so just pick it.
rospy.logerr('I am ignoring the weight check since it has failed too many times.')
return 'succeeded'
if dw < 4:
# We haven't got anything, go back to the camera position and try again.
rospy.logerr('No detected change in weight, failed grasp.')
userdata.data['last_failed'][userdata.data['item_to_pick']['label']] = 0
return 'suck_failed'
if item_meta[userdata.data['item_to_pick']['label']].get('ignore_weight') == True:
return 'succeeded'
weight_correct, possible_matches = w.weight_matches(userdata.data['item_to_pick']['label'], dw, userdata.data['visible_objects'], 3)
if not weight_correct:
rospy.logerr(possible_matches)
if userdata.data.get('i_think_im_done', False) == True:
# Don't do weight reclassifications at the end.
pass
elif userdata.data['move_objects_between_bins_mode'] is not None:
pass
elif len(possible_matches) == 1 and possible_matches[0][0] < 5:
# There's only one thing it could actually be!
if userdata.data['task'] == 'stow' or (userdata.data['task'] == 'pick' and possible_matches[0][1] in userdata.data['wanted_items']):
rospy.logerr("I'M CHANGING THE OBJECT TO %s" % possible_matches[0][1])
userdata.data['weight_reclassifications'].append( (userdata.data['item_to_pick']['label'], possible_matches[0][1]) )
userdata.data['item_to_pick']['label'] = possible_matches[0][1]
suck_level = item_meta[possible_matches[0][1]].get('suction_pressure', 3)
suction.set_suck_level(suck_level)
return 'succeeded'
elif len(possible_matches) > 1 and userdata.data['task'] == 'stow':
userdata.data['double_check_objects'] = [i[1] for i in possible_matches]
suck_pos = t.current_robot_pose('global_xyz_link', 'sucker_endpoint')
suck_pos.position.z = suck_height
userdata.data['original_pick_position'] = suck_pos
return 'secondary_check'
# Put the item back down and bail.
userdata.data['last_failed'][userdata.data['item_to_pick']['label']] = 0
if userdata.data['item_to_pick']['label'] not in userdata.data['weight_failures']:
userdata.data['weight_failures'][userdata.data['item_to_pick']['label']] = 1
else:
userdata.data['weight_failures'][userdata.data['item_to_pick']['label']] += 1
rospy.logerr("WEIGHT DOESN'T MATCH: MEASURED: %s, EXPECTED: %s" % (dw, item_meta[userdata.data['item_to_pick']['label']].get('weight')*1000.0))
# Move towards the centre.
mid_x, mid_y = POSITION_LIMITS[userdata.data['picking_from']]['centre']
if curr.position.x > mid_x:
curr.position.x -= 0.08
else:
curr.position.x += 0.08
if curr.position.y > mid_y:
curr.position.y -= 0.08
else:
curr.position.y += 0.08
res, _, _ = m.move_to_global_monitor_weight(curr.position.x, curr.position.y, suck_height - 0.13, 'sucker', scales_topic, velo_scale=0.1)
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
return 'suck_failed'
return 'succeeded'
def execute(self, userdata):
rospy.loginfo('MOVE TO DOUBLE CHECK POSITION')
res = m.move_to_global(0.579, 0.608, 0.630, 'sucker')
if not res:
return 'failed'
try:
img_msg = rospy.wait_for_message('/realsense/rgb_hd/image_raw',
Image, 5.0)
except rospy.ROSException:
# Don't reset vision, this is a different camera.
return 'classification_failed'
img = self.cb.imgmsg_to_cv2(img_msg, 'rgb8')
img = img[155:155 + 540, 340:340 + 960, :] # Crop
img = cv2.resize(img, (640, 360))
img_msg = self.cb.cv2_to_imgmsg(img)
visible_objects = userdata.data['double_check_objects']
rospy.loginfo(visible_objects)
visible_objects_ros = [String(s) for s in visible_objects]
visible_objects_ros.append(String('tote'))
class_res = self.refinenet_segmentation_service.call(
visible_objects_ros, img_msg, img_msg, img_msg)
self.overlay_pub.publish(class_res.overlay_img)
labels = [l.data for l in class_res.label]
segment_certainties = [i.data for i in class_res.segment_certainties]
print(labels)
print(segment_certainties)
if len(labels):
top_certainty, top_label = max(zip(segment_certainties, labels))
try:
if top_label != 'tote':
if top_certainty > 10:
if userdata.data['task'] == 'stow' or (
userdata.data['task'] == 'pick' and top_label
in userdata.data['wanted_items']):
rospy.logerr("I'M CHANGING THE OBJECT TO %s" %
top_label)
userdata.data['item_to_pick']['label'] = top_label
suck_level = item_meta[top_label].get(
'suction_pressure', 3)
suction.set_suck_level(suck_level)
return 'succeeded'
except:
pass
mid_x, mid_y = POSITION_LIMITS[userdata.data['picking_from']]['centre']
orig = userdata.data['original_pick_position']
if orig.position.x > mid_x:
orig.position.x -= 0.08
else:
orig.position.x += 0.08
if orig.position.y > mid_y:
orig.position.y -= 0.08
else:
orig.position.y += 0.08
orig.position.y = min(orig.position.y, SUCKER_MAX_Y_STRAIGHT)
res = m.move_to_global(orig.position.x, orig.position.y,
SUCKER_LIFT_HEIGHT + 0.01, 'sucker')
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
scales_topic = userdata.data['picking_from'] + '_scales/weight'
res, _, _ = m.move_to_global_monitor_weight(orig.position.x,
orig.position.y,
orig.position.z - 0.13,
'sucker',
scales_topic,
velo_scale=0.2)
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
return 'classification_failed'
def execute(self, userdata):
into_bin = 'B'
weight_before = -1
drop_orientation = Quaternion()
drop_orientation.x = 0
drop_orientation.y = 0
drop_orientation.z = 0
drop_orientation.w = 1
drop_velo_scale = 0.5
x_pos = None
y_pos = None
z_pos = None
if userdata.data['task'] == 'stow':
#bins = ['storage_B', 'storage_A']
bins = ['storage_B'] # Only B for finals.
elif userdata.data['task'] == 'pick':
if userdata.data['move_objects_between_bins_mode'] is not None:
bins = ['storage_' + userdata.data['move_objects_between_bins_mode']]
else:
l = userdata.data['item_to_pick']['label']
box_id = None
for o in userdata.data['order']:
if l in o['contents']:
box_id = o['size_id']
bins = [box_id]
if box_id is None:
rospy.logerr("NO BOX FOR ITEM")
return 'failed'
else:
return 'failed'
for bin_id in bins:
if bin_id not in userdata.data['empty_space_capture']:
continue
pc = userdata.data['empty_space_capture'][bin_id].get('point_cloud', None)
if pc is None:
# Make sure we actually have one.
continue
req = ObjectPlacementPoseFromCloudRequest()
req.cropped_cloud = userdata.data['empty_space_capture'][bin_id]['point_cloud']
req.camera_to_world = userdata.data['empty_space_capture'][bin_id]['pose']
l = userdata.data['item_to_pick']['label']
dimensions = item_meta[l]['dimensions']
dimensions.sort()
req.object_longest_side_length.data = dimensions[2]
req.object_middlest_side_length.data = dimensions[1]
req.object_shortest_side_length.data = dimensions[0]
req.resolution.data = 0.01
if max(item_meta[l]['dimensions']) > 0.12:
drop_velo_scale = 0.15
res = self.free_space_service.call(req)
if not res.success.data:
continue
x_pos = res.x.data
y_pos = res.y.data
z_pos = res.z.data - 0.13
if res.degrees_to_global_y.data == 90 and (x_pos < 0.75 or y_pos < 0.75):
res.degrees_to_global_y.data = -90 # Rotate away from the vacuum hose, unless we're in the to left coner.
if res.degrees_to_global_y.data > 89: # Stops the robot doing weird gimbal lock things.
res.degrees_to_global_y.data = 89
if res.degrees_to_global_y.data < -89:
res.degrees_to_global_y.data = -89
tq = tft.quaternion_from_euler(0, 0, res.degrees_to_global_y.data * 3.14/180.0)
drop_orientation.x = tq[0]
drop_orientation.y = tq[1]
drop_orientation.z = tq[2]
drop_orientation.w = tq[3]
t.publish_tf_quaterion_as_transform((x_pos, y_pos, z_pos), tq, 'global_xyz_link', 'DROP_HERE', 0.5)
userdata.data['empty_space_capture'][bin_id] = None
break
if userdata.data['task'] == 'stow':
into_bin = bin_id[-1]
if x_pos is None:
rospy.logerr('Couldn\'t find space to place.')
x_pos = 0.350
y_pos = 0.440
z_pos = 0.900
into_bin = 'B'
bin_id = 'storage_B'
userdata.data['empty_space_capture']['storage_B'] = None
if item_meta[userdata.data['item_to_pick']['label']].get('ignore_weight') == True:
rospy.logerr("Overriding drop point for floppy item")
x_pos = 0.350
y_pos = 0.210
z_pos = 0.800
into_bin = 'B'
bin_id = 'storage_B'
userdata.data['empty_space_capture']['storage_B'] = None
position_limits = POSITION_LIMITS[bin_id]['sucker']
x_pos = max(x_pos, position_limits['x_min'])
x_pos = min(x_pos, position_limits['x_max'])
y_pos = max(y_pos, position_limits['y_min'])
y_pos = min(y_pos, position_limits['y_max'])
scales_topic = bin_id + '_scales/weight'
try:
weight = rospy.wait_for_message(scales_topic, Int16, 2.0)
weight_before = weight.data
except:
rospy.logerr('Unable to read weight from %s' % scales_topic)
weight_before = -1
elif userdata.data['task'] == 'pick' and userdata.data['move_objects_between_bins_mode'] is None:
if x_pos is None:
rospy.logerr('Couldn\'t find space to place.')
box = userdata.data['boxes'][box_id]
x_pos = (box['left'] + box['right'])/2
y_pos = (box['top'] + box['bottom'])/2
z_pos = 0.95
yaw = 0
userdata.data['empty_space_capture'][box_id] = None
if item_meta[userdata.data['item_to_pick']['label']].get('ignore_weight') == True:
rospy.logerr('Overriding drop point for floppy item')
box = userdata.data['boxes'][box_id]
x_pos = (box['left'] + box['right'])/2
y_pos = box['top'] - 0.02
z_pos = 0.9
yaw = 0
userdata.data['empty_space_capture'][box_id] = None
x_pos = max(x_pos, SUCKER_MIN_GLOBAL_X)
x_pos = min(x_pos, SUCKER_MAX_GLOBAL_X)
y_pos = min(y_pos, SUCKER_MAX_Y_STRAIGHT)
elif userdata.data['task'] == 'pick' and userdata.data['move_objects_between_bins_mode'] is not None:
drop_pos = POSITION_LIMITS['storage_' + userdata.data['move_objects_between_bins_mode']]['centre']
x_pos = drop_pos[0]
y_pos = drop_pos[1]
if userdata.data['move_objects_between_bins_mode'] == 'A':
x_pos += 0.08
elif userdata.data['move_objects_between_bins_mode'] == 'B':
x_pos -= 0.08
z_pos = 0.9
into_bin = userdata.data['move_objects_between_bins_mode']
move_velo_scale = 1.0
if item_meta[userdata.data['item_to_pick']['label']]['weight'] > 0.5:
# Heavy Item
move_velo_scale = 0.5
curr = t.current_robot_pose('global_xyz_link', 'sucker_endpoint')
res = m.move_to_global(x_pos, y_pos, curr.position.z + 0.01, 'sucker', orientation=drop_orientation, velo_scale=move_velo_scale)
if not res:
rospy.logerr('Failed to move to drop point')
return 'failed'
# if suction.check_pressure_sensor() == False:
# rospy.logerr('No suction pressure, we probably dropped it')
# # Undo an added weight reclassification if it happened.
# if userdata.data['weight_reclassifications']:
# if userdata.data['weight_reclassifications'][-1][1] == userdata.data['item_to_pick']['label']:
# userdata.data['weight_reclassifications'] = userdata.data['weight_reclassifications'][:-1]
# return 'succeeded'
if userdata.data['task'] == 'stow':
# We can use the scales to check our movement
scales_topic = 'storage_%s_scales/weight' % into_bin
res, stopped, bail = m.move_to_global_monitor_weight(x_pos, y_pos, z_pos, 'sucker', scales_topic, velo_scale=drop_velo_scale)
if not res:
rospy.logerr('Failed to perform a move')
#return 'failed' # Don't fail, just continue because the item will probably end up in the storage system.
if bail:
m.move_relative(0, 0, -0.04, 'sucker_endpoint', 'sucker')
else:
# No scales for pick task, just normal move.
res = m.move_to_global(x_pos, y_pos, z_pos, 'sucker', velo_scale=0.2)
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
set_suck_level(0)
suction.pump_off()
rospy.sleep(1.0)
userdata.data['last_drop_bin'] = into_bin
if weight_before != -1 and userdata.data['task'] == 'stow':
scales_topic = bin_id + '_scales/weight'
try:
weight = rospy.wait_for_message(scales_topic, Int16, 2.0)
if (weight.data - weight_before) < 5:
rospy.logerr('No weight change, we must have dropped it.')
# Don't update the item locations.
if userdata.data['weight_reclassifications']:
if userdata.data['weight_reclassifications'][-1][1] == userdata.data['item_to_pick']['label']:
userdata.data['weight_reclassifications'] = userdata.data['weight_reclassifications'][:-1]
return 'succeeded'
rospy.logerr("SUCCESSFUL DROP")
except:
rospy.logerr('Unable to read weight from %s' % scales_topic)
if userdata.data['task'] == 'pick' and userdata.data['weight_before'] != -1:
# Make sure the weight in the source storage system is still down.
scales_topic = userdata.data['picking_from'] + '_scales/weight'
try:
weight = rospy.wait_for_message(scales_topic, Int16)
except:
rospy.logerr('Failed to get weight from %s, ignoring weight check')
else:
dw = userdata.data['weight_before'] - weight.data
if dw < 4:
# The item must have fallen out before we got to the box.
# Don't update the item locations
rospy.logerr('The source storage system didn\'t change weight, we probably dropped the item')
return 'succeeded'
# Update item locations.
if userdata.data['task'] == 'stow':
cur_label = userdata.data['item_to_pick']['label']
try:
userdata.data['item_locations']['tote']['contents'].remove(cur_label)
except ValueError:
# The item wasn't meant to be in the tote, maybe we thought we already moved it.
# Amend the list.
for b in userdata.data['item_locations']['bins']:
if cur_label in b['contents']:
b['contents'].remove(cur_label)
for b in userdata.data['item_locations']['bins']:
if b['bin_id'] == into_bin:
b['contents'].append(cur_label)
elif userdata.data['task'] == 'pick' and userdata.data['move_objects_between_bins_mode'] is None:
cur_label = userdata.data['item_to_pick']['label']
for b in userdata.data['item_locations']['bins']:
if cur_label in b['contents']:
b['contents'].remove(cur_label)
for b in userdata.data['item_locations']['boxes']:
if b['size_id'] == box_id:
b['contents'].append(cur_label)
elif userdata.data['task'] == 'pick' and userdata.data['move_objects_between_bins_mode'] is not None:
cur_label = userdata.data['item_to_pick']['label']
for b in userdata.data['item_locations']['bins']:
if cur_label in b['contents']:
b['contents'].remove(cur_label)
if b['bin_id'] == userdata.data['move_objects_between_bins_mode']:
b['contents'].append(cur_label)
userdata.data['move_objects_between_bins_mode'] = None
# Update the output file.
json_functions.save_file(userdata.data['task'], 'item_location_file_out.json', userdata.data['item_locations'])
return 'succeeded'