def delete_annotations(self, request):
'''
Deletes the given annotations and its data from database.
@param request: Service request.
'''
response = DeleteAnnotationsResponse()
if len(request.annotations) == 0:
return self.service_error(response, "No annotation ids on request; you must be kidding!")
annot_data_ids = [a.data_id for a in request.annotations]
query = {'id': {'$in': [unique_id.toHexString(id) for id in annot_data_ids]}}
rospy.logdebug("Removing %d annotations data with query %s" % (len(annot_data_ids), query))
data_removed = self.data_collection.remove(query)
annotation_ids = [a.id for a in request.annotations]
query = {'id': {'$in': [unique_id.toHexString(id) for id in annotation_ids]}}
rospy.logdebug("Removing %d annotations with query %s" % (len(annotation_ids), query))
removed = self.anns_collection.remove(query)
rospy.loginfo("%d annotations and %d data removed from database" % (removed, data_removed))
if removed != len(annotation_ids):
# Not all the doomed annotations where found on database. That's not terrible; can happen
# easily, for example as explained here: https://github.com/corot/world_canvas/issues/38
# TODO: but we should notify the client lib somehow
rospy.logwarn("Requested (%d) and deleted (%d) annotations counts doesn't match"
% (len(annotation_ids), removed))
return self.service_success(response)
def delete_annotations(self, request):
"""
Deletes the given annotations and its data from database.
@param request: Service request.
"""
response = DeleteAnnotationsResponse()
if len(request.annotations) == 0:
return self.service_error(response, "No annotation ids on request; you must be kidding!")
annot_data_ids = [a.data_id for a in request.annotations]
query = {"id": {"$in": [unique_id.toHexString(id) for id in annot_data_ids]}}
rospy.logdebug("Removing %d annotations data with query %s" % (len(annot_data_ids), query))
data_removed = self.data_collection.remove(query)
annotation_ids = [a.id for a in request.annotations]
query = {"id": {"$in": [unique_id.toHexString(id) for id in annotation_ids]}}
rospy.logdebug("Removing %d annotations with query %s" % (len(annotation_ids), query))
removed = self.anns_collection.remove(query)
rospy.loginfo("%d annotations and %d data removed from database" % (removed, data_removed))
if removed != len(annotation_ids):
# Not all the doomed annotations where found on database. That's not terrible; can happen
# easily, for example as explained here: https://github.com/corot/world_canvas/issues/38
# TODO: but we should notify the client lib somehow
rospy.logwarn(
"Requested (%d) and deleted (%d) annotations counts doesn't match" % (len(annotation_ids), removed)
)
return self.service_success(response)
def save(self):
'''
Save to database current annotations list with their associated data. Also remove from database
the annotations doomed by delete method, if any.
WARN/TODO: we are ignoring the case of N annotations - 1 data!
:raises WCFError: If something went wrong.
'''
rospy.loginfo("Requesting server to save annotations")
annotations = []
annots_data = []
# This brittle saving procedure requires parallelly ordered annotations and data vectors
# As this don't need to be the case, we must short them; but we need a better saving procedure (TODO)
for a in self._annotations:
for d in self._annots_data:
if a.data_id == d.id:
rospy.logdebug("Add annotation for saving with uuid '%s'",
unique_id.toHexString(a.id))
rospy.logdebug("Add annot. data for saving with uuid '%s'",
unique_id.toHexString(d.id))
annotations.append(a)
annots_data.append(d)
break
# Do at least a rudimentary check
if not (len(self._annotations) == len(self._annots_data) ==
len(annotations) == len(annots_data)):
message = "Incoherent annotation and data sizes: %d != %d != %d != %d" \
% (len(self._annotations), len(self._annots_data), len(annotations), len(annots_data))
rospy.logerr(message)
raise WCFError(message)
# Request server to save current annotations list, with its data
save_data_srv = self._get_service_handle(
'save_annotations_data', world_canvas_msgs.srv.SaveAnnotationsData)
rospy.loginfo("Requesting server to save annotations")
response = save_data_srv(annotations, annots_data)
if not response.result:
message = "Server reported an error: %s" % response.message
rospy.logerr(message)
raise WCFError(message)
# We must also remove from database the annotations doomed by delete method, if any
if len(self._annots_to_delete) > 0:
del_anns_srv = self._get_service_handle(
'delete_annotations', world_canvas_msgs.srv.DeleteAnnotations)
rospy.loginfo("Requesting server to delete %d doomed annotations" %
len(self._annots_to_delete))
response = del_anns_srv(self._annots_to_delete)
if not response.result:
message = "Server reported an error: %s" % response.message
rospy.logerr(message)
raise WCFError(message)
self._saved = True
def test_non_blocking_call(self):
self._non_blocking_event = threading.Event()
msg_id = self.testies(generate_request_message(),
callback=self.callback)
self._non_blocking_event.wait()
self.assertEquals(unique_id.toHexString(msg_id),
unique_id.toHexString(self._non_blocking_msg_id))
self.assertEquals(
self._non_blocking_msg_response.data, self.response_message,
"Should have received '%s' but got '%s'" %
(self.response_message, self._non_blocking_msg_response.data))
def save(self):
'''
Save to database current annotations list with their associated data. Also remove from database
the annotations doomed by delete method, if any.
WARN/TODO: we are ignoring the case of N annotations - 1 data!
@raise WCFError: If something went wrong.
'''
rospy.loginfo("Requesting server to save annotations")
annotations = []
annots_data = []
# This brittle saving procedure requires parallelly ordered annotations and data vectors
# As this don't need to be the case, we must short them; but we need a better saving procedure (TODO)
for a in self._annotations:
for d in self._annots_data:
if a.data_id == d.id:
rospy.logdebug("Add annotation for saving with uuid '%s'", unique_id.toHexString(a.id))
rospy.logdebug("Add annot. data for saving with uuid '%s'", unique_id.toHexString(d.id))
annotations.append(a)
annots_data.append(d)
break
# Do at least a rudimentary check
if not (len(self._annotations) == len(self._annots_data) == len(annotations) == len(annots_data)):
message = "Incoherent annotation and data sizes: %d != %d != %d != %d" \
% (len(self._annotations), len(self._annots_data), len(annotations), len(annots_data))
rospy.logerr(message)
raise WCFError(message)
# Request server to save current annotations list, with its data
save_data_srv = self._get_service_handle('save_annotations_data', world_canvas_msgs.srv.SaveAnnotationsData)
rospy.loginfo("Requesting server to save annotations")
response = save_data_srv(annotations, annots_data)
if not response.result:
message = "Server reported an error: %s" % response.message
rospy.logerr(message)
raise WCFError(message)
# We must also remove from database the annotations doomed by delete method, if any
if len(self._annots_to_delete) > 0:
del_anns_srv = self._get_service_handle('delete_annotations', world_canvas_msgs.srv.DeleteAnnotations)
rospy.loginfo("Requesting server to delete %d doomed annotations" % len(self._annots_to_delete))
response = del_anns_srv(self._annots_to_delete)
if not response.result:
message = "Server reported an error: %s" % response.message
rospy.logerr(message)
raise WCFError(message)
self._saved = True
def get_annotations(self, request):
response = GetAnnotationsResponse()
# Compose query concatenating filter criteria in an '$and' operator
# Except world, all criteria are lists: operator '$in' makes a N to N matching
# Empty fields are ignored
query = {'$and':[]}
query['$and'].append({'world': {'$in': [request.world]}})
if len(request.ids) > 0:
query['$and'].append({'id': {'$in': [unique_id.toHexString(id) for id in request.ids]}})
if len(request.names) > 0:
query['$and'].append({'name': {'$in': request.names}})
if len(request.types) > 0:
query['$and'].append({'type': {'$in': request.types}})
if len(request.keywords) > 0:
query['$and'].append({'keywords': {'$in': request.keywords}})
if len(request.relationships) > 0:
query['$and'].append({'relationships': {'$in': [unique_id.toHexString(r) for r in request.relationships]}})
# Execute the query and retrieve results
rospy.logdebug("Find annotations with query %s" % query)
matching_anns = self.anns_collection.query(query)
i = 0
while True:
try:
response.annotations.append(matching_anns.next()[0])
i += 1
except StopIteration:
if (i == 0):
rospy.loginfo("No annotations found")
return self.service_success(response) # we don't consider this an error
break
# if (len(matching_anns) != len(matching_data)):
# # we consider this an error by now, as we assume a 1 to 1 relationship;
# # but in future implementations this will change, probably, to a N to 1 relationship
# rospy.logerr("Pulled annotations and associated data don't match (%lu != %lu)",
# len(matching_anns), len(matching_data))
# response.message = "Pulled annotations and associated data don't match"
# response.result = False
# return response
#
# response.annotations = matching_anns
# response.data = matching_data
rospy.loginfo("%lu annotations loaded" % i)
return self.service_success(response)
def getAnnotations(self, request):
response = GetAnnotationsResponse()
# Compose query concatenating filter criteria in an '$and' operator
# Keywords and relationships are lists: operator '$in' makes a N to N matching
# Empty fields are ignored
query = {'$and':[]}
query['$and'].append({'world_id': {'$in': [unique_id.toHexString(request.world_id)]}})
if len(request.ids) > 0:
query['$and'].append({'id': {'$in': [unique_id.toHexString(id) for id in request.ids]}})
if len(request.types) > 0:
query['$and'].append({'type': {'$in': request.types}})
if len(request.keywords) > 0:
query['$and'].append({'keywords': {'$in': request.keywords}})
if len(request.relationships) > 0:
query['$and'].append({'relationships': {'$in': [unique_id.toHexString(r) for r in request.relationships]}})
# Execute the query and retrieve results
matching_anns = self.anns_collection.query(query)
i = 0
while True:
try:
response.annotations.append(matching_anns.next()[0])
i += 1
except StopIteration:
if (i == 0):
rospy.loginfo("No annotations found for query %s" % query)
response.result = True # we don't consider this an error
return response
break
# if (len(matching_anns) != len(matching_data)):
# # we consider this an error by now, as we assume a 1 to 1 relationship;
# # but in future implementations this will change, probably, to a N to 1 relationship
# rospy.logerr("Pulled annotations and associated data don't match (%lu != %lu)",
# len(matching_anns), len(matching_data))
# response.message = "Pulled annotations and associated data don't match"
# response.result = False
# return response
#
# response.annotations = matching_anns
# response.data = matching_data
rospy.loginfo("%lu annotations loaded" % i)
response.result = True
return response
def get_annotations(self, request):
response = GetAnnotationsResponse()
# Compose query concatenating filter criteria in an '$and' operator
# Except world, all criteria are lists: operator '$in' makes a N to N matching
# Empty fields are ignored
query = {"$and": []}
query["$and"].append({"world": {"$in": [request.world]}})
if len(request.ids) > 0:
query["$and"].append({"id": {"$in": [unique_id.toHexString(id) for id in request.ids]}})
if len(request.names) > 0:
query["$and"].append({"name": {"$in": request.names}})
if len(request.types) > 0:
query["$and"].append({"type": {"$in": request.types}})
if len(request.keywords) > 0:
query["$and"].append({"keywords": {"$in": request.keywords}})
if len(request.relationships) > 0:
query["$and"].append({"relationships": {"$in": [unique_id.toHexString(r) for r in request.relationships]}})
# Execute the query and retrieve results
rospy.logdebug("Find annotations with query %s" % query)
matching_anns = self.anns_collection.query(query)
i = 0
while True:
try:
response.annotations.append(matching_anns.next()[0])
i += 1
except StopIteration:
if i == 0:
rospy.loginfo("No annotations found")
return self.service_success(response) # we don't consider this an error
break
# if (len(matching_anns) != len(matching_data)):
# # we consider this an error by now, as we assume a 1 to 1 relationship;
# # but in future implementations this will change, probably, to a N to 1 relationship
# rospy.logerr("Pulled annotations and associated data don't match (%lu != %lu)",
# len(matching_anns), len(matching_data))
# response.message = "Pulled annotations and associated data don't match"
# response.result = False
# return response
#
# response.annotations = matching_anns
# response.data = matching_data
rospy.loginfo("%lu annotations loaded" % i)
return self.service_success(response)
def _internal_callback(self, msg):
'''
:param msg: message returned from the server (with pair id etc)
:type msg: ServicePairResponse
'''
# Check if it is a blocking call that has requested it.
key = unique_id.toHexString(msg.id)
already_handled = False
non_blocking_request_handler = None
self._lock.acquire()
try:
request_handler = self._request_handlers[key]
request_handler.response = msg.response
if isinstance(request_handler, BlockingRequestHandler):
request_handler.event.set()
already_handled = True
else: # NonBlocking
# make a copy and delete so we can release the lock. Process after.
non_blocking_request_handler = request_handler.copy()
del self._request_handlers[key]
except KeyError:
already_handled = True # it's either a blocking, or a non-blocking call handled by the timeout
self._lock.release()
if not already_handled:
# Could use EAFP approach here since they will almost never be None, but this is more readable
if non_blocking_request_handler.callback is not None:
request_handler.callback(msg.id, msg.response)
def __call__(self, msg, timeout=None, callback=None, error_callback=None):
'''
Initiates and executes the client request to the server. The type of arguments
supplied determines whether to apply blocking or non-blocking behaviour.
@param msg : the request message
@type <name>Request
@param timeout : time to wait for data
@type rospy.Duration
@param callback : user callback invoked for responses of non-blocking calls
@type method with arguments (uuid_msgs.UniqueID, <name>Response)
@return msg/id : for blocking calls it is the response message, for non-blocking it is the unique id
@rtype <name>Response/uuid_msgs.UniqueID
'''
pair_request_msg = self.ServicePairRequest()
pair_request_msg.id = unique_id.toMsg(unique_id.fromRandom())
pair_request_msg.request = msg
key = unique_id.toHexString(pair_request_msg.id)
if callback == None and error_callback == None:
self._request_handlers[key] = BlockingRequestHandler(key)
return self._make_blocking_call(self._request_handlers[key],
pair_request_msg, timeout)
else:
request_handler = NonBlockingRequestHandler(
key, callback, error_callback)
self._request_handlers[key] = request_handler.copy()
self._make_non_blocking_call(request_handler, pair_request_msg,
timeout)
return pair_request_msg.id
def _internal_callback(self, msg):
'''
@param msg : message returned from the server (with pair id etc)
@type self.ServicePairResponse
'''
# Check if it is a blocking call that has requested it.
key = unique_id.toHexString(msg.id)
already_handled = False
non_blocking_request_handler = None
self._lock.acquire()
try:
request_handler = self._request_handlers[key]
request_handler.response = msg.response
if isinstance(request_handler, BlockingRequestHandler):
request_handler.event.set()
already_handled = True
else: # NonBlocking
# make a copy and delete so we can release the lock. Process after.
non_blocking_request_handler = request_handler.copy()
del self._request_handlers[key]
except KeyError:
already_handled = True # it's either a blocking, or a non-blocking call handled by the timeout
self._lock.release()
if not already_handled:
# Could use EAFP approach here since they will almost never be None, but this is more readable
if non_blocking_request_handler.callback is not None:
request_handler.callback(msg.id, msg.response)
def get_data(self, annotation):
'''
Get the associated data (ROS message) of a given annotation.
@returns The ROS message associated to the given annotation or None if it was not found.
@raise WCFError: If something else went wrong.
'''
for d in self._annots_data:
if d.id == annotation.data_id:
# Keep the class of the messages to be published; we need it later when deserializing them
msg_class = roslib.message.get_message_class(d.type)
if msg_class is None:
# This could happen if the message type is wrong or not known for this node (i.e. its
# package is not on ROS_PACKAGE_PATH). Both cases are really weird in the client side.
message = "Data type '%s' definition not found" % d.type
rospy.logerr(message)
raise WCFError(message)
try:
object = deserialize_msg(d.data, msg_class)
except SerializationError as e:
message = "Deserialization failed: %s" % str(e)
rospy.logerr(message)
raise WCFError(message)
return object
rospy.logwarn("Data uuid not found: " + unique_id.toHexString(annotation.data_id))
return None
def _flag_resource_trackers(self,
resources,
tracking,
allocated,
high_priority_flag=False):
'''
Update the flags in the resource trackers for each resource. This is used to
follow whether a particular resource is getting tracked, or is already allocated so
that we can determine if new requests should be made and at what priority.
'''
for resource in resources:
# do a quick check to make sure individual resources haven't been previously allocated, and then lost
# WARNING : this unallocated check doesn't actually work - the requester isn't sending us back this info yet.
if rocon_uri.parse(
resource.uri
).name.string == concert_msgs.Strings.SCHEDULER_UNALLOCATED_RESOURCE:
tracking = False
allocated = False
resource_tracker = self._find_resource_tracker(
unique_id.toHexString(resource.id))
if resource_tracker is None:
pass # should raise an exception
else:
resource_tracker.tracking = tracking
resource_tracker.allocated = allocated
resource_tracker.high_priority_flag = high_priority_flag
def __call__(self, msg, timeout=None, callback=None, error_callback=None):
'''
Initiates and executes the client request to the server. The type of arguments
supplied determines whether to apply blocking or non-blocking behaviour.
If no callback is supplied, the mode is blocking, otherwise non-blocking.
If no timeout is specified, then a return of None indicates that the
operation timed out.
:param msg: the request message
:type msg: <name>Request
:param rospy.Duration timeout: time to wait for data
:param callback: user callback invoked for responses of non-blocking calls
:type callback: method with arguments (uuid_msgs.UniqueID, <name>Response)
:returns: msg/id for blocking calls it is the response message, for non-blocking it is the unique id
:rtype: <name>Response/uuid_msgs.UniqueID or None (if timed out)
'''
pair_request_msg = self.ServicePairRequest()
pair_request_msg.id = unique_id.toMsg(unique_id.fromRandom())
pair_request_msg.request = msg
key = unique_id.toHexString(pair_request_msg.id)
if callback == None and error_callback == None:
self._request_handlers[key] = BlockingRequestHandler(key)
return self._make_blocking_call(self._request_handlers[key], pair_request_msg, timeout)
else:
request_handler = NonBlockingRequestHandler(key, callback, error_callback)
self._request_handlers[key] = request_handler.copy()
self._make_non_blocking_call(request_handler, pair_request_msg, timeout)
return pair_request_msg.id
def add(self, annotation, msg=None, gen_uuid=True):
'''
Add a new annotation with a new associated data or for an existing data.
:param annotation: The new annotation.
:param msg: Its associated data. If None, we assume that we are adding an annotation to existing data.
:param gen_uuid: Generate an unique id for the new annotation or use the received one.
:raises WCFError: If something went wrong.
'''
if gen_uuid:
annotation.id = unique_id.toMsg(unique_id.fromRandom())
else:
for a in self._annotations:
if a.id == annotation.id:
message = "Duplicated annotation with uuid '%s'" % unique_id.toHexString(
annotation.id)
rospy.logerr(message)
raise WCFError(message)
if msg is None:
# Msg not provided, so we assume that we are adding an annotation to existing data; find it by its id
msg_found = False
for d in self._annots_data:
if d.id == annotation.data_id:
rospy.logdebug("Annotation data with uuid '%s' found",
unique_id.toHexString(annotation.data_id))
msg_found = True
break
if not msg_found:
message = "Annotation data with uuid '%s' not found" % unique_id.toHexString(
annotation.data_id)
rospy.logerr(message)
raise WCFError(message)
else:
# Annotation comes with its data; create a common unique id to link both
annotation.data_id = unique_id.toMsg(unique_id.fromRandom())
annot_data = world_canvas_msgs.msg.AnnotationData()
annot_data.id = annotation.data_id
annot_data.type = annotation.type
annot_data.data = serialize_msg(msg)
self._annots_data.append(annot_data)
self._annotations.append(annotation)
self._saved = False
def get_metadata(self, uuid):
# Get metadata for the given annotation id
annot_id = unique_id.toHexString(uuid)
matching_anns = self.anns_collection.query({'id': {'$in': [annot_id]}}, True)
try:
return annot_id, matching_anns.next()
except StopIteration:
rospy.logwarn("Annotation %s not found" % annot_id)
return annot_id, None
def getMetadata(self, uuid):
# Get metadata for the given annotation id
annot_id = unique_id.toHexString(uuid)
matching_anns = self.anns_collection.query({'id': {'$in': [annot_id]}})
try:
return annot_id, matching_anns.next()[1]
except StopIteration:
rospy.logwarn("Annotation %s not found" % annot_id)
return annot_id, None
def test_msg_route_segment(self):
start = 'da7c242f-2efe-5175-9961-49cc621b80b9'
end = '812f1c08-a34b-5a21-92b9-18b2b0cf4950'
x = makeUniqueID('http://ros.org/wiki/road_network/'
+ start + '/' + end)
y = makeUniqueID('http://ros.org/wiki/road_network/'
+ end + '/' + start)
self.assertNotEqual(x, y)
self.assertEqual(unique_id.toHexString(x),
'acaa906e-8411-5b45-a446-ccdc2fc39f29')
def add(self, annotation, msg=None, gen_uuid=True):
'''
Add a new annotation with a new associated data or for an existing data.
@param annotation: The new annotation.
@param msg: Its associated data. If None, we assume that we are adding an annotation to existing data.
@param gen_uuid: Generate an unique id for the new annotation or use the received one.
@raise WCFError: If something went wrong.
'''
if gen_uuid:
annotation.id = unique_id.toMsg(unique_id.fromRandom())
else:
for a in self._annotations:
if a.id == annotation.id:
message = "Duplicated annotation with uuid '%s'" % unique_id.toHexString(annotation.id)
rospy.logerr(message)
raise WCFError(message)
if msg is None:
# Msg not provided, so we assume that we are adding an annotation to existing data; find it by its id
msg_found = False
for d in self._annots_data:
if d.id == annotation.data_id:
rospy.logdebug("Annotation data with uuid '%s' found", unique_id.toHexString(annotation.data_id))
msg_found = True
break
if not msg_found:
message = "Annotation data with uuid '%s' not found" % unique_id.toHexString(annotation.data_id)
rospy.logerr(message)
raise WCFError(message)
else:
# Annotation comes with its data; create a common unique id to link both
annotation.data_id = unique_id.toMsg(unique_id.fromRandom())
annot_data = world_canvas_msgs.msg.AnnotationData()
annot_data.id = annotation.data_id
annot_data.type = annotation.type
annot_data.data = serialize_msg(msg)
self._annots_data.append(annot_data)
self._annotations.append(annotation)
self._saved = False
def update(self, annotation, msg=None):
'''
Update an existing annotation and optionally its associated data.
:param annotation: The modified annotation.
:param msg: Its associated data. If None, just the annotation is updated.
:raises WCFError: If something went wrong.
'''
found = False
for i, a in enumerate(self._annotations):
if a.id == annotation.id:
self._annotations[i] = annotation
found = True
break
if not found:
message = "Annotation with uuid '%s' not found" % unique_id.toHexString(
annotation.id)
rospy.logerr(message)
raise WCFError(message)
if msg is None:
return
found = False
for i, d in enumerate(self._annots_data):
if d.id == annotation.data_id:
rospy.logdebug("Annotation data with uuid '%s' found",
unique_id.toHexString(annotation.data_id))
self._annots_data[i].type = annotation.type
self._annots_data[i].data = serialize_msg(msg)
found = True
break
if not found:
message = "Annotation data with uuid '%s' not found" % unique_id.toHexString(
annotation.data_id)
rospy.logerr(message)
raise WCFError(message)
self._saved = False
def update(self, annotation, msg=None):
'''
Update an existing annotation and optionally its associated data.
@param annotation: The modified annotation.
@param msg: Its associated data. If None, just the annotation is updated.
@raise WCFError: If something went wrong.
'''
found = False
for i, a in enumerate(self._annotations):
if a.id == annotation.id:
self._annotations[i] = annotation
found = True
break
if not found:
message = "Annotation with uuid '%s' not found" % unique_id.toHexString(annotation.id)
rospy.logerr(message)
raise WCFError(message)
if msg is None:
return
found = False
for i, d in enumerate(self._annots_data):
if d.id == annotation.data_id:
rospy.logdebug("Annotation data with uuid '%s' found", unique_id.toHexString(annotation.data_id))
self._annots_data[i].type = annotation.type
self._annots_data[i].data = serialize_msg(msg)
found = True
break
if not found:
message = "Annotation data with uuid '%s' not found" % unique_id.toHexString(annotation.data_id)
rospy.logerr(message)
raise WCFError(message)
self._saved = False
def threaded_callback(self, request_id, msg):
'''
@param request_id
@type uuid_msgs/UniqueID
@param msg
@type ServiceRequest
'''
# need warning so that the test program visualises it when run with --text
rospy.logwarn("QuickCallServer : threaded received [%s][%s]" %
(msg.data, unique_id.toHexString(request_id)))
response = rocon_service_pair_msgs.TestiesResponse()
response.data = "I heard ya dude"
rospy.sleep(2.0)
self.threaded_server.reply(request_id, response)
def __init__(self, minimum, resources):
'''
Constructor fully initialises this request.
@param minimum
@type int
@param resources : dict of resources eligible for use in constructing scheduler requests
@type { uuid hexstring : scheduler_msgs.Resource }
'''
self._resources = {}
for resource in resources:
resource.id = unique_id.toMsg(unique_id.fromRandom())
key = unique_id.toHexString(resource.id)
self._resources[key] = ResourceTracker(resource)
self._min = minimum
self._max = len(resources)
self._validate() # can raise an exception
def _setup_service_parameters(self, name, description, priority,
unique_identifier):
'''
Dump some important information for the services to self-introspect on in the namespace in which
they will be started.
:param str name: text name for the service (unique)
:param str description: text description of the service
:param int priority: a numeric priority level that can be configured at service level for establishing resource requests
:param uuid.UUID unique_identifier: unique id for the service
'''
namespace = concert_msgs.Strings.SERVICE_NAMESPACE + '/' + name
rospy.set_param(namespace + "/name", name)
rospy.set_param(namespace + "/description", description)
rospy.set_param(namespace + "/priority", priority)
rospy.set_param(
namespace + "/uuid",
unique_id.toHexString(unique_id.toMsg(unique_identifier)))
def _setup_service_parameters(self, name, description, unique_identifier):
'''
Dump some important information for the services to self-introspect on in the namespace in which
they will be started.
@param name : text name for the service (unique)
@type str
@param description : text description of the service
@type str
@param unique_identifier : unique id for the service
@type uuid.UUID
'''
namespace = concert_msgs.Strings.SERVICE_NAMESPACE + '/' + name
rospy.set_param(namespace + "/name", name)
rospy.set_param(namespace + "/description", description)
rospy.set_param(namespace + "/uuid", unique_id.toHexString(unique_id.toMsg(unique_identifier)))
def remove(self, uuid):
'''
Delete an annotation with its associated data.
WARN/TODO: we are ignoring the case of N annotations - 1 data!
:param uuid: The uuid of the annotation to delete.
:returns True if successfully removed, False if the annotation was not found.
:raises WCFError: If something else went wrong.
'''
for a in self._annotations:
if a.id == uuid:
rospy.logdebug("Annotation '%s' found",
unique_id.toHexString(uuid))
ann_to_delete = a
break
if 'ann_to_delete' not in locals():
rospy.logwarn("Annotation '%s' not found",
unique_id.toHexString(uuid))
return False
for d in self._annots_data:
if d.id == a.data_id:
rospy.logdebug("Annotation data '%s' found",
unique_id.toHexString(d.id))
data_to_delete = d
break
if 'data_to_delete' not in locals():
message = "No data found for annotation '%s' (data uuid is '%s')" \
% (unique_id.toHexString(uuid), unique_id.toHexString(ann_to_delete.data_id))
rospy.logerr(message)
raise WCFError(message)
rospy.logdebug("Removed annotation with uuid '%s'",
unique_id.toHexString(ann_to_delete.id))
rospy.logdebug("Removed annot. data with uuid '%s'",
unique_id.toHexString(data_to_delete.id))
self._annotations.remove(ann_to_delete)
self._annots_data.remove(data_to_delete)
self._annots_to_delete.append(ann_to_delete)
self._saved = False
return True
def setRelationship(self, request):
response = SetRelationshipResponse()
annot_id, metadata = self.getMetadata(request.id)
relat_id = unique_id.toHexString(request.relationship)
if metadata is None:
response.message = 'Annotation not found'
response.result = False
return response
if request.action == SetRelationshipRequest.ADD:
return self.addElement(annot_id, relat_id, metadata, 'relationships', response)
elif request.action == SetRelationshipRequest.DEL:
return self.delElement(annot_id, relat_id, metadata, 'relationships', response)
else:
# Sanity check against crazy service clients
rospy.logerr('Invalid action: %d' % request.action)
response.message = 'Invalid action: %d' % request.action
response.result = False
return response
def test_tiny_map_features(self):
gm = GeoMap(xml_map.get_osm('package://osm_cartography/tests/tiny.osm',
bounding_box.makeGlobal()))
self.assertEqual(gm.n_features, 2)
# expected feature IDs
uuids = ['8e8b355f-f1e8-5d82-827d-91e688e807e4',
'199dd143-8309-5401-9728-6ca5e1c6e235']
# test GeoMapFeatures iterator
gf = GeoMapFeatures(gm)
i = 0
for f in gf:
if type(f.id.uuid) == str(): # old-style message?
self.assertEqual(f.id.uuid, uuids[i])
else:
self.assertEqual(unique_id.toHexString(f.id), uuids[i])
i += 1
self.assertEqual(i, 2)
self.assertEqual(len(gf), 2)
def getAnnotationsData(self, request):
response = GetAnnotationsDataResponse()
query = {'id': {'$in': [unique_id.toHexString(id) for id in request.annotation_ids]}}
matching_data = self.data_collection.query(query)
i = 0
while True:
try:
response.data.append(matching_data.next()[0])
i += 1
except StopIteration:
if (i == 0):
rospy.loginfo("No annotations data found for query %s" % query) # we don't consider this an error
else:
rospy.loginfo("%d objects found for %d annotations" % (i, len(request.annotation_ids)))
break
response.result = True
return response
def set_relationship(self, request):
response = SetRelationshipResponse()
annot_id, metadata = self.get_metadata(request.id)
relat_id = unique_id.toHexString(request.relationship)
if metadata is None:
response.message = 'Annotation not found'
response.result = False
return response
if request.action == SetRelationshipRequest.ADD:
return self.add_element(annot_id, relat_id, metadata, 'relationships', response)
elif request.action == SetRelationshipRequest.DEL:
return self.del_element(annot_id, relat_id, metadata, 'relationships', response)
else:
# Sanity check against crazy service clients
rospy.logerr("Invalid action: %d" % request.action)
response.message = "Invalid action: %d" % request.action
response.result = False
return response
def remove(self, uuid):
'''
Delete an annotation with its associated data.
WARN/TODO: we are ignoring the case of N annotations - 1 data!
@param uuid: The uuid of the annotation to delete.
@returns True if successfully removed, False if the annotation was not found.
@raise WCFError: If something else went wrong.
'''
for a in self._annotations:
if a.id == uuid:
rospy.logdebug("Annotation '%s' found", unique_id.toHexString(uuid))
ann_to_delete = a
break
if 'ann_to_delete' not in locals():
rospy.logwarn("Annotation '%s' not found", unique_id.toHexString(uuid))
return False
for d in self._annots_data:
if d.id == a.data_id:
rospy.logdebug("Annotation data '%s' found", unique_id.toHexString(d.id))
data_to_delete = d
break
if 'data_to_delete' not in locals():
message = "No data found for annotation '%s' (data uuid is '%s')" \
% (unique_id.toHexString(uuid), unique_id.toHexString(ann_to_delete.data_id))
rospy.logerr(message)
raise WCFError(message)
rospy.logdebug("Removed annotation with uuid '%s'", unique_id.toHexString(ann_to_delete.id))
rospy.logdebug("Removed annot. data with uuid '%s'", unique_id.toHexString(data_to_delete.id))
self._annotations.remove(ann_to_delete)
self._annots_data.remove(data_to_delete)
self._annots_to_delete.append(ann_to_delete)
self._saved = False
return True
def test_tiny_map_features(self):
gm = GeoMap(
xml_map.get_osm('package://osm_cartography/tests/tiny.osm',
bounding_box.makeGlobal()))
self.assertEqual(gm.n_features, 2)
# expected feature IDs
uuids = [
'8e8b355f-f1e8-5d82-827d-91e688e807e4',
'199dd143-8309-5401-9728-6ca5e1c6e235'
]
# test GeoMapFeatures iterator
gf = GeoMapFeatures(gm)
i = 0
for f in gf:
if type(f.id.uuid) == str(): # old-style message?
self.assertEqual(f.id.uuid, uuids[i])
else:
self.assertEqual(unique_id.toHexString(f.id), uuids[i])
i += 1
self.assertEqual(i, 2)
self.assertEqual(len(gf), 2)
def saveAnnotationsData(self, request):
'''
Legacy method kept for debug purposes: saves together annotations and its data
assuming a 1-1 relationship.
:param request: Service request.
'''
response = SaveAnnotationsDataResponse()
print request.annotations
for annotation, data in zip(request.annotations, request.data):
# Compose metadata: mandatory fields
metadata = { 'world_id': unique_id.toHexString(annotation.world_id),
'data_id' : unique_id.toHexString(annotation.data_id),
'id' : unique_id.toHexString(annotation.id),
'name' : annotation.name,
'type' : annotation.type,
}
# Optional fields; note that both are stored as lists of strings
if len(annotation.keywords) > 0:
metadata['keywords'] = annotation.keywords
if len(annotation.relationships) > 0:
metadata['relationships'] = [unique_id.toHexString(r) for r in annotation.relationships]
# Data metadata: just the object id, as all querying is done over the annotations
data_metadata = { 'id' : unique_id.toHexString(annotation.data_id) }
rospy.logdebug("Saving annotation %s for map %s" % (annotation.id, annotation.world_id))
# Insert both annotation and associated data to the appropriate collection
self.anns_collection.remove({'id': {'$in': [unique_id.toHexString(annotation.id)]}})
self.anns_collection.insert(annotation, metadata)
self.data_collection.remove({'id': {'$in': [unique_id.toHexString(annotation.data_id)]}})
self.data_collection.insert(data, data_metadata)
rospy.loginfo("%lu annotations saved" % len(request.annotations))
response.result = True
return response
def get_annotations_data(self, request):
response = GetAnnotationsDataResponse()
if len(request.annotation_ids) == 0:
return self.service_error(response, "No annotation ids on request; you must be kidding!")
query = {'id': {'$in': [unique_id.toHexString(id) for id in request.annotation_ids]}}
matching_data = self.data_collection.query(query)
rospy.logdebug("Load annotations data with query %s" % query)
i = 0
while True:
try:
response.data.append(matching_data.next()[0])
i += 1
except StopIteration:
if (i == 0):
# we don't consider this an error
rospy.loginfo("No data found for %d requested annotations" % len(request.annotation_ids))
else:
rospy.loginfo("%d objects found for %d annotations" % (i, len(request.annotation_ids)))
break
return self.service_success(response)
def get_annotations_data(self, request):
response = GetAnnotationsDataResponse()
if len(request.annotation_ids) == 0:
return self.service_error(response, "No annotation ids on request; you must be kidding!")
query = {"id": {"$in": [unique_id.toHexString(id) for id in request.annotation_ids]}}
matching_data = self.data_collection.query(query)
rospy.logdebug("Load annotations data with query %s" % query)
i = 0
while True:
try:
response.data.append(matching_data.next()[0])
i += 1
except StopIteration:
if i == 0:
# we don't consider this an error
rospy.loginfo("No data found for %d requested annotations" % len(request.annotation_ids))
else:
rospy.loginfo("%d objects found for %d annotations" % (i, len(request.annotation_ids)))
break
return self.service_success(response)
def save_annotations_data(self, request):
"""
Legacy method kept for debug purposes: saves together annotations and its data
assuming a 1-1 relationship.
@param request: Service request.
"""
response = SaveAnnotationsDataResponse()
print request.annotations
for annotation, data in zip(request.annotations, request.data):
# Compose metadata: mandatory fields
metadata = {
"world": annotation.world,
"data_id": unique_id.toHexString(annotation.data_id),
"id": unique_id.toHexString(annotation.id),
"name": annotation.name,
"type": annotation.type,
}
# Optional fields; note that both are stored as lists of strings
if len(annotation.keywords) > 0:
metadata["keywords"] = annotation.keywords
if len(annotation.relationships) > 0:
metadata["relationships"] = [unique_id.toHexString(r) for r in annotation.relationships]
# Data metadata: just the object id, as all querying is done over the annotations
data_metadata = {"id": unique_id.toHexString(annotation.data_id)}
rospy.logdebug("Saving annotation %s for world %s" % (annotation.id, annotation.world))
# Insert both annotation and associated data to the appropriate collection
self.anns_collection.remove({"id": {"$in": [unique_id.toHexString(annotation.id)]}})
self.anns_collection.insert(annotation, metadata)
self.data_collection.remove({"id": {"$in": [unique_id.toHexString(annotation.data_id)]}})
self.data_collection.insert(data, data_metadata)
rospy.loginfo("%lu annotations saved" % len(request.annotations))
return self.service_success(response)
def save_annotations_data(self, request):
'''
Legacy method kept for debug purposes: saves together annotations and its data
assuming a 1-1 relationship.
@param request: Service request.
'''
response = SaveAnnotationsDataResponse()
print request.annotations
for annotation, data in zip(request.annotations, request.data):
# Compose metadata: mandatory fields
metadata = { 'world' : annotation.world,
'data_id' : unique_id.toHexString(annotation.data_id),
'id' : unique_id.toHexString(annotation.id),
'name' : annotation.name,
'type' : annotation.type
}
# Optional fields; note that both are stored as lists of strings
if len(annotation.keywords) > 0:
metadata['keywords'] = annotation.keywords
if len(annotation.relationships) > 0:
metadata['relationships'] = [unique_id.toHexString(r) for r in annotation.relationships]
# Data metadata: just the object id, as all querying is done over the annotations
data_metadata = { 'id' : unique_id.toHexString(annotation.data_id) }
rospy.logdebug("Saving annotation %s for world %s" % (annotation.id, annotation.world))
# Insert both annotation and associated data to the appropriate collection
self.anns_collection.remove({'id': {'$in': [unique_id.toHexString(annotation.id)]}})
self.anns_collection.insert(annotation, metadata)
self.data_collection.remove({'id': {'$in': [unique_id.toHexString(annotation.data_id)]}})
self.data_collection.insert(data, data_metadata)
rospy.loginfo("%lu annotations saved" % len(request.annotations))
return self.service_success(response)
def pub_annotations_data(self, request):
response = PubAnnotationsDataResponse()
if len(request.annotation_ids) == 0:
return self.service_error(response, "No annotation ids on request; you must be kidding!")
# Verify that all annotations on list belong to the same type (as we will publish them in
# the same topic) and that at least one is really present in database
query = {'data_id': {'$in': [unique_id.toHexString(id) for id in request.annotation_ids]}}
matching_anns = self.anns_collection.query(query, metadata_only=True)
while True:
try:
# Get annotation metadata; we just need the annotation type
ann_md = matching_anns.next()
if 'topic_type' not in locals():
topic_type = ann_md['type']
elif topic_type != ann_md['type']:
return self.service_error(response, "Cannot publish annotations of different types (%s, %s)"
% (topic_type, ann_md['type']))
except StopIteration:
break
if 'topic_type' not in locals():
return self.service_error(response, "None of the %d requested annotations was found in database"
% len(request.annotation_ids))
# Keep the class of the messages to be published; we need it later when deserializing them
msg_class = roslib.message.get_message_class(topic_type)
if msg_class is None:
# This happens if the topic type is wrong or not known for the server (i.e. the package describing it is
# absent from ROS_PACKAGE_PATH). The second condition is a tricky one, as is a big known limitation of WCF
# (https://github.com/corot/world_canvas/issues/5)
return self.service_error(response, "Topic type %s definition not found" % topic_type)
# Advertise a topic with message type request.topic_type if we will publish results as a list (note that we
# ignore list's type) or use the retrieved annotations type otherwise (we have verified that it's unique)
if request.pub_as_list:
topic_type = request.topic_type
topic_class = roslib.message.get_message_class(topic_type)
if topic_class is None:
# Same comment as in previous service_error call applies here
return self.service_error(response, "Topic type %s definition not found" % topic_type)
else:
topic_class = msg_class
pub = rospy.Publisher(request.topic_name, topic_class, latch=True, queue_size=5)
# Now retrieve data associated to the requested annotations; reuse query to skip toHexString calls
query['id'] = query.pop('data_id')
matching_data = self.data_collection.query(query)
rospy.logdebug("Publish data for annotations on query %s" % query)
i = 0
object_list = list()
while True:
try:
# Get annotation data and deserialize data field to get the original message of type request.topic_type
ann_data = matching_data.next()[0]
ann_msg = deserialize_msg(ann_data.data, msg_class)
if request.pub_as_list:
object_list.append(ann_msg)
else:
pub.publish(ann_msg)
i += 1
except SerializationError as e:
rospy.logerr("Deserialization failed: %s" % str(e))
continue
except StopIteration:
if (i == 0):
# This must be an error cause we verified before that at least one annotation is present!
return self.service_error(response, "No data found for %d requested annotations"
% len(request.annotation_ids))
if i != len(request.annotation_ids):
# Don't need to be an error, as multiple annotations can reference the same data
rospy.logwarn("Only %d objects found for %d annotations" % (i, len(request.annotation_ids)))
else:
rospy.loginfo("%d objects found for %d annotations" % (i, len(request.annotation_ids)))
if request.pub_as_list:
pub.publish(object_list)
break
return self.service_success(response)
def key(self):
return unique_id.toHexString(self.resource.id)
def pubAnnotationsData(self, request):
response = PubAnnotationsDataResponse()
if len(request.annotation_ids) == 0:
return self.serviceError(response, "No annotation ids on request; you must be kidding!")
# Verify that all annotations on list belong to the same type (as we will publish them in
# the same topic) and that at least one is really present in database
query = {'data_id': {'$in': [unique_id.toHexString(id) for id in request.annotation_ids]}}
matching_anns = self.anns_collection.query(query, metadata_only=True)
while True:
try:
# Get annotation metadata; we just need the annotation type
ann_md = matching_anns.next()
if 'topic_type' not in locals():
topic_type = ann_md['type']
elif topic_type != ann_md['type']:
return self.serviceError(response, "Cannot publish annotations of different types (%s, %s)"
% (topic_type, ann_md['type']))
except StopIteration:
break
if 'topic_type' not in locals():
return self.serviceError(response, "None of the %d requested annotations was found in database"
% len(request.annotation_ids))
# Advertise a topic with message type request.topic_type if we will publish results as a list
if request.pub_as_list:
topic_class = roslib.message.get_message_class(request.topic_type)
else:
# Use the retrieved annotations type otherwise (we have verified that it's unique)
topic_class = roslib.message.get_message_class(topic_type)
pub = rospy.Publisher(request.topic_name, topic_class, latch=True, queue_size=5)
# Now retrieve data associated to the requested annotations; reuse query to skip toHexString calls
query['id'] = query.pop('data_id')
matching_data = self.data_collection.query(query)
i = 0
object_list = list()
while True:
try:
# Get annotation data and deserialize data field to get the original message of type request.topic_type
ann_data = matching_data.next()[0]
object = pickle.loads(ann_data.data)
if request.pub_as_list:
object_list.append(object)
else:
pub.publish(object)
i += 1
except StopIteration:
if (i == 0):
# This must be an error cause we verified before that at least one annotation is present!
return self.serviceError(response, "No annotations data found for query %s" % query)
if i != len(request.annotation_ids):
# Don't need to be an error, as multiple annotations can reference the same data
rospy.logwarn("Only %d objects found for %d annotations" % (i, len(request.annotation_ids)))
else:
rospy.loginfo("%d objects found for %d annotations" % (i, len(request.annotation_ids)))
if request.pub_as_list:
pub.publish(object_list)
break
response.result = True
return response
def import_from_yaml(self, request):
response = YAMLImportResponse()
if not os.path.isfile(request.filename):
return self.service_error(response, "File does not exist: %s" % (request.filename))
try:
with open(request.filename, 'r') as f:
# load all documents
yaml_data = yaml.load(f)
if yaml_data is None:
raise yaml.YAMLError("Empty files not allowed")
except yaml.YAMLError as e:
return self.service_error(response, "Invalid YAML file: %s" % (str(e)))
# Clear existing database content TODO: flag to choose whether keep content
self.anns_collection.remove({})
self.data_collection.remove({})
# Parse file: a database is composed of a list of elements, each one containing a list of annotations plus
# their referenced data. TODO: Can change according to issue https://github.com/corot/world_canvas/issues/9
for t in yaml_data:
# Annotations: a list of one or more annotations
try:
anns_list = t['annotations']
except KeyError as e:
return self.service_error(response, "Invalid database file format: %s field not found" % str(e))
if len(anns_list) == 0:
# Coherence check: there must be at least one annotation for database entry
return self.service_error(response, "Invalid database file format: " \
"there must be at least one annotation for database entry")
for a in anns_list:
annotation = Annotation()
try:
genpy.message.fill_message_args(annotation, a)
if 'prev_data_id' in locals() and prev_data_id != annotation.data_id.uuid:
# Coherence check: all data_id fields must be equal between them and to data id
return self.service_error(response, "Invalid database file format: " \
"data ids must be equal for annotations referencing the same data")
prev_data_id = annotation.data_id.uuid
# Forced conversion because UUID expects a string of 16 bytes, not a list
annotation.data_id.uuid = ''.join(chr(x) for x in annotation.data_id.uuid)
annotation.id.uuid = ''.join(chr(x) for x in annotation.id.uuid)
for r in annotation.relationships:
r.uuid = ''.join(chr(x) for x in r.uuid)
# Compose metadata: mandatory fields
metadata = { 'data_id' : unique_id.toHexString(annotation.data_id),
'id' : unique_id.toHexString(annotation.id),
'world' : annotation.world,
'name' : annotation.name,
'type' : annotation.type
}
# Optional fields; note that both are stored as lists of strings
if len(annotation.keywords) > 0:
metadata['keywords'] = annotation.keywords
if len(annotation.relationships) > 0:
metadata['relationships'] = [unique_id.toHexString(r) for r in annotation.relationships]
rospy.logdebug("Saving annotation %s for world %s" % (annotation.id, annotation.world))
# TODO recuperate if implement TODO on line 83 self.anns_collection.remove({'id': {'$in': [unique_id.toHexString(annotation.id)]}})
self.anns_collection.insert(annotation, metadata, safe=True)
except (genpy.MessageException, genpy.message.SerializationError) as e:
return self.service_error(response, "Invalid annotation msg format: %s" % str(e))
except Exception as e:
# Assume collection.insert raised this, as we set safe=True (typically a DuplicateKeyError)
return self.service_error(response, "Insert annotation failed: %s" % str(e))
# Annotation data, of message type annotation.type
msg_class = roslib.message.get_message_class(annotation.type)
if msg_class is None:
# annotation.type doesn't contain a known message type; we cannot insert on database
return self.service_error(response, "Unknown message type: %s" % annotation.type)
data = msg_class()
# Data metadata: just the object id, as all querying is done over the annotations
data_metadata = { 'id' : unique_id.toHexString(annotation.data_id) }
try:
genpy.message.fill_message_args(data, t['data'])
data_msg = AnnotationData()
data_msg.id = annotation.data_id
data_msg.type = annotation.type
data_msg.data = serialize_msg(data)
self.data_collection.insert(data_msg, data_metadata, safe=True)
except (genpy.MessageException, genpy.message.SerializationError) as e:
# TODO: here I would have an incoherence in db: annotations without data;
# do mongo has rollback? do it manually? or just clear database content?
return self.service_error(response, "Invalid %s msg format: %s" % (annotation.type, str(e)))
except KeyError as e:
return self.service_error(response, "Invalid database file format: %s field not found" % str(e))
except Exception as e:
# Assume collection.insert raised this, as we set safe=True (typically a DuplicateKeyError)
return self.service_error(response, "Insert annotation data failed: %s" % str(e))
# self.data_collection.remove({'id': {'$in': [unique_id.toHexString(annotation.id)]}})
del prev_data_id # clear this so it doen't interfere with next element validation
return self.service_success(response, "%lu annotations imported on database" % len(yaml_data))
def _update(self):
"""
Logic for allocating resources after there has been a state change in either the list of
available concert clients or the incoming resource requests.
Note : this must be protected by being called inside locked code
@todo test use of rlocks here
"""
there_be_requests = False
for request_set in self._request_sets.values():
if request_set.keys():
there_be_requests = True
if not there_be_requests:
return # Nothing to do
########################################
# Sort the request sets
########################################
unallocated_clients = [
client for client in self._clients.values() if not client.allocated
]
new_replies = []
pending_replies = []
releasing_replies = []
for request_set in self._request_sets.values():
new_replies.extend([
r for r in request_set.values()
if r.msg.status == scheduler_msgs.Request.NEW
])
pending_replies.extend([
r for r in request_set.values()
if r.msg.status == scheduler_msgs.Request.NEW
or r.msg.status == scheduler_msgs.Request.WAITING
])
releasing_replies.extend([
r for r in request_set.values()
if r.msg.status == scheduler_msgs.Request.CANCELING
])
# get all requests for compatibility tree processing and sort by priority
# this is a bit inefficient, should just sort the request set directly? modifying it directly may be not right though
pending_replies[:] = sorted(pending_replies,
key=lambda request: request.msg.priority)
########################################
# No allocations
########################################
if not unallocated_clients and new_replies:
for reply in new_replies:
# should do something here (maybe validation)?
reply.wait()
########################################
# Allocations
########################################
resource_pool_state_changed = False
if unallocated_clients:
last_failed_priority = None # used to check if we should block further allocations to lower priorities
for reply in pending_replies:
request = reply.msg
if last_failed_priority is not None and request.priority < last_failed_priority:
rospy.loginfo(
"Scheduler : ignoring lower priority requests until higher priorities are filled"
)
break
request_id = unique_id.toHexString(request.id)
compatibility_tree = create_compatibility_tree(
request.resources, unallocated_clients)
if self._debug_show_compatibility_tree:
compatibility_tree.print_branches("Compatibility Tree")
pruned_branches = prune_compatibility_tree(
compatibility_tree,
verbosity=self._debug_show_compatibility_tree)
pruned_compatibility_tree = CompatibilityTree(pruned_branches)
if self._debug_show_compatibility_tree:
pruned_compatibility_tree.print_branches(
"Pruned Tree", ' ')
if pruned_compatibility_tree.is_valid():
rospy.loginfo(
"Scheduler : compatibility tree is valid, attempting to allocate [%s]"
% request_id)
last_failed_priority = None
resources = []
failed_to_allocate = False
for branch in pruned_compatibility_tree.branches:
if failed_to_allocate: # nested break catch
break
for leaf in branch.leaves: # there should be but one
# this info is actually embedding into self._clients
try:
leaf.allocate(request_id, branch.limb)
rospy.loginfo("Scheduler : allocated [%s]" %
leaf.name)
except FailedToAllocateException as e:
rospy.logwarn(
"Scheduler : failed to allocate [%s][%s]"
% (leaf.name, str(e)))
failed_to_allocate = True
break
resource = copy.deepcopy(branch.limb)
uri = rocon_uri.parse(
leaf.msg.platform_info.uri
) # leaf.msg is concert_msgs/ConcertClient
uri.name = leaf.msg.gateway_name # store the unique name of the concert client
resource.uri = str(uri)
resources.append(resource)
if failed_to_allocate:
rospy.logwarn(
"Scheduler : aborting request allocation [%s]" %
request_id)
# aborting request allocation
for branch in pruned_compatibility_tree.branches:
for leaf in branch.leaves: # there should be but one
if leaf.allocated:
leaf.abandon()
last_failed_priority = request.priority
if reply.msg.status == scheduler_msgs.Request.NEW:
reply.wait()
else:
reply.grant(resources)
resource_pool_state_changed = True
else:
if reply.msg.status == scheduler_msgs.Request.NEW:
reply.wait()
last_failed_priority = request.priority
rospy.loginfo(
"Scheduler : insufficient resources to satisfy request [%s]"
% request_id)
########################################
# Releasing Allocated Concert Clients
########################################
for reply in releasing_replies:
#print("Releasing Resources: %s" % [r.rapp for r in reply.msg.resources])
#print(" Releasing Resources: %s" % [r.uri for r in reply.msg.resources])
#print(" Clients: %s" % self._clients.keys())
#for client in self._clients.values():
# print(str(client))
for resource in reply.msg.resources:
try:
self._clients[rocon_uri.parse(
resource.uri).name.string].abandon()
except KeyError:
pass # nothing was allocated to that resource yet (i.e. unique gateway_name was not yet set)
reply.close()
#reply.msg.status = scheduler_msgs.Request.RELEASED
# Publish an update?
if resource_pool_state_changed or releasing_replies:
self._publish_resource_pool()
def test_msg_creation(self):
msg = makeUniqueID('http://openstreetmap.org/node/', 152370223)
self.assertEqual(unique_id.toHexString(msg),
'8e0b7d8a-c433-5c42-be2e-fbd97ddff9ac')
def test_msg_same_id_different_namespace(self):
x = makeUniqueID('http://openstreetmap.org/node/', 1)
y = makeUniqueID('http://openstreetmap.org/way/', 1)
self.assertNotEqual(x, y)
self.assertEqual(unique_id.toHexString(y),
'b3180681-b125-5e41-bd04-3c8b046175b4')
def pub_annotations_data(self, request):
response = PubAnnotationsDataResponse()
if len(request.annotation_ids) == 0:
return self.service_error(response, "No annotation ids on request; you must be kidding!")
# Verify that all annotations on list belong to the same type (as we will publish them in
# the same topic) and that at least one is really present in database
query = {"data_id": {"$in": [unique_id.toHexString(id) for id in request.annotation_ids]}}
matching_anns = self.anns_collection.query(query, metadata_only=True)
while True:
try:
# Get annotation metadata; we just need the annotation type
ann_md = matching_anns.next()
if "topic_type" not in locals():
topic_type = ann_md["type"]
elif topic_type != ann_md["type"]:
return self.service_error(
response,
"Cannot publish annotations of different types (%s, %s)" % (topic_type, ann_md["type"]),
)
except StopIteration:
break
if "topic_type" not in locals():
return self.service_error(
response, "None of the %d requested annotations was found in database" % len(request.annotation_ids)
)
# Keep the class of the messages to be published; we need it later when deserializing them
msg_class = roslib.message.get_message_class(topic_type)
if msg_class is None:
# This happens if the topic type is wrong or not known for the server (i.e. the package describing it is
# absent from ROS_PACKAGE_PATH). The second condition is a tricky one, as is a big known limitation of WCF
# (https://github.com/corot/world_canvas/issues/5)
return self.service_error(response, "Topic type %s definition not found" % topic_type)
# Advertise a topic with message type request.topic_type if we will publish results as a list (note that we
# ignore list's type) or use the retrieved annotations type otherwise (we have verified that it's unique)
if request.pub_as_list:
topic_type = request.topic_type
topic_class = roslib.message.get_message_class(topic_type)
if topic_class is None:
# Same comment as in previous service_error call applies here
return self.service_error(response, "Topic type %s definition not found" % topic_type)
else:
topic_class = msg_class
pub = rospy.Publisher(request.topic_name, topic_class, latch=True, queue_size=5)
# Now retrieve data associated to the requested annotations; reuse query to skip toHexString calls
query["id"] = query.pop("data_id")
matching_data = self.data_collection.query(query)
rospy.logdebug("Publish data for annotations on query %s" % query)
i = 0
object_list = list()
while True:
try:
# Get annotation data and deserialize data field to get the original message of type request.topic_type
ann_data = matching_data.next()[0]
ann_msg = deserialize_msg(ann_data.data, msg_class)
if request.pub_as_list:
object_list.append(ann_msg)
else:
pub.publish(ann_msg)
i += 1
except SerializationError as e:
rospy.logerr("Deserialization failed: %s" % str(e))
continue
except StopIteration:
if i == 0:
# This must be an error cause we verified before that at least one annotation is present!
return self.service_error(
response, "No data found for %d requested annotations" % len(request.annotation_ids)
)
if i != len(request.annotation_ids):
# Don't need to be an error, as multiple annotations can reference the same data
rospy.logwarn("Only %d objects found for %d annotations" % (i, len(request.annotation_ids)))
else:
rospy.loginfo("%d objects found for %d annotations" % (i, len(request.annotation_ids)))
if request.pub_as_list:
pub.publish(object_list)
break
return self.service_success(response)
def _update(self, external_update=False):
"""
Logic for allocating resources after there has been a state change in either the list of
available concert clients or the incoming resource requests.
:param external_update bool: whether or not this is called inside the scheduler thread or not.
:returns: list of requester id's that have notifications pending.
We have to be careful with sending notifications if calling this from outside the scheduler thread.
"""
there_be_requests = False
for request_set in self._request_sets.values():
if request_set.keys():
there_be_requests = True
if not there_be_requests:
return [] # nothing to do
pending_notifications = []
########################################
# Sort the request sets
########################################
unallocated_clients = [
client for client in self._clients.values() if not client.allocated
]
new_replies = []
pending_replies = []
releasing_replies = []
for request_set in self._request_sets.values():
new_replies.extend([
r for r in request_set.values()
if r.msg.status == scheduler_msgs.Request.NEW
])
pending_replies.extend([
r for r in request_set.values()
if r.msg.status == scheduler_msgs.Request.WAITING
])
#pending_replies.extend([r for r in request_set.values() if r.msg.status == scheduler_msgs.Request.NEW or r.msg.status == scheduler_msgs.Request.WAITING])
releasing_replies.extend([
r for r in request_set.values()
if r.msg.status == scheduler_msgs.Request.CANCELING
])
# get all requests for compatibility tree processing and sort by priority
# this is a bit inefficient, should just sort the request set directly? modifying it directly may be not right though
pending_replies[:] = sorted(pending_replies,
key=lambda request: request.msg.priority)
########################################
# New
########################################
for reply in new_replies:
reply.wait()
########################################
# Pending
########################################
resource_pool_state_changed = False
# used to check if we should block further allocations to lower priorities
# important to use this variable because we might have a group of requests with equal priorities
# and we don't want to block the whole group because the first one failed.
last_failed_priority = None
reallocated_clients = {
} # dic of uri string keys and request uuid hex strings
for reply in pending_replies:
request = reply.msg
request_id = unique_id.toHexString(request.id)
if last_failed_priority is not None and request.priority < last_failed_priority:
rospy.loginfo(
"Scheduler : ignoring lower priority requests until higher priorities are filled"
)
break
# add preemptible clients to the candidates
if self._parameters['enable_preemptions']:
allocatable_clients = unallocated_clients + [
client for client in self._clients.values()
if (client.allocated
and client.allocated_priority < request.priority)
]
else:
allocatable_clients = unallocated_clients
if not allocatable_clients:
# this gets spammy...
#rospy.loginfo("Scheduler : no resources available to satisfy request [%s]" % request_id)
last_failed_priority = request.priority
continue
compatibility_tree = create_compatibility_tree(
request.resources, allocatable_clients)
if self._parameters['debug_show_compatibility_tree']:
compatibility_tree.print_branches("Compatibility Tree")
pruned_branches = prune_compatibility_tree(
compatibility_tree,
verbosity=self._parameters['debug_show_compatibility_tree'])
pruned_compatibility_tree = CompatibilityTree(pruned_branches)
if self._parameters['debug_show_compatibility_tree']:
pruned_compatibility_tree.print_branches("Pruned Tree", ' ')
if pruned_compatibility_tree.is_valid():
rospy.loginfo(
"Scheduler : compatibility tree is valid, attempting to allocate [%s]"
% request_id)
last_failed_priority = None
resources = []
failed_to_allocate = False
for branch in pruned_compatibility_tree.branches:
if failed_to_allocate: # nested break catch
break
for leaf in branch.leaves: # there should be but one
# this info is actually embedding into self._clients
try:
if leaf.allocated:
old_request_id = leaf.reallocate(
request_id, request.priority, branch.limb)
reallocated_clients[leaf.msg.platform_info.
uri] = old_request_id
rospy.loginfo(
"Scheduler : pre-empted and reallocated [%s]"
% leaf.name)
else:
leaf.allocate(request_id, request.priority,
branch.limb)
rospy.loginfo("Scheduler : allocated [%s]" %
leaf.name)
except FailedToAllocateException as e:
rospy.logwarn(
"Scheduler : failed to (re)allocate [%s][%s]"
% (leaf.name, str(e)))
failed_to_allocate = True
break
resource = copy.deepcopy(branch.limb)
uri = rocon_uri.parse(
leaf.msg.platform_info.uri
) # leaf.msg is concert_msgs/ConcertClient
uri.name = leaf.msg.gateway_name.lower().replace(
' ',
'_') # store the unique name of the concert client
resource.uri = str(uri)
resources.append(resource)
if failed_to_allocate:
rospy.logwarn(
"Scheduler : aborting request allocation [%s]" %
request_id)
# aborting request allocation
for branch in pruned_compatibility_tree.branches:
for leaf in branch.leaves: # there should be but one
if leaf.allocated:
leaf.abandon()
last_failed_priority = request.priority
else:
reply.grant(resources)
resource_pool_state_changed = True
# remove allocated clients from the unallocated list so they don't get doubly allocated on the next request in line
newly_allocated_client_names = []
for branch in pruned_compatibility_tree.branches:
newly_allocated_client_names.extend([
leaf.name for leaf in branch.leaves
if leaf.allocated
])
unallocated_clients[:] = [
client for client in unallocated_clients
if client.name not in newly_allocated_client_names
]
else:
last_failed_priority = request.priority
rospy.loginfo(
"Scheduler : insufficient resources to satisfy request [%s]"
% request_id)
########################################
# Preempted resource handling
########################################
# this is basically equivalent to what is in the concert client changes
# subscriber callback...can we move this somewhere centralised?
for (resource_uri_string,
request_id_hexstring) in reallocated_clients.iteritems():
request_id = uuid.UUID(request_id_hexstring)
# awkward that I don't have the requester id saved anywhere so
# we have to parse through each requester's set of requests to find
# the request id we want
found = False
for request_set in self._request_sets.values():
for request in request_set.values():
if request.uuid == request_id:
for resource in request.msg.resources:
# could use a better way to check for equality than this
if resource.uri == resource_uri_string:
updated_resource_uri = rocon_uri.parse(
resource_uri_string)
updated_resource_uri.name = concert_msgs.Strings.SCHEDULER_UNALLOCATED_RESOURCE
resource.uri = str(updated_resource_uri)
found = True
break
if found:
break
if found:
if external_update:
pending_notifications.append(request_set.requester_id)
else:
self._scheduler.notify(request_set.requester_id)
# @todo might want to consider changing the request status if all resources have been unallocated
break
########################################
# Releasing
########################################
for reply in releasing_replies:
if reply.msg.reason == scheduler_msgs.Request.NONE:
rospy.loginfo(
"Scheduler : releasing resources from cancelled request [%s][none]"
% ([resource.rapp for resource in reply.msg.resources]))
elif reply.msg.reason == scheduler_msgs.Request.TIMEOUT:
rospy.loginfo(
"Scheduler : releasing resources from cancelled request [%s][scheduler-requester watchdog timeout]"
% ([resource.rapp for resource in reply.msg.resources]))
else:
rospy.loginfo(
"Scheduler : releasing resources from cancelled request [%s][%s]"
%
([resource.rapp
for resource in reply.msg.resources], reply.msg.reason))
for resource in reply.msg.resources:
try:
resource_name = rocon_uri.parse(resource.uri).name.string
for key, value in self._clients.items():
if resource_name == rocon_python_utils.ros.get_ros_friendly_name(
key):
value.abandon()
except KeyError as e:
rospy.logwarn(
"Scheduler : Error while releasing resource[%s]" %
str(e))
pass # nothing was allocated to that resource yet (i.e. unique gateway_name was not yet set)
reply.close()
#reply.msg.status = scheduler_msgs.Request.RELEASED
# Publish an update?
if resource_pool_state_changed or releasing_replies:
self._publish_resource_pool()
return pending_notifications
def export_to_yaml(self, request):
response = YAMLExportResponse()
# Query for all annotations in database, shorted by data id, so we can export packed with its referenced data
matching_anns = self.anns_collection.query({}, sort_by='data_id')
try:
with open(request.filename, 'w') as f:
entries = []
annotations = []
last = False
# Loop over annotations retrieved from database
while True:
try:
annotation, metadata = matching_anns.next()
except StopIteration:
last = True
if not last and (len(annotations) == 0 or annotation.data_id == annotations[-1].data_id):
# We pack annotations with the same data_id
annotations.append(annotation)
elif len(annotations) > 0:
# When the next annotation have a different data_id, or is the
# last one, it's time to append a new entry to the output file:
# a) retrieve data for current data_id
data_id_str = unique_id.toHexString(annotations[-1].data_id)
matching_data = self.data_collection.query({'id': {'$in': [data_id_str]}})
try:
d = matching_data.next()[0]
except StopIteration:
return self.service_error(response, "Export to file failed: " \
"No data found with id %s" % data_id_str)
# b) pack together with their referencing annotations in a dictionary
# We need the annotation data class to deserialize the bytes array stored in database
data_class = roslib.message.get_message_class(annotations[0].type)
if data_class is None:
rospy.logerr("Annotation type %s definition not found" % annotation.type)
return False
data = deserialize_msg(d.data, data_class)
entry = dict(
annotations = [yaml.load(genpy.message.strify_message(a)) for a in annotations],
data = yaml.load(genpy.message.strify_message(data))
)
entries.append(entry)
if last:
break;
else:
# No the last annotation; note that it's still not stored,
# in the entries list so add to list for the next iteration
annotations = [annotation]
else:
# just to verify the not-very-clear logic of the loop...
rospy.logdebug("Final else: %d %d" % (last, len(entries)))
break;
if len(entries) == 0:
# we don't consider this an error
return self.service_success(response, "Database is empty!; nothing to export")
else:
# I must use default_flow_style = False so the resulting YAML looks nice, but then
# the bloody dumper writes lists with an element per-line, (with -), what makes the
# output very long and not very readable. So method flow_style_lists makes uuids and
# covariances list flow-styled, i.e. [x, y, z, ...], while flow_style_occ_grid to the
# same for occupancy grids (maps), but can be easily modified for other big messages
dump = yaml.dump(entries, default_flow_style=False)
dump = self.flow_style_lists(dump)
dump = self.flow_style_occ_grid(dump)
# Add a decimal point to exponential formated floats; if not, get loaded as strings,
# due to a bug in pyyaml. See this ticket for details: http://pyyaml.org/ticket/359
dump = self.fix_exp_numbers(dump)
f.write(dump)
return self.service_success(response, "%lu annotations exported from database" % len(entries))
except Exception as e:
return self.service_error(response, "Export to file failed: %s" % (str(e)))