def get_detect_shelf_layers_path(self, shelf_system_id):
"""
:type shelf_system_id: str
:rtype: FullBodyPath
"""
shelf_system_width = self.knowrob.get_shelf_layer_width(
shelf_system_id)
# shelf_system_height = self.knowrob.get_shelf_system_height(shelf_system_id)
full_body_path = FullBodyPath()
# via points
for via_point in self.get_via_points(shelf_system_id):
full_body_pose = FullBodyPosture()
full_body_pose.base_pos = via_point
full_body_pose.type = FullBodyPosture.BASE
full_body_path.postures.append(full_body_pose)
# calculate base pose
base_pose = self.base_pose_in_front_of_shelf(shelf_system_id,
shelf_system_width / 2,
y=-0.85)
base_pose = transform_pose(
self.knowrob.get_perceived_frame_id(shelf_system_id), base_pose)
if self.is_left(shelf_system_id):
base_pose.pose.position.x += 0.5
else:
base_pose.pose.position.x -= 0.5
base_pose = transform_pose('map', base_pose)
full_body_pose = FullBodyPosture()
full_body_pose.base_pos = base_pose
full_body_pose.type = FullBodyPosture.BASE
full_body_path.postures.append(full_body_pose)
full_body_pose = FullBodyPosture()
if self.is_left(shelf_system_id):
full_body_pose.goal_joint_state = self.get_floor_detection_pose_left(
)
else:
full_body_pose.goal_joint_state = self.get_floor_detection_pose_right(
)
full_body_pose.type = FullBodyPosture.JOINT
full_body_path.postures.append(full_body_pose)
full_body_pose = FullBodyPosture()
full_body_pose.base_pos = self.cam_pose_in_front_of_shelf(
shelf_system_id, x=shelf_system_width / 2, y=-0.55)
full_body_pose.type = FullBodyPosture.CAM_FOOTPRINT
full_body_path.postures.append(full_body_pose)
full_body_pose = FullBodyPosture()
full_body_pose.type = FullBodyPosture.CAMERA
full_body_pose.camera_pos.header.frame_id = 'camera_link'
full_body_pose.camera_pos.pose.position = Point(0, 1.364, 0)
full_body_pose.camera_pos.pose.orientation = Quaternion(0, 0, 0, 1)
full_body_path.postures.append(full_body_pose)
return full_body_path
def stop_barcode_detection(self, frame_id):
"""
:type frame_id: str
:return: dict mapping barcode to pose stamped
:rtype: dict
"""
barcodes = {}
width = self.knowrob.get_shelf_layer_width(self.current_shelf_layer_id)
num_of_barcodes = max(
1, self.number_of_facings - int(np.random.rand() * 3))
for i in range(num_of_barcodes):
barcode = PoseStamped()
barcode.header.frame_id = self.knowrob.get_perceived_frame_id(
self.current_shelf_layer_id)
x = max(
0,
min(width, ((i + .5) / (num_of_barcodes)) * width +
np.random.normal(scale=.1 / num_of_barcodes)))
barcode.pose.position = Point(x, 0, 0)
barcode.pose.orientation.w = 1
barcode = transform_pose('map', barcode)
try:
if np.random.choice([True]):
barcodes[str(self.barcodes.pop())] = barcode
else:
rnd_barcode = self.make_rnd_barcode()
barcodes[rnd_barcode] = barcode
except KeyError:
self.get_all_barcodes()
barcodes[self.barcodes.pop()] = barcode
return barcodes
def cam_pose_in_front_of_shelf(self,
shelf_system_id,
frame_id=None,
x=0.,
y=-0.55,
x_limit=0.1,
goal_angle=0.):
"""
computes a goal for base_link such that torso_Schwenker_Cams is at x/y in front of shelf_system_id
:type shelf_system_id: str
:type x: float
:type y: float
:param x_limit: safety buffer range along x axis of shelf system
:type x_limit: float
:rtype: PoseStamped
"""
width = self.knowrob.get_shelf_system_width(shelf_system_id)
cam_pose = PoseStamped()
if frame_id is None:
cam_pose.header.frame_id = self.knowrob.get_perceived_frame_id(
shelf_system_id)
else:
cam_pose.header.frame_id = frame_id
cam_pose.pose.position.x = max(0 + x_limit, min(width - x_limit, x))
cam_pose.pose.position.y = np.cos(goal_angle) * y
cam_pose.pose.orientation = self.get_goal_base_rotation(
shelf_system_id)
cam_pose = transform_pose('map', cam_pose)
cam_pose.pose.position.z = 0
return cam_pose
def see(self, depth, width, height):
q = {'detect': {'pose': ''}}
self.print_with_prefix('sending: {}'.format(q))
req = RSQueryServiceRequest()
req.query = json.dumps(q)
rospy.sleep(0.4)
objects = []
expected_volume = self.volume(depth, width, height)
while len(objects) < 5:
result = self.robosherlock_service.call(req)
answers = [json.loads(x) for x in result.answer]
if not answers:
continue
real_object = min(
answers,
key=lambda x: abs(self.answer_volume(x) - expected_volume))
if abs(
1 - self.answer_front_area(real_object) /
(width * height)) > 0.3: #175: # reject if more than x% diff
continue
try:
pose = message_converter.convert_dictionary_to_ros_message(
'geometry_msgs/PoseStamped',
real_object['poses'][0]['pose_stamped'])
except:
continue
pose.pose.position.z += real_object['boundingbox'][
'dimensions-3D']['height'] / 2.
objects.append(pose)
self.print_with_prefix('found pose number {}'.format(len(objects)))
objects = self.filter_outlier(objects)
p = self.avg_pose(objects)
# p.pose.position.z += depth/2.
return transform_pose('map', p)
def move_other_frame(self, target_pose, frame='camera_link', retry=True):
if self.enabled:
target_pose = self.cam_pose_to_base_pose(target_pose, frame)
target_pose = transform_pose('map', target_pose)
target_pose.pose.position.z = 0
self.goal_pub.publish(target_pose)
goal = MoveBaseGoal()
goal.target_pose = target_pose
if self.knowrob is not None:
self.knowrob.start_base_movement(self.knowrob.pose_to_prolog(target_pose))
while True:
self.client.send_goal(goal)
wait_result = self.client.wait_for_result(rospy.Duration(self.timeout))
result = self.client.get_result()
state = self.client.get_state()
if wait_result and state == GoalStatus.SUCCEEDED:
break
if retry:
cmd = raw_input('base movement did not finish in time, retry? [y/n]')
retry = cmd == 'y'
if not retry:
print('movement did not finish in time')
if self.knowrob is not None:
self.knowrob.finish_action()
raise TimeoutError()
if self.knowrob is not None:
self.knowrob.finish_action()
return result
def update_shelf_system_pose(knowrob, top_layer_id, separators):
"""
:type knowrob: refills_perception_interface.knowrob_wrapper.KnowRob
:type shelf_system_id: str
:type separators: list
:return:
"""
if not knowrob.is_bottom_layer(top_layer_id):
return
if len(separators) == 0:
return
shelf_system_id = knowrob.get_shelf_system_from_layer(top_layer_id)
shelf_system_frame_id = knowrob.get_object_frame_id(shelf_system_id)
separators_in_system = [
transform_pose(shelf_system_frame_id, p) for p in separators
]
separators_xy = np.array([[p.pose.position.x, p.pose.position.y]
for p in separators_in_system])
separators_y = np.array(separators_xy)[:, 1].mean()
T_system___layer = lookup_pose(
shelf_system_frame_id, knowrob.get_perceived_frame_id(top_layer_id))
y_offset = separators_y - T_system___layer.pose.position.y
A = np.vstack(
[np.array(separators_xy)[:, 0],
np.ones(separators_xy.shape[0])]).T
y = separators_xy[:, 1]
m, _ = np.linalg.lstsq(A, y, rcond=-1)[0]
x = np.array([1, m, 0])
x = x / np.linalg.norm(x)
z = np.array([0, 0, 1])
y = np.cross(z, x)
q = PyKDL.Rotation(x[0], y[0], z[0], x[1], y[1], z[1], y[2], y[2],
z[2]).GetQuaternion()
offset = PoseStamped()
offset.header.frame_id = shelf_system_frame_id
offset.pose.position.y = y_offset
offset.pose.orientation = Quaternion(*q)
offset = transform_pose('map', offset)
knowrob.belief_at_update(shelf_system_id, offset)
rospy.sleep(0.5)
def cb(self, data):
"""
fills a dict that maps barcode to list of PoseStamped where it was seen.
:type data: Barcode
"""
if self.listen:
p = transform_pose(MAP, data.barcode_pose)
if p is not None:
p.header.stamp = rospy.Time()
if p is not None and self.barcode_on_shelf_layer(p):
if data.barcode[0] == '2':
self.barcodes[data.barcode[1:-1]].append(p)
def get_count_product_posture(self, facing_id):
"""
:type facing_id: str
:rtype: FullBodyPosture
"""
shelf_layer_id = self.knowrob.get_shelf_layer_from_facing(facing_id)
shelf_system_id = self.knowrob.get_shelf_system_from_layer(
shelf_layer_id)
# shelf_system_width = self.knowrob.get_shelf_system_width(shelf_system_id)
# shelf_system_frame_id = self.knowrob.get_perceived_frame_id(shelf_system_id)
shelf_layer_frame_id = self.knowrob.get_perceived_frame_id(
shelf_layer_id)
facing_frame_id = self.knowrob.get_object_frame_id(facing_id)
# get height of next layer
# shelf_layer_above_id = self.knowrob.get_shelf_layer_above(shelf_layer_id)
# if shelf_layer_above_id is not None:
# shelf_layer_above_frame_id = self.knowrob.get_perceived_frame_id(shelf_layer_above_id)
# height_of_next_layer = lookup_transform('map', shelf_layer_above_frame_id).pose.position.z
# else:
# height_of_next_layer = self.knowrob.get_shelf_system_height(shelf_system_id)
# joints
shelf_layer_height = lookup_pose('map',
shelf_layer_frame_id).pose.position.z
counting_offset = 0.3
# TODO tune this number
torso_rot_1_height = shelf_layer_height + counting_offset
torso_rot_1_height = max(MIN_CAM_HEIGHT, torso_rot_1_height)
goal_angle = radians(-20)
full_body_pose = self.get_cam_pose(torso_rot_1_height, goal_angle,
self.is_left(shelf_system_id))
facing_pose_on_layer = lookup_pose(shelf_layer_frame_id,
facing_frame_id)
# base_pose = self.cam_pose_in_front_of_facing(facing_id, x=0, x_limit=0, goal_angle=goal_angle)
base_pose = self.cam_pose_in_front_of_layer(
shelf_layer_id,
x=facing_pose_on_layer.pose.position.x,
y=-.55,
goal_angle=goal_angle)
base_pose = transform_pose('map', base_pose)
full_body_pose.type = full_body_pose.BOTH
full_body_pose.base_pos = base_pose
return full_body_pose
def separator_cb(self, separator_array):
"""
adds detected separators to self.detections
:type separator_array: SeparatorArray
"""
if self.listen:
# print('registered separator')
for separator in separator_array.separators:
p = transform_pose('map', separator.separator_pose)
p.header.stamp = rospy.Time()
# p = transform_pose(self.current_frame_id, p)
# rospy.logwarn('cant transform separator')
if p is not None and self.separator_on_shelf_layer(p):
self.detections.append([
p.pose.position.x, p.pose.position.y, p.pose.position.z
])
def stop_separator_detection(self, frame_id):
"""
:type frame_id: str
:return: list of pose stamps
:rtype: list
"""
separators = []
width = self.knowrob.get_shelf_layer_width(self.current_shelf_layer_id)
for i in range(self.number_of_facings + 1):
separator = PoseStamped()
separator.header.frame_id = self.knowrob.get_perceived_frame_id(
self.current_shelf_layer_id)
x = (i / (self.number_of_facings)) * width
if x > 0.01 and x < 0.99:
x = max(0, min(width, x + np.random.normal(scale=0.02)))
separator.pose.position = Point(x, 0, 0)
separator.pose.orientation.w = 1
separator = transform_pose('map', separator)
separators.append(separator)
return separators
def get_facing_ids_from_layer(self, shelf_layer_id):
"""
:type shelf_layer_id: str
:return:
:rtype: OrderedDict
"""
shelf_system_id = self.get_shelf_system_from_layer(shelf_layer_id)
q = 'findall([F, P], (shelf_facing(\'{}\', F),is_at(F, P)), Fs).'.format(
shelf_layer_id)
solutions = self.all_solutions(q)[0]
facings = []
for facing_id, pose in solutions['Fs']:
facing_pose = self.prolog_to_pose_msg(pose)
facing_pose = transform_pose(
self.get_perceived_frame_id(shelf_layer_id), facing_pose)
facings.append((facing_id, facing_pose))
is_left = 1 if self.is_left(shelf_system_id) else -1
facings = list(
sorted(facings, key=lambda x: x[1].pose.position.x * is_left))
return OrderedDict(facings)
def update_shelf_layer_position(self, shelf_layer_id, separators):
"""
:type shelf_layer_id: str
:type separators: list of PoseStamped, positions of separators
"""
if len(separators) > 0:
old_p = lookup_pose('map',
self.get_perceived_frame_id(shelf_layer_id))
separator_zs = [p.pose.position.z for p in separators]
new_floor_height = np.mean(separator_zs)
current_floor_pose = lookup_pose(
MAP, self.get_object_frame_id(shelf_layer_id))
current_floor_pose.pose.position.z += new_floor_height - old_p.pose.position.z
shelf_system = self.get_shelf_system_from_layer(shelf_layer_id)
frame_id = self.get_object_frame_id(shelf_system)
current_floor_pose.header.stamp = rospy.Time()
current_floor_pose = transform_pose(frame_id, current_floor_pose)
q = 'tell(is_at(\'{}\', {}))'.format(
shelf_layer_id, self.pose_to_prolog(current_floor_pose))
self.once(q)
def stop_detect_shelf_layers(self, shelf_system_id):
"""
:type shelf_system_id: str
:return: list of shelf layer heights
:rtype: list
"""
shelf_frame = self.knowrob.get_perceived_frame_id(shelf_system_id)
req = RSQueryServiceRequest()
q = {
'scan': {
'type': 'shelf',
'location': shelf_frame,
'command': 'stop'
}
}
req.query = json.dumps(q)
self.print_with_prefix('sending: {}'.format(q))
result = self.robosherlock_service.call(req)
self.print_with_prefix('received: {}'.format(result))
floors = []
for floor in result.answer:
pose = json.loads(floor)['rs.annotation.PoseAnnotation'][0][
'camera']['rs.tf.StampedPose']
p = self.rs_pose_to_geom_msgs_pose(pose)
# p = message_converter.convert_dictionary_to_ros_message('geometry_msgs/PoseStamped',
# json.loads(floor)['poses'][0]['pose_stamped'])
# TODO potential speedup, safe and reuse transform
p = transform_pose(shelf_frame, p)
floors.append(p.pose.position.z)
floors = list(sorted(floors))
# floors = [x[-1] for x in floors]
floors = add_bottom_layer_if_not_present(floors, shelf_system_id,
self.knowrob)
return floors
def get_detect_facings_path(self, shelf_layer_id):
"""
:type shelf_layer_id: str
:rtype: FullBodyPath
"""
shelf_system_id = self.knowrob.get_shelf_system_from_layer(
shelf_layer_id)
# shelf_system_frame_id = self.knowrob.get_perceived_frame_id(shelf_system_id)
shelf_system_width = self.knowrob.get_shelf_system_width(
shelf_system_id)
shelf_layer_frame_id = self.knowrob.get_perceived_frame_id(
shelf_layer_id)
full_body_path = FullBodyPath()
# TODO consider left right cases
# joints
# TODO layer above
shelf_layer_height = lookup_pose('map',
shelf_layer_frame_id).pose.position.z
# TODO tune this number
to_low_offset = 0.05
other_offset = 0.05
full_body_pose = FullBodyPosture()
full_body_pose.type = FullBodyPosture.CAMERA
if self.layer_too_low(shelf_layer_height):
goal_angle = radians(-20)
full_body_pose = self.get_cam_pose(
shelf_layer_height + to_low_offset, goal_angle,
self.is_left(shelf_system_id))
else:
goal_angle = radians(0)
full_body_pose = self.get_cam_pose(
shelf_layer_height + other_offset, goal_angle,
self.is_left(shelf_system_id))
full_body_path.postures.append(full_body_pose)
# base poses
# start base poses
start_base_pose = self.cam_pose_in_front_of_layer(
shelf_layer_id, goal_angle=goal_angle)
start_base_pose = transform_pose('map', start_base_pose)
start_full_body_pose = FullBodyPosture()
start_full_body_pose.type = FullBodyPosture.CAM_FOOTPRINT
start_full_body_pose.base_pos = start_base_pose
end_base_pose = self.cam_pose_in_front_of_layer(shelf_layer_id,
x=shelf_system_width,
goal_angle=goal_angle)
end_base_pose = transform_pose('map', end_base_pose)
end_full_body_pose = FullBodyPosture()
end_full_body_pose.type = FullBodyPosture.CAM_FOOTPRINT
end_full_body_pose.base_pos = end_base_pose
if self.is_left(shelf_system_id):
full_body_path.postures.append(end_full_body_pose)
full_body_path.postures.append(start_full_body_pose)
else:
full_body_path.postures.append(start_full_body_pose)
full_body_path.postures.append(end_full_body_pose)
return full_body_path
