def execute(self):
# Get the bottle pose from the camera.
square = 0.1175
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
for i in range(6):
for j in range(7):
m.move_to_global(curr.position.x + square * i,
curr.position.y + square * j, curr.position.z,
'gripper')
rospy.sleep(4)
def execute(self):
# Get the bottle pose from the camera.
rospy.loginfo('Moveing to view')
m.move_to_named_pose('whole_arm', 'view_checkerboard')
rospy.sleep(4)
poses = []
qt = tf.transformations.quaternion_from_euler(0.0, 0.0,
-3.141592653 / 2)
for i in range(16):
rospy.loginfo('GET OBJECT POSE')
b_req = cartesian_calibration.srv.GetACoorRequest()
b_req.data_set = "measured"
b_req.object_idx = i
b_res = self.pose_service.call(b_req)
p = b_res.object_pose.pose
# p.orientation.x = qt[0]
# p.orientation.y = qt[1]
# p.orientation.z = qt[2]
# p.orientation.w = qt[3]
object_global = t.convert_pose(p, self.camera_reference_frame,
'global_xyz_link')
# object_global.position.z = min(0.80,p.position.z)
poses.append(object_global)
m.move_to_named_pose('realsense', 'tool_change_position')
m.move_to_named_pose('wrist_only', 'sucker')
for p in poses:
if p.position.x == 0 and p.position.y == 0 and p.position.z == 0:
rospy.logerr('Failed to find an object. %s' % i)
# return 'failed'
continue
print("MOVE SUCKER TO ABOVE OBJECT")
p = t.align_pose_orientation_to_frame(p, 'global_xyz_link',
'sucker_endpoint')
t.publish_pose_as_transform(p, 'global_xyz_link', 'MOVE HERE', 0.5)
res = m.move_to_global(p.position.x - 0.02,
p.position.y + 0.01,
p.position.z - 0.02,
'sucker',
orientation=p.orientation)
rospy.sleep(4)
def execute(self, userdata):
suction.pump_off()
curr = t.current_robot_pose('global_xyz_link', 'realsense_endpoint')
rospy.loginfo('MOVE UP TO SAFE POSITION')
res = m.move_to_global(curr.position.x, curr.position.y, 0.505,
'realsense')
if not res:
rospy.logerr('Failed to move to safe position.')
return 'failed'
rospy.sleep(10.0)
return 'succeeded'
def execute(self, userdata):
rospy.loginfo('MOVE TO SAFE HEIGHT')
curr = t.current_robot_pose('global_xyz_link', 'realsense_endpoint')
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('TESTING THAT THE WRIST MOVES')
res = m.move_to_named_pose('wrist_only', 'test_joints')
if not res:
rospy.logerr('Wrist Failed to Move')
return 'failed'
return 'succeeded'
def execute(self, userdata):
gripper.open_gripper
rospy.loginfo('TOOL CHANGE')
rospy.loginfo('\tMove up to safe height')
curr = t.current_robot_pose('global_xyz_link', 'realsense_endpoint')
res = m.move_to_global(curr.position.x, curr.position.y, 0.505,
'realsense')
if not res:
rospy.logerr('Failed to move to safe position.')
return 'failed'
rospy.loginfo('\tMove to tool change position')
res = m.move_to_named_pose('realsense', 'tool_change_position')
if not res:
rospy.logerr('Failed to move to tool_change_position')
return 'failed'
if self.tool == 'gripper':
rospy.loginfo('\tSelect Gripper')
tool_change_named_pose = 'tool_change_gripper'
elif self.tool == 'sucker':
rospy.loginfo('\tSelect Sucker')
tool_change_named_pose = 'tool_change_sucker'
elif self.tool == 'neutral':
# Allow us to select neutral
rospy.loginfo('\tSelect Neutral')
tool_change_named_pose = 'tool_change_neutral'
else:
rospy.logerr('Incorrect tool name for tool change')
return 'failed'
res = m.move_to_named_pose('whole_arm', tool_change_named_pose)
if not res:
rospy.logerr('Failed to move to %s' % tool_change_named_pose)
return 'failed'
return 'succeeded'
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):
if userdata.data['task'] == 'stow':
if userdata.data['empty_space_capture'].get('storage_A') is None:
# We need to update storage_A
res = m.move_to_named_pose('realsense',
'realsense_above_storage_system_A')
if not res:
return 'failed'
pc, pose = self.get_point_cloud_and_position('storage_A')
if pc is None and pose is None:
return 'failed'
userdata.data['empty_space_capture']['storage_A'] = {
'point_cloud': pc,
'pose': pose
}
if userdata.data['empty_space_capture'].get('storage_B') is None:
# We need to update storage_A
res = m.move_to_named_pose('realsense',
'realsense_above_storage_system_B')
if not res:
return 'failed'
pc, pose = self.get_point_cloud_and_position('storage_B')
if pc is None and pose is None:
return 'failed'
userdata.data['empty_space_capture']['storage_B'] = {
'point_cloud': pc,
'pose': pose
}
elif userdata.data['task'] == 'pick':
for b in boxes:
if userdata.data['empty_space_capture'].get(b) is None:
bd = boxes[b]
# Find centre of box in x direction
x_pos = (bd['right'] + bd['left']) / 2
x_pos = min(x_pos, REALSENSE_MAX_GLOBAL_X)
x_pos = max(x_pos, REALSENSE_MIN_GLOBAL_X)
y_pos = 0.819 # This stops us hitting the gripper on the frame.
res = m.move_to_global(x_pos, y_pos, 0.575, 'realsense')
if not res:
return 'failed'
rospy.set_param('/storage_specs/%s/h_high' % b, 180)
rospy.set_param('/storage_specs/%s/h_inside' % b, True)
rospy.set_param('/storage_specs/%s/h_low' % b, 0)
rospy.set_param('/storage_specs/%s/s_high' % b, 255)
rospy.set_param('/storage_specs/%s/s_low' % b, 0)
rospy.set_param('/storage_specs/%s/v_high' % b, 255)
rospy.set_param('/storage_specs/%s/v_low' % b, 0)
rospy.set_param('/storage_specs/%s/x_max' % b,
(x_pos - bd['right']) + 0.02)
rospy.set_param('/storage_specs/%s/x_min' % b,
(x_pos - bd['left']) - 0.02)
rospy.set_param('/storage_specs/%s/y_max' % b,
(y_pos - bd['bottom']) + 0.02)
rospy.set_param('/storage_specs/%s/y_min' % b,
(y_pos - bd['top']) - 0.04)
rospy.set_param('/storage_specs/%s/z_max' % b, 1000)
rospy.set_param('/storage_specs/%s/z_min' % b, 0.4)
pc, pose = self.get_point_cloud_and_position(b)
if pc is None and pose is None:
return 'failed'
userdata.data['empty_space_capture'][b] = {
'point_cloud': pc,
'pose': pose
}
self.pc_pub.publish(pc)
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):
# wait for the specified duration or until we are asked to preempt
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
rospy.loginfo('MOVE UP TO DROP POSITION')
into_bin = 'B' # Default
weight_before = -1
drop_orientation = Quaternion() # Default straight
drop_orientation.x = 0
drop_orientation.y = 0
drop_orientation.z = 0
drop_orientation.w = 1
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:
# Try and find free space based on the point cloud.
# Try both 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
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 > 89:
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['storage_' + into_bin]['gripper']
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]
z_pos = 0.9
into_bin = userdata.data['move_objects_between_bins_mode']
res = m.move_to_global(x_pos,
y_pos,
GRIPPER_LIFT_HEIGHT_STRAIGHT + 0.01,
'gripper',
orientation=drop_orientation)
if not res:
rospy.logerr('Failed to move to tote')
return 'failed'
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
pose = gmsg.Pose()
pose.position.x = curr.position.x
pose.position.y = curr.position.y
pose.position.z = z_pos
pose.orientation = drop_orientation
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(
pose.position.x,
pose.position.y,
pose.position.z,
'gripper',
scales_topic,
orientation=pose.orientation,
velo_scale=0.4)
if not res:
return 'failed'
if bail:
m.move_relative(0, 0, -0.02, 'gripper_endpoint', 'gripper')
else:
# No scales, perform a normal movement
res = m.move_to_global(pose.position.x,
pose.position.y,
pose.position.z,
'gripper',
orientation=pose.orientation,
velo_scale=0.4)
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
res = gripper.open_gripper()
if not res:
rospy.logerr('Failed to open gripper')
return 'failed'
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.')
userdata.data['last_failed'][userdata.data['item_to_pick']
['label']] = 0
# 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]
# Don't update the item locations.
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
json_functions.save_file(userdata.data['task'],
'item_location_file_out.json',
userdata.data['item_locations'])
return 'succeeded'
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):
gripper.open_gripper()
position_limits = POSITION_LIMITS[userdata.data['picking_from']]['gripper']
# Get the weight before the pick
scales_topic = userdata.data['picking_from'] + '_scales/weight'
if item_meta[userdata.data['item_to_pick']['label']]['grasp_type'] != 'suck_grip':
# 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 'grip_failed'
# No way to detect failure yet, so just use one grasp point.
object_global = userdata.data['item_to_pick']['grasp_poses'][0]
object_global.position.x += 0.015
object_global.position.y -= 0.03
object_global.position.x = max(object_global.position.x, position_limits['x_min'])
object_global.position.x = min(object_global.position.x, position_limits['x_max'])
object_global.position.y = max(object_global.position.y, position_limits['y_min'])
object_global.position.y = min(object_global.position.y, position_limits['y_max'])
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', 0.5)
rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT')
curr = t.current_robot_pose('global_xyz_link', 'realsense_endpoint')
# res = m.move_to_global(curr.position.x, curr.position.y, TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense')
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 GRIPPER')
res = m.move_to_named_pose('wrist_only', 'gripper')
if not res:
return 'failed'
object_global.position.z += 0.015
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.55: # Stop gimbal lock and weird joint configurations.
e[2] = 1.55
if e[2] < -1.55:
e[2] = -1.55
q = tft.quaternion_from_euler(0, 0, 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]
rospy.loginfo('MOVE GRIPPER TO ABOVE OBJECT')
res = m.move_to_global(object_global.position.x, object_global.position.y, max(object_global.position.z - 0.2, GRIPPER_MIN_GLOBAL_Z), 'gripper', orientation=object_global.orientation)
if not res:
return 'failed'
rospy.loginfo('MOVING TO OBJECT')
v_scale = 0.1
if item_meta[userdata.data['item_to_pick']['label']]['grasp_point_type'] == 'rgb_centroid':
# Don't trust the depth.
v_scale = 0.1
if userdata.data['item_to_pick']['label'] == 'mesh_cup':
v_scale = 0.05
(res, stopped, bail) = m.move_to_global_monitor_weight(object_global.position.x, object_global.position.y,
min(object_global.position.z, GRIPPER_MAX_GLOBAL_Z), 'gripper', scales_topic, velo_scale=v_scale, orientation=object_global.orientation)
if not res:
return 'failed'
if bail:
m.move_relative(0, 0, -0.02, 'gripper_endpoint', 'gripper')
# rospy.sleep(0.5)
rospy.loginfo('CLOSE THE GRIPPER')
gripper.close_gripper()
rospy.sleep(0.5)
return('succeeded')
if tool is 'gripper':
#TOOL CHANGE TO 45 DEGREES
rospy.init_node('my_ros_node')
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
pose = gmsg.Pose()
qt = tf.transformations.quaternion_from_euler(0.785398, 0.0, 1.57)
pose.orientation.x = qt[0]
pose.orientation.y = qt[1]
pose.orientation.z = qt[2]
pose.orientation.w = qt[3]
rospy.loginfo('MOVE UP TO 45 DEGREES POSITION')
res = m.move_to_global(curr.position.x,
curr.position.y,
0.6,
'gripper',
orientation=pose.orientation)
if not res:
rospy.logerr(move_error)
rospy.loginfo(move_success + str(res))
#MOVE TO DROP POSITION
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
rospy.loginfo('MOVE UP TO DROP POSITION')
res = m.move_to_global(0.3,
curr.position.y,
curr.position.z,
'gripper',
orientation=None)
def execute(self, userdata):
# Only do it once.
move_objects_mode = userdata.data['move_objects_mode']
userdata.data['move_objects_mode'] = False
# Lift the gripper up slowly so that it gets a good grip.
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
res = m.move_to_global(curr.position.x,
curr.position.y,
max(curr.position.z - 0.05,
GRIPPER_MIN_GLOBAL_Z),
'gripper',
velo_scale=0.02)
curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
# Update current pose with new orientation.
grip_height = curr.position.z
curr.position.z = GRIPPER_LIFT_HEIGHT_STRAIGHT
#t.publish_pose_as_transform(curr, 'global_xyz_link', 'MOVE HERE', 1.0)
res = m.move_to_global(curr.position.x,
curr.position.y,
curr.position.z,
'gripper',
orientation=curr.orientation,
velo_scale=0.5)
if not res:
rospy.logerr('Failed to perform a move (lift object gripper)')
return 'failed'
hold_gripper()
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.15
else:
curr.position.y += 0.15
res, _, _ = m.move_to_global_monitor_weight(curr.position.x,
curr.position.y,
grip_height - 0.13,
'gripper',
scales_topic,
velo_scale=0.1)
return 'grip_failed'
if userdata.data['weight_before'] == -1:
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.
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.')
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_type'] == 'grip_suck':
return 'try_sucker'
userdata.data['last_failed'][userdata.data['item_to_pick']
['label']] = 0
return 'grip_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]
return 'succeeded'
# Put the item back down and bail.
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
userdata.data['last_failed'][userdata.data['item_to_pick']
['label']] = 0
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,
grip_height - 0.13,
'gripper',
scales_topic,
velo_scale=0.1)
if not res:
rospy.logerr('Failed to perform a move')
return 'failed'
return 'grip_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'