def __init__(self):
# TODO implement all the things [high]
# TODO use paramserver [low]
self.floors = {}
self.shelf_ids = []
self.separators = {}
self.perceived_frame_id_map = {}
self.action_graph = None
self.tf = TfWrapper()
self.prolog = json_prolog.Prolog()
self.prolog.wait_for_service()
self.query_lock = Lock()
def __init__(self):
# TODO use paramserver [low]
self.shelf_width = 1
self.tf = TfWrapper()
self.marker_pub = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=10)
self.marker_ns = 'baseboard_marker'
self.shelves = []
self.baseboard_detector_topic = '/ros_markers/tf'
# try:
# rospy.wait_for_message('/refills_wrist_camera/image_color', rospy.AnyMsg, timeout=1)
# except ROSException as e:
# rospy.loginfo('camera offline; \'detecting\' shelves anyway')
# self.detect_fake_shelves()
self.left_color = ColorRGBA(1, 1, 0, 1)
self.right_color = ColorRGBA(1, .5, 0, 1)
def __init__(self, knowrob):
self.knowrob = knowrob
# TODO use paramserver [low]
self.tf = TfWrapper(6)
self.marker_pub = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=10)
self.detections = []
self.map_frame_id = 'map'
self.separator_maker_color = ColorRGBA(.8, .8, .8, .8)
self.separator_maker_scale = Vector3(.01, .5, .05)
self.min_samples = 1
self.max_dist = 0.02
self.hanging = False
self.listen = False
self.separator_sub = rospy.Subscriber(
'/separator_marker_detector_node/data_out',
SeparatorArray,
self.separator_cb,
queue_size=10)
def __init__(self, knowrob):
self.knowrob = knowrob
# TODO use paramserver [low]
# TODO implement all the things [high]
self.separator_detection = SeparatorClustering(knowrob)
self.baseboard_detection = BaseboardDetector()
self.barcode_detection = BarcodeDetector(knowrob)
self.ring_light_srv = rospy.ServiceProxy('ring_light_switch/setbool',
SetBool)
self.floor_detection = True
try:
rospy.wait_for_service('/RoboSherlock/json_query', 1)
self.robosherlock_service = rospy.ServiceProxy(
'/RoboSherlock/json_query', RSQueryService)
self.robosherlock = True
except ROSException as e:
rospy.logwarn(
'robosherlock not available; activating fake perception')
self.robosherlock = False
self.tf = TfWrapper()
class BaseboardDetector(object):
def __init__(self):
# TODO use paramserver [low]
self.shelf_width = 1
self.tf = TfWrapper()
self.marker_pub = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=10)
self.marker_ns = 'baseboard_marker'
self.shelves = []
self.baseboard_detector_topic = '/ros_markers/tf'
# try:
# rospy.wait_for_message('/refills_wrist_camera/image_color', rospy.AnyMsg, timeout=1)
# except ROSException as e:
# rospy.loginfo('camera offline; \'detecting\' shelves anyway')
# self.detect_fake_shelves()
self.left_color = ColorRGBA(1, 1, 0, 1)
self.right_color = ColorRGBA(1, .5, 0, 1)
def start_listening(self):
self.shelves = []
self.detector_sub = rospy.Subscriber(self.baseboard_detector_topic,
TFMessage,
self.cb,
queue_size=10)
def stop_listening(self):
self.detector_sub.unregister()
# self.publish_as_marker()
return OrderedDict(
[x.get_shelf() for x in self.shelves if x.is_complete()])
def detect_fake_shelves(self, ids):
if '0' in ids:
s1 = Shelf(20, [0.695, -0.59, 0])
s1.add_measurement(21, [1.59, -0.59, 0])
self.shelves.append(s1)
if '1' in ids:
s2 = Shelf(22, [1.695, -0.59, 0])
s2.add_measurement(23, [2.585, -0.59, 0])
self.shelves.append(s2)
if '2' in ids:
s3 = Shelf(24, [2.695, -0.59, 0])
s3.add_measurement(25, [3.585, -0.59, 0])
self.shelves.append(s3)
if '3' in ids:
s4 = Shelf(26, [3.695, -0.59, 0])
s4.add_measurement(27, [4.585, -0.59, 0])
self.shelves.append(s4)
def cb(self, data):
for msg in data.transforms:
number = int(msg.child_frame_id.split('_')[-1])
pose = PoseStamped()
pose.header = msg.header
pose.pose.position = Point(msg.transform.translation.x,
msg.transform.translation.y,
msg.transform.translation.z)
pose.pose.orientation.w = 1
pose = self.tf.transform_pose(MAP, pose)
if pose is not None:
position = [
pose.pose.position.x, pose.pose.position.y,
pose.pose.position.z
]
for shelf in self.shelves:
if shelf.add_measurement(number, position):
break
else: # if not break
self.shelves.append(Shelf(number, position))
def publish_as_marker(self):
# TODO use current frame id
ma = MarkerArray()
for i, shelf in enumerate(self.shelves):
if shelf.is_complete():
# shelf
m = Marker()
m.header.frame_id = shelf.get_name()
m.ns = self.marker_ns
m.id = i
m.pose.orientation = Quaternion(
*quaternion_about_axis(-np.pi / 2, [0, 0, 1]))
if shelf.id == 0:
m.pose.position.y -= 0.07
m.action = Marker.ADD
m.type = Marker.MESH_RESOURCE
m.mesh_resource = 'package://iai_shelves/meshes/Shelf_{}.dae'.format(
shelf.id)
m.scale = Vector3(1, 1, 1)
m.color = ColorRGBA(0, 0, 0, 0)
m.mesh_use_embedded_materials = True
ma.markers.append(m)
# left
m = Marker()
m.header.frame_id = MAP
m.ns = self.marker_ns
m.id = i + 1000
m.type = Marker.CUBE
m.action = Marker.ADD
m.pose.position = Point(*shelf.get_left())
m.pose.position.z = 0.03
m.pose.orientation = Quaternion(*shelf.get_orientation())
m.scale = Vector3(.05, .03, .05)
m.color = self.left_color
ma.markers.append(m)
# right
m = deepcopy(m)
m.id += 10000
m.pose.position = Point(*shelf.get_right())
m.pose.position.z = 0.03
m.color = self.right_color
ma.markers.append(m)
self.marker_pub.publish(ma)
class KnowRob(object):
def __init__(self):
# TODO implement all the things [high]
# TODO use paramserver [low]
self.floors = {}
self.shelf_ids = []
self.separators = {}
self.perceived_frame_id_map = {}
self.action_graph = None
self.tf = TfWrapper()
self.prolog = json_prolog.Prolog()
self.prolog.wait_for_service()
self.query_lock = Lock()
def prolog_query(self, q):
# print('before lock')
with self.query_lock:
print('sending {}'.format(q))
query = self.prolog.query(q)
solutions = [x if x != {} else True for x in query.solutions()]
# if len(solutions) > 1:
# rospy.logwarn('{} returned more than one result'.format(q))
# elif len(solutions) == 0:
# rospy.logwarn('{} returned nothing'.format(q))
query.finish()
print('solutions {}'.format(solutions))
print('----------------------')
return solutions
def remove_http_shit(self, s):
return s.split('#')[-1].split('\'')[0]
def load_barcode_to_mesh_map(self):
self.barcode_to_mesh = json.load(open('../../data/barcode_to_mesh.json'))
def pose_to_prolog(self, pose_stamped):
return '[\'{}\', _, [{},{},{}], [{},{},{},{}]]'.format(pose_stamped.header.frame_id,
pose_stamped.pose.position.x,
pose_stamped.pose.position.y,
pose_stamped.pose.position.z,
pose_stamped.pose.orientation.x,
pose_stamped.pose.orientation.y,
pose_stamped.pose.orientation.z,
pose_stamped.pose.orientation.w)
def prolog_to_pose_msg(self, query_result):
ros_pose = PoseStamped()
ros_pose.header.frame_id = query_result[0]
ros_pose.pose.position = Point(*query_result[2])
ros_pose.pose.orientation = Quaternion(*query_result[3])
return ros_pose
def add_shelf_system(self):
q = 'belief_new_object({}, R), rdf_assert(R, knowrob:describedInMap, iaishop:\'IAIShop_0\', belief_state)'.format(
SHELF_SYSTEM)
shelf_system_id = self.prolog_query(q)[0]['R'].replace('\'', '')
return shelf_system_id
# shelves
def add_shelves(self, shelf_system_id, shelves):
# TODO failure handling
for name, pose in shelves.items():
q = 'belief_new_object({}, ID), ' \
'rdf_assert(\'{}\', knowrob:properPhysicalParts, ID, belief_state),' \
'object_affordance_static_transform(ID, A, [_,_,T,R]),' \
'rdfs_individual_of(A, {})'.format(SHELF_METER, shelf_system_id, PERCEPTION_AFFORDANCE)
solutions = self.prolog_query(q)[0]
pose.pose.position.x -= solutions['T'][0]
pose.pose.position.y -= solutions['T'][1]
pose.pose.position.z -= solutions['T'][2]
object_id = solutions['ID'].replace('\'', '')
q = 'belief_at_update(\'{}\', {})'.format(object_id, self.pose_to_prolog(pose))
solutions = self.prolog_query(q)
return True
def get_objects(self, type):
# TODO failure handling
objects = OrderedDict()
q = 'rdfs_individual_of(R, {}).'.format(type)
solutions = self.prolog_query(q)
for solution in solutions:
object_id = solution['R'].replace('\'', '')
pose_q = 'belief_at(\'{}\', R).'.format(object_id)
believed_pose = self.prolog_query(pose_q)[0]['R']
ros_pose = PoseStamped()
ros_pose.header.frame_id = believed_pose[0]
ros_pose.pose.position = Point(*believed_pose[2])
ros_pose.pose.orientation = Quaternion(*believed_pose[3])
objects[object_id] = ros_pose
return objects
def get_shelves(self):
return self.get_objects(SHELF_METER)
def get_perceived_frame_id(self, object_id):
if object_id not in self.perceived_frame_id_map:
q = 'object_perception_affordance_frame_name(\'{}\', F)'.format(object_id)
self.perceived_frame_id_map[object_id] = self.prolog_query(q)[0]['F'].replace('\'', '')
return self.perceived_frame_id_map[object_id]
def get_object_frame_id(self, object_id):
q = 'object_frame_name(\'{}\', R).'.format(object_id)
return self.prolog_query(q)[0]['R'].replace('\'', '')
# floor
def add_shelf_floors(self, shelf_id, floors):
for position in floors:
if position[1] < 0.13:
if position[2] < 0.2:
layer_type = SHELF_FLOOR_STANDING_GROUND
else:
layer_type = SHELF_FLOOR_STANDING
else:
layer_type = SHELF_FLOOR_MOUNTING
q = 'belief_shelf_part_at(\'{}\', {}, {}, R)'.format(shelf_id, layer_type, position[-1])
self.prolog_query(q)
return True
def get_floor_ids(self, shelf_id):
q = 'rdf_has(\'{}\', knowrob:properPhysicalParts, Floor), ' \
'rdfs_individual_of(Floor, {}), ' \
'object_perception_affordance_frame_name(Floor, Frame).'.format(shelf_id, SHELF_FLOOR)
solutions = self.prolog_query(q)
floors = []
shelf_frame_id = self.get_perceived_frame_id(shelf_id)
for solution in solutions:
floor_id = solution['Floor'].replace('\'', '')
floor_pose = self.tf.lookup_transform(shelf_frame_id, solution['Frame'].replace('\'', ''))
if floor_pose.pose.position.z < 1.2:
floors.append((floor_id, floor_pose))
floors = list(sorted(floors, key=lambda x: x[1].pose.position.z))
self.floors = OrderedDict(floors)
return self.floors
def get_floor_width(self):
# TODO
return 1.0
def get_floor_position(self, floor_id):
return self.floors[floor_id]
def is_floor_too_high(self, floor_id):
return self.get_floor_position(floor_id).pose.position.z > 1.2
def is_bottom_floor(self, floor_id):
return self.get_floor_position(floor_id).pose.position.z < 0.16
def is_hanging_foor(self, floor_id):
q = 'rdfs_individual_of(\'{}\', {})'.format(floor_id, SHELF_FLOOR_MOUNTING)
solutions = self.prolog_query(q)
return len(solutions) > 0
def is_normal_floor(self, floor_id):
return not self.is_bottom_floor(floor_id) and not self.is_hanging_foor(floor_id)
def add_separators(self, floor_id, separators):
for p in separators:
q = 'belief_shelf_part_at(\'{}\', {}, {}, _)'.format(floor_id, SEPARATOR, p.pose.position.x)
self.prolog_query(q)
return True
def add_barcodes(self, floor_id, barcodes):
for barcode, p in barcodes.items():
q = 'belief_shelf_barcode_at(\'{}\', {}, dan(\'{}\'), {}, _)'.format(floor_id, BARCODE,
barcode, p.pose.position.x)
self.prolog_query(q)
def add_separators_and_barcodes(self, floor_id, separators, barcodes):
# update floor height
new_floor_height = np.mean([self.tf.transform_pose(
self.get_perceived_frame_id(floor_id), p).pose.position.z for p in separators])
current_floor_pose = self.tf.lookup_transform(MAP, self.get_object_frame_id(floor_id))
current_floor_pose.pose.position.z += new_floor_height - 0.01
q = 'belief_at_update(\'{}\', {})'.format(floor_id, self.pose_to_prolog(current_floor_pose))
self.prolog_query(q)
for p in separators:
q = 'belief_shelf_part_at(\'{}\', {}, norm({}), _)'.format(floor_id, SEPARATOR, p.pose.position.x)
self.prolog_query(q)
for barcode, p in barcodes.items():
q = 'belief_shelf_barcode_at(\'{}\', {}, dan(\'{}\'), norm({}), _)'.format(floor_id, BARCODE, barcode,
p.pose.position.x)
self.prolog_query(q)
self.start_shelf_separator_perception(self.get_separators(floor_id))
self.finish_action()
self.start_shelf_label_perception(self.get_barcodes(floor_id))
self.finish_action()
def get_separators(self, floor_id):
q = 'findall(S, shelf_layer_separator(\'{}\', S), Ss)'.format(floor_id)
solutions = self.prolog_query(q)
return solutions[0]['Ss']
def get_mounting_bars(self, floor_id):
q = 'findall(S, shelf_layer_mounting_bar(\'{}\', S), Ss)'.format(floor_id)
solutions = self.prolog_query(q)
return solutions[0]['Ss']
def get_barcodes(self, floor_id):
q = 'findall(S, shelf_layer_label(\'{}\', S), Ss)'.format(floor_id)
solutions = self.prolog_query(q)
return solutions[0]['Ss']
def add_mounting_bars_and_barcodes(self, floor_id, separators, barcodes):
if len(separators) > 0:
for p in separators:
q = 'belief_shelf_part_at(\'{}\', {}, norm({}), _)'.format(floor_id, MOUNTING_BAR, p.pose.position.x)
self.prolog_query(q)
else:
for barcode, p in barcodes.items():
q = 'belief_shelf_part_at(\'{}\', {}, norm({}), _)'.format(floor_id, MOUNTING_BAR, p.pose.position.x+0.02)
self.prolog_query(q)
for barcode, p in barcodes.items():
q = 'belief_shelf_barcode_at(\'{}\', {}, dan(\'{}\'), norm({}), _)'.format(floor_id, BARCODE, barcode,
p.pose.position.x)
self.prolog_query(q)
self.start_shelf_bar_perception(self.get_mounting_bars(floor_id))
self.finish_action()
self.start_shelf_label_perception(self.get_barcodes(floor_id))
self.finish_action()
def get_facings(self, floor_id):
q = 'findall([F, P, W, L], (shelf_facing(\'{}\', F), ' \
'shelf_facing_product_type(F,P), ' \
'comp_facingWidth(F,literal(type(_, W))), ' \
'(rdf_has(F, shop:leftSeparator, L); rdf_has(F, shop:mountingBarOfFacing, L))),' \
'Facings).'.format(floor_id)
solutions = self.prolog_query(q)[0]
facings = {}
for facing_id, product, width, left_separator_id in solutions['Facings']:
facing_pose = self.tf.lookup_transform(self.get_perceived_frame_id(floor_id),
self.get_object_frame_id(facing_id))
facings[facing_id] = (facing_pose, product, float(width), left_separator_id)
return facings
def add_object(self, facing_id):
q = 'product_spawn_front_to_back(\'{}\', ObjId)'.format(facing_id)
self.prolog_query(q)
def save_beliefstate(self, path=None):
if path is None:
path = '{}/data/beliefstate.owl'.format(RosPack().get_path('refills_first_review'))
q = 'rdf_save(\'{}\', belief_state)'.format(path)
self.prolog_query(q)
def save_action_graph(self, path=None):
if path is None:
path = '{}/data/actions.owl'.format(RosPack().get_path('refills_first_review'))
q = 'rdf_save(\'{}\', [graph(\'LoggingGraph\')])'.format(path)
self.prolog_query(q)
def start_everything(self):
a = 'http://knowrob.org/kb/knowrob.owl#RobotExperiment'
self.action_graph = ActionGraph.start_experiment(self, a)
def start_shelf_system_mapping(self, object_acted_on=None):
"""
:param object_acted_on: shelf system id
:return:
"""
a = 'http://knowrob.org/kb/shop.owl#ShelfSystemMapping'
self.action_graph = self.action_graph.add_sub_action(self, a, object_acted_on=object_acted_on)
def start_shelf_frame_mapping(self, object_acted_on=None):
a = 'http://knowrob.org/kb/shop.owl#ShelfFrameMapping'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_action(a, object_acted_on=object_acted_on)
def start_shelf_layer_mapping(self, object_acted_on=None):
"""
:param object_acted_on: floor_id
:return:
"""
a = 'http://knowrob.org/kb/shop.owl#ShelfLayerMapping'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_action(a, object_acted_on=object_acted_on)
def start_finding_shelf_layer(self):
a = 'http://knowrob.org/kb/shop.owl#FindingShelfLayer'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_action(a)
def start_finding_shelf_layer_parts(self):
a = 'http://knowrob.org/kb/shop.owl#FindingShelfLayerParts'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_action(a)
def start_shelf_layer_perception(self, detected_objects=None):
"""
:param detected_objects: list of floor_id
:return:
"""
a = 'http://knowrob.org/kb/shop.owl#ShelfLayerPerception'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_event(a, detected_objects=detected_objects)
def start_shelf_layer_counting(self):
a = 'http://knowrob.org/kb/shop.owl#ShelfLayerCounting'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_action(a)
def start_move_to_shelf_frame(self, goal_location=None):
"""
:param goal_location: shelf id
:return:
"""
a = 'http://knowrob.org/kb/shop.owl#MoveToShelfFrame'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_action(a, goal_location=goal_location)
def start_move_to_shelf_frame_end(self, goal_location=None):
"""
:param goal_location: pose
:return:
"""
a = 'http://knowrob.org/kb/shop.owl#MoveToShelfFrameEnd'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_action(a, goal_location=goal_location)
def start_move_to_shelf_layer(self, goal_location=None):
"""
:param goal_location: floor id
:return:
"""
a = 'http://knowrob.org/kb/shop.owl#MoveToShelfLayer'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_action(a, goal_location=goal_location)
def start_looking_at_location(self, goal_location=None):
"""
:param goal_location: pose
:return:
"""
a = 'http://knowrob.org/kb/knowrob.owl#LookingAtLocation'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_action(a, goal_location=goal_location)
def start_base_movement(self, goal_location=None):
"""
:param goal_location: pose
:return:
"""
a = 'http://knowrob.org/kb/motions.owl#BaseMovement'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_motion(a, goal_location=goal_location)
def start_head_movement(self, goal_location=None):
"""
:param goal_location: pose
:return:
"""
a = 'http://knowrob.org/kb/motions.owl#HeadMovement'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_motion(a, goal_location=goal_location)
def start_shelf_separator_perception(self, detected_objects=None):
a = 'http://knowrob.org/kb/shop.owl#ShelfSeparatorPerception'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_event(a, detected_objects=detected_objects)
def start_shelf_bar_perception(self, detected_objects=None):
a = 'http://knowrob.org/kb/shop.owl#ShelfBarPerception'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_event(a, detected_objects=detected_objects)
def start_shelf_label_perception(self, detected_objects=None):
a = 'http://knowrob.org/kb/shop.owl#ShelfLabelPerception'
if self.action_graph is not None:
self.action_graph = self.action_graph.add_sub_event(a, detected_objects=detected_objects)
def finish_action(self):
if self.action_graph is not None:
self.action_graph = self.action_graph.finish()
class SeparatorClustering(object):
def __init__(self, knowrob):
self.knowrob = knowrob
# TODO use paramserver [low]
self.tf = TfWrapper(6)
self.marker_pub = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=10)
self.detections = []
self.map_frame_id = 'map'
self.separator_maker_color = ColorRGBA(.8, .8, .8, .8)
self.separator_maker_scale = Vector3(.01, .5, .05)
self.min_samples = 1
self.max_dist = 0.02
self.hanging = False
self.listen = False
self.separator_sub = rospy.Subscriber(
'/separator_marker_detector_node/data_out',
SeparatorArray,
self.separator_cb,
queue_size=10)
def start_listening_separators(
self, floor_id, topic='/separator_marker_detector_node/data_out'):
self.hanging = False
# self.topic = topic
self.current_floor_id = floor_id
self.listen = True
self.detections = []
self.marker_ns = 'separator_{}'.format(floor_id)
def start_listening_mounting_bars(self, floor_id):
self.start_listening_separators(floor_id, topic='/muh')
self.hanging = True
def stop_listening(self):
self.listen = False
# self.separator_sub.unregister()
try:
rospy.wait_for_message('/refills_wrist_camera/image_color',
rospy.AnyMsg,
timeout=1)
except ROSException as e:
rospy.loginfo('camera offline; \'detecting\' separators anyway')
self.fake_detection()
separators = self.cluster()
return separators
def separator_cb(self, separator_array):
if self.listen:
frame_id = self.knowrob.get_perceived_frame_id(
self.current_floor_id)
for separator in separator_array.separators:
p = self.tf.transform_pose(frame_id, separator.separator_pose)
if p is not None and 0.04 <= p.pose.position.x and p.pose.position.x <= 0.96:
self.detections.append([
p.pose.position.x, p.pose.position.y, p.pose.position.z
])
def cluster(self):
if not self.hanging:
self.hacky()
data = np.array(self.detections)
separators = []
old_frame_id = self.knowrob.get_perceived_frame_id(
self.current_floor_id)
if len(data) == 0:
rospy.logwarn('no separators detected')
else:
clusters = DBSCAN(eps=self.max_dist,
min_samples=self.min_samples).fit(data)
labels = np.unique(clusters.labels_)
rospy.loginfo('detected {} separators'.format(len(labels)))
for i, label in enumerate(labels):
if label != -1:
separator = PoseStamped()
separator.header.frame_id = old_frame_id
separator.pose.position = Point(*self.cluster_to_separator(
data[clusters.labels_ == label]))
separator.pose.orientation = Quaternion(
*quaternion_about_axis(-np.pi / 2, [0, 0, 1]))
if 0.0 <= separator.pose.position.x and separator.pose.position.x <= 1:
separators.append(separator)
return separators
def cluster_to_separator(self, separator_cluster):
return separator_cluster.mean(axis=0)
def fake_detection(self):
if not self.hanging:
num_fake_separators = 15
frame_id = self.knowrob.get_perceived_frame_id(
self.current_floor_id)
for i in range(num_fake_separators):
for j in range(self.min_samples + 1):
p = PoseStamped()
p.header.frame_id = frame_id
if self.hanging:
p.pose.position.x = (i + 0.5) / (num_fake_separators -
1)
else:
p.pose.position.x = i / (num_fake_separators - 1)
p.pose.position.y = 0
p.pose.orientation = Quaternion(0, 0, 0, 1)
if (self.hanging and
i < num_fake_separators - 1) or not self.hanging:
self.detections.append([
p.pose.position.x, p.pose.position.y,
p.pose.position.z
])
def hacky(self):
for i in range(200):
self.detections.append([0, 0, 0])
self.detections.append([1, 0, 0])
for i in range(20):
self.detections.append([0.01, 0, 0])
self.detections.append([0.99, 0, 0])
class RoboSherlock(object):
def __init__(self, knowrob):
self.knowrob = knowrob
# TODO use paramserver [low]
# TODO implement all the things [high]
self.separator_detection = SeparatorClustering(knowrob)
self.baseboard_detection = BaseboardDetector()
self.barcode_detection = BarcodeDetector(knowrob)
self.ring_light_srv = rospy.ServiceProxy('ring_light_switch/setbool',
SetBool)
self.floor_detection = True
try:
rospy.wait_for_service('/RoboSherlock/json_query', 1)
self.robosherlock_service = rospy.ServiceProxy(
'/RoboSherlock/json_query', RSQueryService)
self.robosherlock = True
except ROSException as e:
rospy.logwarn(
'robosherlock not available; activating fake perception')
self.robosherlock = False
self.tf = TfWrapper()
def set_ring_light(self, value=True):
req = SetBoolRequest()
req.data = value
try:
self.ring_light_srv.call(req)
except:
rospy.logwarn('ring_light_switch not available')
def start_separator_detection(self, floor_id):
self.set_ring_light(True)
self.separator_detection.start_listening_separators(floor_id)
def start_mounting_bar_detection(self, floor_id):
self.set_ring_light(True)
self.separator_detection.start_listening_mounting_bars(floor_id)
def stop_separator_detection(self):
return self.separator_detection.stop_listening()
def start_baseboard_detection(self):
self.set_ring_light(True)
self.baseboard_detection.start_listening()
def stop_baseboard_detection(self):
return self.baseboard_detection.stop_listening()
def start_barcode_detection(self, shelf_id, floor_id):
self.set_ring_light(True)
self.barcode_detection.start_listening(shelf_id, floor_id)
pass
def stop_barcode_detection(self):
return self.barcode_detection.stop_listening()
def detect_floors(self, shelf_id):
# TODO
rospy.loginfo('detecting floors for shelf {}'.format(shelf_id))
rospy.logwarn('floor detection not implemented')
return FLOORS[shelf_id]
def start_floor_detection(self, shelf_id):
self.set_ring_light(False)
if self.robosherlock and self.floor_detection:
req = RSQueryServiceRequest()
q = {
"scan": {
"type": "shelf",
"location": self.knowrob.get_perceived_frame_id(shelf_id),
"command": "start"
}
}
req.query = json.dumps(q)
self.robosherlock_service.call(req)
def stop_floor_detection(self, shelf_id):
shelf_frame = self.knowrob.get_perceived_frame_id(shelf_id)
if self.robosherlock and self.floor_detection:
req = RSQueryServiceRequest()
q = {
'scan': {
'type': 'shelf',
'location': shelf_frame,
'command': 'stop'
}
}
req.query = json.dumps(q)
result = self.robosherlock_service.call(req)
floors = []
for floor in result.answer:
p = message_converter.convert_dictionary_to_ros_message(
'geometry_msgs/PoseStamped',
json.loads(floor)['poses'][0]['pose_stamped'])
p = self.tf.transform_pose(shelf_frame, p)
floors.append([0, p.pose.position.y, p.pose.position.z])
floors = list(sorted(floors, key=lambda x: x[-1]))
floors = [x for x in floors if x[-1] > 0.3]
floors = [FLOORS[0][0]] + floors
print('detected shelfs at heights: {}'.format(floors))
# TODO remove this if floor detection works
shelf_pose = self.tf.lookup_transform(MAP, shelf_frame)
floors = FLOORS[int(shelf_pose.pose.position.x)]
else:
shelf_pose = self.tf.lookup_transform(MAP, shelf_frame)
floors = FLOORS[int(shelf_pose.pose.position.x)]
return floors
def count(self,
product,
width,
left_separator,
perceived_shelf_frame_id,
facing_type='standing'):
self.set_ring_light(False)
if self.robosherlock:
ls = self.tf.lookup_transform(
perceived_shelf_frame_id,
self.knowrob.get_perceived_frame_id(left_separator))
q = {
'detect': {
'type': product,
'pose_stamped': convert_ros_message_to_dictionary(ls),
'shelf_type': facing_type,
'width': width,
'location': perceived_shelf_frame_id
}
}
print(q)
req = RSQueryServiceRequest()
req.query = json.dumps(q)
rospy.sleep(0.4)
result = self.robosherlock_service.call(req)
print(result)
count = len(result.answer)
else:
count = int(np.random.random() * 4)
# self.set_ring_light(True)
return count