def stop_perception(self, interrupted):
result = DetectFacingsResult()
if interrupted:
result.error = DetectFacingsResult.ABORTED
print_with_prefix('interrupted', self.prefix)
else:
result.error = DetectFacingsResult.SUCCESS
separators = self.get_robosherlock().stop_separator_detection(
self.current_goal.id)
barcodes = self.get_robosherlock().stop_barcode_detection(
self.current_goal.id)
update_shelf_system_pose(self.get_knowrob(), self.current_goal.id,
separators)
self.get_knowrob().update_shelf_layer_position(
self.current_goal.id, separators)
self.get_knowrob().create_unknown_barcodes(barcodes)
self.get_knowrob().add_separators_and_barcodes(
self.current_goal.id, separators, barcodes)
result.ids = self.get_knowrob().get_facing_ids_from_layer(
self.current_goal.id).keys()
print_with_prefix('finished', self.prefix)
return result
def cluster(self, visualize=False):
"""
:param visualize: whether or not the debug plut should be shown !this might result in a exception because pyplot does not like multithreading!
:type visualize: bool
:return: list of PoseStamped
:rtype: list
"""
data = np.array(self.detections)
separators = []
# old_frame_id = self.get_frame_id()
if len(data) == 0:
print_with_prefix('no separators detected', self.prefix)
else:
clusters = DBSCAN(eps=self.max_dist,
min_samples=self.min_samples).fit(data)
labels = np.unique(clusters.labels_)
print_with_prefix('detected {} separators'.format(len(labels)),
self.prefix)
for i, label in enumerate(labels):
if label != -1:
separator = PoseStamped()
separator.header.frame_id = 'map'
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]))
separators.append(separator)
if visualize:
detections = np.array(self.detections)
self.visualize_detections(clusters.labels_, detections,
self.pose_list_to_np(separators))
return separators
def stop_listening(self):
"""
:return: list of PoseStamped
:rtype: list
"""
self.listen = False
separators = self.cluster()
# separators.extend(self.get_edge_separators())
print_with_prefix('stopped', self.prefix)
return separators
def query_shelf_systems_cb(self, data):
"""
:type data: QueryShelfSystemsRequest
:rtype: QueryShelfSystemsResponse
"""
prefix = 'query_shelf_systems'
print_with_prefix('called', prefix)
r = QueryShelfSystemsResponse()
r.ids = self.get_knowrob().get_shelf_system_ids()
self.wait_for_update()
return r
def start_perception(self, goal):
"""
:type goal: DetectShelfLayersGoal
"""
result = DetectShelfLayersResult()
print_with_prefix('started', self.prefix)
if not self.get_knowrob().shelf_system_exists(goal.id):
result.error = DetectShelfLayersResult.INVALID_ID
result.error_msg = 'invalid shelf id: {}'.format(goal.id)
print_with_prefix('invalid id', self.prefix)
return result
self.get_robosherlock().start_detect_shelf_layers(goal.id)
self.current_goal = goal
def start_perception(self, goal):
"""
:type goal: DetectFacingsGoal
"""
print_with_prefix('started', self.prefix)
result = DetectFacingsResult()
if not self.get_knowrob().shelf_layer_exists(goal.id):
result.error = DetectFacingsResult.INVALID_ID
result.error_msg = 'invalid layer id: {}'.format(goal.id)
return result
self.get_robosherlock().start_separator_detection(goal.id)
self.get_robosherlock().start_barcode_detection(goal.id)
self.current_goal = goal
def load_initial_beliefstate(self):
self.initial_beliefstate = rospy.get_param('~initial_beliefstate')
self.clear_beliefstate(self.initial_beliefstate)
if self.start_episode(self.initial_beliefstate):
print_with_prefix(
'loaded initial beliefstate {}'.format(
self.initial_beliefstate), self.prefix)
self.reset_object_state_publisher.call(TriggerRequest())
return True
else:
print_with_prefix(
'error loading initial beliefstate {}'.format(
self.initial_beliefstate), self.prefix)
return False
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)
def stop_perception(self, interrupted):
result = DetectShelfLayersResult()
if interrupted:
result.error = DetectShelfLayersResult.ABORTED
print_with_prefix('interrupted', self.prefix)
else:
result.error = DetectShelfLayersResult.SUCCESS
shelf_layer_heights = self.get_robosherlock(
).stop_detect_shelf_layers(self.current_goal.id)
self.get_knowrob().add_shelf_layers(self.current_goal.id,
shelf_layer_heights)
result.ids = self.get_knowrob().get_shelf_layer_from_system(
self.current_goal.id).keys()
print_with_prefix('finished', self.prefix)
return result
def query_facings_cb(self, data):
"""
:type data: QueryFacingsRequest
:rtype: QueryFacingsResponse
"""
prefix = 'query_facings'
print_with_prefix('called', prefix)
r = QueryFacingsResponse()
if self.get_knowrob().shelf_layer_exists(data.id):
r.error = QueryFacingsResponse.SUCCESS
r.ids = self.get_knowrob().get_facing_ids_from_layer(data.id).keys()
else:
print_with_prefix('invalid id', prefix)
r.error = QueryFacingsResponse.INVALID_ID
self.wait_for_update()
return r
def execute_cb(self, goal):
"""
:type goal: MoveGoal
"""
with self.lock.acquire_timeout(0) as got_lock:
if got_lock and not self.cancel_next_goal:
self.my_state = Status.RUNNING
self.goal_queue.put(goal)
self.result_queue.get()()
else:
self.my_state = Status.FAILURE
r = self._as.action_server.ActionResultType()
r.error = DetectShelfLayersResult.SERVER_BUSY
print_with_prefix('rejected goal because server busy server busy', self.action_name)
self._as.set_aborted(r)
self.cancel_next_goal = False
def query_detect_facings_path_cb(self, data):
"""
:type data: QueryDetectFacingsPathRequest
:rtype: QueryDetectFacingsPathResponse
"""
print_with_prefix('called', 'query_detect_facings_path')
r = QueryDetectFacingsPathResponse()
if self.get_knowrob().shelf_layer_exists(data.id):
r.error = QueryDetectFacingsPathResponse.SUCCESS
try:
r.path = self.paths.get_detect_facings_path(data.id)
except:
rospy.logerr('path error')
else:
r.error = QueryDetectFacingsPathResponse.INVALID_ID
self.wait_for_update()
return r
def query_count_products_posture_cb(self, data):
"""
:type data: QueryCountProductsPostureRequest
:rtype: QueryCountProductsPostureResponse
"""
print_with_prefix('called', 'query_count_products_posture')
r = QueryCountProductsPostureResponse()
if self.get_knowrob().facing_exists(data.id):
r.error = QueryCountProductsPostureResponse.SUCCESS
# try:
r.posture = self.paths.get_count_product_posture(data.id)
# except:
# rospy.logerr('path error')
else:
r.error = QueryCountProductsPostureResponse.INVALID_ID
self.wait_for_update()
return r
def start_perception(self, goal):
"""
:type goal: CountProductsGoal
"""
print_with_prefix('started', self.prefix)
result = CountProductsResult()
if self.get_knowrob().facing_exists(goal.id):
result.error = CountProductsResult.SUCCESS
result.count = self.get_robosherlock().count_product(goal.id)
self.get_knowrob().add_objects(goal.id, result.count)
print_with_prefix('counted {} times'.format(result.count),
self.prefix)
print_with_prefix('finished', self.prefix)
return result
else:
result.error = DetectFacingsResult.INVALID_ID
result.error_msg = 'invalid facing id: {}'.format(goal.id)
print_with_prefix('invalid id', self.prefix)
return result
def finish_perception_cb(data):
"""
:type data: FinishPerceptionRequest
:rtype: FinishPerceptionResponse
"""
prefix = 'finish_perception'
print_with_prefix('called', prefix)
lock = Blackboard().lock # type: TimeoutLock
result = FinishPerceptionResponse()
with lock.acquire_timeout(0) as got_lock:
if not got_lock:
Blackboard().finished = True
result.error = FinishPerceptionResponse.SUCCESS
else:
result.error = FinishPerceptionResponse.NO_RUNNING_JOB
print_with_prefix('no running job', prefix)
print_with_prefix('finished', prefix)
return result
def print_with_prefix(self, msg):
print_with_prefix(msg, self.prefix)
def print_with_prefix(self, msg):
"""
:type msg: str
"""
print_with_prefix(msg, self.prefix)
def canceled(self):
result = DetectFacingsResult()
result.error = DetectShelfLayersResult.ABORTED
print_with_prefix('canceled', self.prefix)
return result
