def add_separators_and_barcodes(self, shelf_layer_id, separators,
barcodes):
t = lookup_transform(self.get_perceived_frame_id(shelf_layer_id),
'map')
separators = [do_transform_pose(p, t) for p in separators]
barcodes = {
code: do_transform_pose(p, t)
for code, p in barcodes.items()
}
shelf_layer_width = self.get_shelf_layer_width(shelf_layer_id)
separators_xs = [
p.pose.position.x / shelf_layer_width for p in separators
]
barcodes = [(p.pose.position.x / shelf_layer_width, barcode)
for barcode, p in barcodes.items()]
# definitely no hacks here
separators_xs, barcodes = add_separator_between_barcodes(
separators_xs, barcodes)
separators_xs = add_edge_separators(separators_xs)
separators_xs = merge_close_separators(separators_xs)
q = 'bulk_insert_floor(\'{}\', separators({}), labels({}))'.format(
shelf_layer_id, separators_xs, barcodes)
self.once(q)
rospy.sleep(5)
q = 'shelf_facings_mark_dirty(\'{}\')'.format(shelf_layer_id)
self.once(q)
def get_frame_in_base_footprint_kdl(self, frame):
"""
:rtype: PyKDL.Frame
"""
# if self.T_bf___cam_joint is None:
T_bf___cam_joint = msg_to_kdl(lookup_transform('base_footprint', frame))
T_bf___cam_joint.p[2] = 0
T_bf___cam_joint.M = PyKDL.Rotation()
return T_bf___cam_joint
def start_listening(self, shelf_layer_id):
"""
:type shelf_layer_id: str
"""
self.shelf_layer_id = shelf_layer_id
self.barcodes = defaultdict(list)
self.shelf_width = self.knowrob.get_shelf_layer_width(shelf_layer_id)
self.current_shelf_layer_width = self.knowrob.get_shelf_layer_width(shelf_layer_id)
self.current_frame_id = self.knowrob.get_perceived_frame_id(self.shelf_layer_id)
self.T_map___layer = lookup_transform(self.current_frame_id, 'map')
self.listen = True
def start_listening_separators(self, shelf_layer_id):
"""
:type shelf_layer_id: str
"""
self.hanging = False
self.current_shelf_layer_id = shelf_layer_id
self.detections = []
self.marker_ns = 'separator_{}'.format(shelf_layer_id)
self.current_shelf_layer_width = self.knowrob.get_shelf_layer_width(
shelf_layer_id)
self.current_frame_id = self.knowrob.get_perceived_frame_id(
self.current_shelf_layer_id)
self.T_map___layer = lookup_transform(self.current_frame_id, 'map')
self.listen = True
print_with_prefix('started', self.prefix)