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 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
q = 'is_at(\'{}\', {})'.format(
shelf_layer_id, self.pose_to_prolog(current_floor_pose))
self.once(q)
def height_to_cam_pose(self, desired_height):
"""
puts cam at desired height, keeping rot the same
:type desired_height: float
:return: goal height for lin joint 1, goal height for lin joint 2
:rtype: PoseStamped
"""
cam_pose = lookup_pose('base_footprint', 'camera_link')
cam_pose.pose.position.z = max(MIN_CAM_HEIGHT,
min(desired_height, MAX_CAM_HEIGHT))
return cam_pose
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 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 get_shelf_layer_from_system(self, shelf_system_id):
"""
:type shelf_system_id: str
:return: returns dict mapping floor id to pose ordered from lowest to highest
:rtype: dict
"""
q = 'triple(\'{}\', dul:hasComponent, Floor), ' \
'instance_of(Floor, {}), ' \
'object_feature_type(Floor, Feature, dmshop:\'DMShelfPerceptionFeature\'),' \
'object_frame_name(Feature, FeatureFrame).'.format(shelf_system_id, SHELF_FLOOR)
solutions = self.all_solutions(q)
floors = []
shelf_frame_id = self.get_perceived_frame_id(shelf_system_id)
for solution in solutions:
floor_id = solution['Floor'].replace('\'', '')
floor_pose = lookup_pose(
shelf_frame_id, solution['FeatureFrame'].replace('\'', ''))
floors.append((floor_id, floor_pose))
floors = list(sorted(floors, key=lambda x: x[1].pose.position.z))
floors = [x for x in floors if x[1].pose.position.z < MAX_SHELF_HEIGHT]
self.floors = OrderedDict(floors)
return self.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
def get_shelf_pose(self, shelf_system_id):
return lookup_pose("map", self.get_object_frame_id(shelf_system_id))
import yaml
import rospy
from refills_perception_interface.tfwrapper import lookup_pose, init
rospy.init_node('pose_printer')
init(1, 1)
p = lookup_pose('map', 'base_footprint')
p.header.stamp.secs = 0.0
p.header.stamp.nsecs = 0.0
print(yaml.dump(str(p)).replace('\\"\\\n ','\'').replace('\\\n \\','').replace('\\n','\n').replace('\\"','\'').replace('"',''))
