Ejemplo n.º 1
0
    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)
Ejemplo n.º 2
0
    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
Ejemplo n.º 4
0
 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
Ejemplo n.º 6
0
    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)
Ejemplo n.º 7
0
    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
Ejemplo n.º 8
0
    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)
Ejemplo n.º 10
0
    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
Ejemplo n.º 11
0
 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)
Ejemplo n.º 12
0
    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
Ejemplo n.º 13
0
 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
Ejemplo n.º 16
0
    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
Ejemplo n.º 17
0
    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
Ejemplo n.º 18
0
 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
Ejemplo n.º 22
0
 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)
Ejemplo n.º 23
0
    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
Ejemplo n.º 24
0
    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)))
Ejemplo n.º 25
0
    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
Ejemplo n.º 27
0
    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
Ejemplo n.º 28
0
    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)
Ejemplo n.º 29
0
    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
Ejemplo n.º 30
0
    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
Ejemplo n.º 32
0
    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)
Ejemplo n.º 33
0
    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
Ejemplo n.º 34
0
    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)
Ejemplo n.º 35
0
    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)
Ejemplo n.º 36
0
    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)
Ejemplo n.º 37
0
    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)
Ejemplo n.º 38
0
    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)
Ejemplo n.º 39
0
 def key(self):
     return unique_id.toHexString(self.resource.id)
Ejemplo n.º 40
0
    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
Ejemplo n.º 41
0
    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))
Ejemplo n.º 42
0
    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()
Ejemplo n.º 43
0
 def test_msg_creation(self):
     msg = makeUniqueID('http://openstreetmap.org/node/', 152370223)
     self.assertEqual(unique_id.toHexString(msg),
                      '8e0b7d8a-c433-5c42-be2e-fbd97ddff9ac')
Ejemplo n.º 44
0
 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')
Ejemplo n.º 45
0
    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)
Ejemplo n.º 46
0
    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
Ejemplo n.º 47
0
    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)))