00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 import genpy
00037 import rospy
00038 import roslib
00039 import copy
00040 import uuid
00041 import unique_id
00042 import world_canvas_msgs.msg
00043 import world_canvas_msgs.srv
00044
00045 from rospy_message_converter import message_converter
00046 from visualization_msgs.msg import Marker, MarkerArray
00047 from world_canvas_utils.serialization import *
00048 from .exceptions import WCFError
00049
00050
00051 class AnnotationCollection:
00052 '''
00053 A collection of annotations and its associated data, initially empty. Annotations
00054 and data are retrieved from the world canvas server, filtered by a set of selection
00055 criteria (see constructor parameters).
00056 The collection can be reloaded with different filter criteria. There're also methods
00057 to add and delete individual annotations (an "update" method is TODO). But changes are
00058 only saved to database by calling "save" method.
00059 This class can also publish the retrieved annotations and RViz visualization markers,
00060 mostly for debug purposes.
00061 '''
00062
00063 def __init__(self, world=None, uuids=[], names=[], types=[], keywords=[], relationships=[], srv_namespace=''):
00064 '''
00065 Creates a collection of annotations and its associated data, initially empty.
00066 Annotations and data are retrieved from the world canvas server, filtered by
00067 the described parameters.
00068
00069 @param world: Annotations in this collection belong to this world.
00070 @param uuids: Filter annotations by their uuid.
00071 @param names: Filter annotations by their name.
00072 @param types: Filter annotations by their type.
00073 @param keywords: Filter annotations by their keywords.
00074 @param relationships: Filter annotations by their relationships.
00075 @param srv_namespace: World canvas handles can be found under this namespace.
00076 @raise WCFError: If something went wrong.
00077 '''
00078 if not srv_namespace.endswith('/'):
00079 self._srv_namespace = srv_namespace + '/'
00080 else:
00081 self._srv_namespace = srv_namespace
00082 self._annotations = list()
00083 self._annots_data = list()
00084 self._annots_to_delete = list()
00085 self._saved = True
00086
00087 if world is not None:
00088
00089 self.filter_by(world, uuids, names, types, keywords, relationships)
00090
00091 def filter_by(self, world, uuids=[], names=[], types=[], keywords=[], relationships=[]):
00092 '''
00093 Reload annotations collection, filtered by new selection criteria.
00094
00095 @param world: Annotations in this collection belong to this world.
00096 @param uuids: Filter annotations by their uuid.
00097 @param names: Filter annotations by their name.
00098 @param types: Filter annotations by their type.
00099 @param keywords: Filter annotations by their keywords.
00100 @param relationships: Filter annotations by their relationships.
00101 @raise WCFError: If something went wrong.
00102 '''
00103 rospy.loginfo("Getting annotations for world '%s' and additional filter criteria", world)
00104 get_anns_srv = self._get_service_handle('get_annotations', world_canvas_msgs.srv.GetAnnotations)
00105
00106 ids = [unique_id.toMsg(uuid.UUID('urn:uuid:' + id)) for id in uuids]
00107 relationships = [unique_id.toMsg(uuid.UUID('urn:uuid:' + id)) for id in relationships]
00108 response = get_anns_srv(world, ids, names, types, keywords, relationships)
00109
00110 if response.result:
00111 if len(response.annotations) > 0:
00112 rospy.loginfo("%d annotations found", len(response.annotations))
00113 self._annotations = response.annotations
00114 else:
00115 rospy.loginfo("No annotations found for world '%s' with the given search criteria", world)
00116 del self._annotations[:]
00117
00118
00119 del self._annots_data[:]
00120 del self._annots_to_delete[:]
00121 self._saved = True
00122 else:
00123 message = "Server reported an error: %s" % response.message
00124 rospy.logerr(message)
00125 raise WCFError(message)
00126
00127 def get_annotations(self, name=None, type=None):
00128 '''
00129 Return the currently loaded annotations, optionally restricted to the given name and/or type.
00130
00131 @param name: Get only annotation(s) with the given name. Can be more than one, as we don't
00132 enforce uniqueness on annotation names.
00133 @param type: Get only annotation(s) with the given type.
00134 @returns Currently loaded annotations matching name and type, or empty string if none.
00135 '''
00136 ret = []
00137 if len(self._annotations) == 0:
00138 message = "No annotations retrieved. Nothing to return!"
00139 rospy.logwarn(message)
00140 else:
00141
00142 if name:
00143 name_filtered = [a for a in self._annotations if a.name == name]
00144 else:
00145 name_filtered = [a for a in self._annotations]
00146
00147 if type:
00148 ret = [copy.deepcopy(a) for a in name_filtered if a.type == type]
00149 else:
00150 ret = [copy.deepcopy(a) for a in name_filtered]
00151 return ret
00152
00153 def load_data(self):
00154 '''
00155 Load associated data for the current annotations collection.
00156
00157 @returns Number of annotations retrieved.
00158 @raise WCFError: If something went wrong.
00159 '''
00160 if len(self._annotations) == 0:
00161 message = "No annotations retrieved. Nothing to load!"
00162 rospy.logwarn(message)
00163 return 0
00164
00165 get_data_srv = self._get_service_handle('get_annotations_data', world_canvas_msgs.srv.GetAnnotationsData)
00166 rospy.loginfo("Loading data for the %d retrieved annotations", len(self._annotations))
00167 response = get_data_srv([a.data_id for a in self._annotations])
00168
00169 if response.result:
00170 if len(response.data) > 0:
00171 rospy.loginfo("%d annotations data retrieved", len(response.data))
00172 self._annots_data = response.data
00173 else:
00174 rospy.logwarn("No data found for the %d retrieved annotations", len(self._annotations))
00175 del self._annots_data[:]
00176 else:
00177 message = "Server reported an error: %s" % response.message
00178 rospy.logerr(message)
00179 raise WCFError(message)
00180
00181 return len(response.data)
00182
00183 def get_data(self, annotation):
00184 '''
00185 Get the associated data (ROS message) of a given annotation.
00186
00187 @returns The ROS message associated to the given annotation or None if it was not found.
00188 @raise WCFError: If something else went wrong.
00189 '''
00190 for d in self._annots_data:
00191 if d.id == annotation.data_id:
00192
00193 msg_class = roslib.message.get_message_class(d.type)
00194 if msg_class is None:
00195
00196
00197 message = "Data type '%s' definition not found" % d.type
00198 rospy.logerr(message)
00199 raise WCFError(message)
00200 try:
00201 object = deserialize_msg(d.data, msg_class)
00202 except SerializationError as e:
00203 message = "Deserialization failed: %s" % str(e)
00204 rospy.logerr(message)
00205 raise WCFError(message)
00206 return object
00207 rospy.logwarn("Data uuid not found: " + unique_id.toHexString(annotation.data_id))
00208 return None
00209
00210 def get_data_of_type(self, type):
00211 '''
00212 Get all associated data (ROS messages) of a given type.
00213
00214 @returns The list of ROS messages of the given type.
00215 '''
00216 result = list()
00217
00218 for d in self._annots_data:
00219 try:
00220 object = deserialize_msg(d.data, type)
00221 except SerializationError as e:
00222
00223
00224
00225
00226
00227
00228 continue
00229
00230 result.append(object)
00231
00232 return result;
00233
00234 def publish_markers(self, topic):
00235 '''
00236 Publish RViz visualization markers for the current collection of annotations.
00237
00238 @param topic: Where we must publish annotations markers.
00239 '''
00240 if len(self._annotations) == 0:
00241 messages = "No annotations retrieved. Nothing to publish!"
00242 rospy.logwarn(messages)
00243 return
00244
00245
00246 markers_pub = rospy.Publisher(topic, MarkerArray, latch=True, queue_size=5)
00247
00248
00249 markers_list = MarkerArray()
00250
00251 marker_id = 1
00252 for a in self._annotations:
00253 marker = Marker()
00254 marker.id = marker_id
00255 marker.header = a.pose.header
00256 marker.type = a.shape
00257 marker.ns = a.type
00258 marker.action = Marker.ADD
00259 marker.lifetime = rospy.Duration.from_sec(0)
00260 marker.pose = copy.deepcopy(a.pose.pose.pose)
00261 marker.scale = a.size
00262 marker.color = a.color
00263
00264 markers_list.markers.append(marker)
00265
00266 label = copy.deepcopy(marker)
00267 label.id = label.id + 1000000
00268 label.type = Marker.TEXT_VIEW_FACING
00269 label.text = a.name + ' [' + a.type + ']'
00270 label.pose.position.z = label.pose.position.z + label.scale.z/2.0 + 0.12
00271 label.scale.x = label.scale.y = label.scale.z = 0.3
00272 label.color.a = 1.0
00273
00274 markers_list.markers.append(label)
00275
00276 marker_id = marker_id + 1
00277
00278
00279 markers_pub.publish(markers_list)
00280
00281 def publish(self, topic_name, topic_type=None, by_server=False, as_list=False, list_attribute=None):
00282 '''
00283 Publish the current collection of annotations, by this client or by the server.
00284 As we use just one topic, all annotations must be of the same type (function will return
00285 with error otherwise).
00286
00287 @param topic_name: Where we must publish annotations data.
00288 @param topic_type: The message type to publish annotations data.
00289 Mandatory if as_list is true; ignored otherwise.
00290 @param by_server: Request the server to publish the annotations instead of this client.
00291 @param as_list: If true, annotations will be packed in a list before publishing,
00292 so topic_type must be an array of currently loaded annotations.
00293 @raise WCFError: If something went wrong.
00294 '''
00295 if len(self._annotations) == 0:
00296 message = "No annotations retrieved. Nothing to publish!"
00297 rospy.logwarn(message)
00298 return
00299
00300 if by_server:
00301
00302 pub_data_srv = self._get_service_handle('pub_annotations_data', world_canvas_msgs.srv.PubAnnotationsData)
00303 rospy.loginfo("Requesting server to publish annotations")
00304 response = pub_data_srv([a.data_id for a in self._annotations], topic_name, topic_type, as_list)
00305 if not response.result:
00306 message = "Server reported an error: %s" % response.message
00307 rospy.logerr(message)
00308 raise WCFError(message)
00309 else:
00310
00311
00312 for a in self._annotations:
00313 if 'msg_type' not in locals():
00314 msg_type = a.type
00315 elif msg_type != a.type:
00316 message = "Cannot publish annotations of different types (%s, %s)" % (msg_type, a.type)
00317 rospy.logerr(message)
00318 raise WCFError(message)
00319
00320 if 'msg_type' not in locals():
00321 message = "Annotations message type not found? impossible! (we already checked at method start)"
00322 rospy.logerr(message)
00323 raise WCFError(message)
00324
00325
00326 msg_class = roslib.message.get_message_class(msg_type)
00327 if msg_class is None:
00328
00329
00330 message = "Topic type '%s' definition not found" % topic_type
00331 rospy.logerr(message)
00332 raise WCFError(message)
00333
00334
00335
00336 if as_list:
00337 if topic_type is None:
00338 message ="Topic type argument is mandatory if as_list is true"
00339 rospy.logerr(message)
00340 raise WCFError(message)
00341 topic_class = roslib.message.get_message_class(topic_type)
00342 if topic_class is None:
00343
00344 message = "Topic type '%s' definition not found" % topic_type
00345 rospy.logerr(message)
00346 raise WCFError(message)
00347 else:
00348 topic_class = msg_class
00349
00350
00351 objects_pub = rospy.Publisher(topic_name, topic_class, latch=True, queue_size=5)
00352
00353
00354 objects_list = self.get_data_of_type(msg_class)
00355
00356
00357 if as_list:
00358
00359 if list_attribute:
00360 try:
00361 msg = topic_class()
00362 setattr(msg, list_attribute, objects_list)
00363 except AttributeError as e:
00364 message = "List attribute caused error[%s]" % str(e)
00365 rospy.logerr(message)
00366 raise WCFError(message)
00367 else:
00368 msg = topic_class(objects_list)
00369 objects_pub.publish(msg)
00370 else:
00371
00372 for object in objects_list:
00373 objects_pub.publish(object)
00374
00375 def add(self, annotation, msg=None, gen_uuid=True):
00376 '''
00377 Add a new annotation with a new associated data or for an existing data.
00378
00379 @param annotation: The new annotation.
00380 @param msg: Its associated data. If None, we assume that we are adding an annotation to existing data.
00381 @param gen_uuid: Generate an unique id for the new annotation or use the received one.
00382 @raise WCFError: If something went wrong.
00383 '''
00384 if gen_uuid:
00385 annotation.id = unique_id.toMsg(unique_id.fromRandom())
00386 else:
00387 for a in self._annotations:
00388 if a.id == annotation.id:
00389 message = "Duplicated annotation with uuid '%s'" % unique_id.toHexString(annotation.id)
00390 rospy.logerr(message)
00391 raise WCFError(message)
00392
00393 if msg is None:
00394
00395 msg_found = False
00396 for d in self._annots_data:
00397 if d.id == annotation.data_id:
00398 rospy.logdebug("Annotation data with uuid '%s' found", unique_id.toHexString(annotation.data_id))
00399 msg_found = True
00400 break
00401
00402 if not msg_found:
00403 message = "Annotation data with uuid '%s' not found" % unique_id.toHexString(annotation.data_id)
00404 rospy.logerr(message)
00405 raise WCFError(message)
00406 else:
00407
00408 annotation.data_id = unique_id.toMsg(unique_id.fromRandom())
00409 annot_data = world_canvas_msgs.msg.AnnotationData()
00410 annot_data.id = annotation.data_id
00411 annot_data.type = annotation.type
00412 annot_data.data = serialize_msg(msg)
00413 self._annots_data.append(annot_data)
00414
00415 self._annotations.append(annotation)
00416 self._saved = False
00417
00418
00419 def update(self, annotation, msg=None):
00420 '''
00421 Update an existing annotation and optionally its associated data.
00422
00423 @param annotation: The modified annotation.
00424 @param msg: Its associated data. If None, just the annotation is updated.
00425 @raise WCFError: If something went wrong.
00426 '''
00427 found = False
00428 for i, a in enumerate(self._annotations):
00429 if a.id == annotation.id:
00430 self._annotations[i] = annotation
00431 found = True
00432 break
00433
00434 if not found:
00435 message = "Annotation with uuid '%s' not found" % unique_id.toHexString(annotation.id)
00436 rospy.logerr(message)
00437 raise WCFError(message)
00438
00439 if msg is None:
00440 return
00441
00442 found = False
00443 for i, d in enumerate(self._annots_data):
00444 if d.id == annotation.data_id:
00445 rospy.logdebug("Annotation data with uuid '%s' found", unique_id.toHexString(annotation.data_id))
00446 self._annots_data[i].type = annotation.type
00447 self._annots_data[i].data = serialize_msg(msg)
00448 found = True
00449 break
00450
00451 if not found:
00452 message = "Annotation data with uuid '%s' not found" % unique_id.toHexString(annotation.data_id)
00453 rospy.logerr(message)
00454 raise WCFError(message)
00455
00456 self._saved = False
00457
00458 def remove(self, uuid):
00459 '''
00460 Delete an annotation with its associated data.
00461 WARN/TODO: we are ignoring the case of N annotations - 1 data!
00462
00463 @param uuid: The uuid of the annotation to delete.
00464 @returns True if successfully removed, False if the annotation was not found.
00465 @raise WCFError: If something else went wrong.
00466 '''
00467 for a in self._annotations:
00468 if a.id == uuid:
00469 rospy.logdebug("Annotation '%s' found", unique_id.toHexString(uuid))
00470 ann_to_delete = a
00471 break
00472
00473 if 'ann_to_delete' not in locals():
00474 rospy.logwarn("Annotation '%s' not found", unique_id.toHexString(uuid))
00475 return False
00476
00477 for d in self._annots_data:
00478 if d.id == a.data_id:
00479 rospy.logdebug("Annotation data '%s' found", unique_id.toHexString(d.id))
00480 data_to_delete = d
00481 break
00482
00483 if 'data_to_delete' not in locals():
00484 message = "No data found for annotation '%s' (data uuid is '%s')" \
00485 % (unique_id.toHexString(uuid), unique_id.toHexString(ann_to_delete.data_id))
00486 rospy.logerr(message)
00487 raise WCFError(message)
00488
00489 rospy.logdebug("Removed annotation with uuid '%s'", unique_id.toHexString(ann_to_delete.id))
00490 rospy.logdebug("Removed annot. data with uuid '%s'", unique_id.toHexString(data_to_delete.id))
00491 self._annotations.remove(ann_to_delete)
00492 self._annots_data.remove(data_to_delete)
00493
00494 self._annots_to_delete.append(ann_to_delete)
00495 self._saved = False
00496
00497 return True
00498
00499 def save(self):
00500 '''
00501 Save to database current annotations list with their associated data. Also remove from database
00502 the annotations doomed by delete method, if any.
00503 WARN/TODO: we are ignoring the case of N annotations - 1 data!
00504
00505 @raise WCFError: If something went wrong.
00506 '''
00507 rospy.loginfo("Requesting server to save annotations")
00508 annotations = []
00509 annots_data = []
00510
00511
00512
00513 for a in self._annotations:
00514 for d in self._annots_data:
00515 if a.data_id == d.id:
00516 rospy.logdebug("Add annotation for saving with uuid '%s'", unique_id.toHexString(a.id))
00517 rospy.logdebug("Add annot. data for saving with uuid '%s'", unique_id.toHexString(d.id))
00518 annotations.append(a)
00519 annots_data.append(d)
00520 break
00521
00522
00523 if not (len(self._annotations) == len(self._annots_data) == len(annotations) == len(annots_data)):
00524 message = "Incoherent annotation and data sizes: %d != %d != %d != %d" \
00525 % (len(self._annotations), len(self._annots_data), len(annotations), len(annots_data))
00526 rospy.logerr(message)
00527 raise WCFError(message)
00528
00529
00530 save_data_srv = self._get_service_handle('save_annotations_data', world_canvas_msgs.srv.SaveAnnotationsData)
00531 rospy.loginfo("Requesting server to save annotations")
00532 response = save_data_srv(annotations, annots_data)
00533 if not response.result:
00534 message = "Server reported an error: %s" % response.message
00535 rospy.logerr(message)
00536 raise WCFError(message)
00537
00538
00539 if len(self._annots_to_delete) > 0:
00540 del_anns_srv = self._get_service_handle('delete_annotations', world_canvas_msgs.srv.DeleteAnnotations)
00541 rospy.loginfo("Requesting server to delete %d doomed annotations" % len(self._annots_to_delete))
00542 response = del_anns_srv(self._annots_to_delete)
00543 if not response.result:
00544 message = "Server reported an error: %s" % response.message
00545 rospy.logerr(message)
00546 raise WCFError(message)
00547
00548 self._saved = True
00549
00550 def is_saved(self):
00551 return self._saved
00552
00553 def get_world_list(self):
00554 '''
00555 TODO: temporally here until we create a WorldCollection class, as on C++ library.
00556
00557 @returns Currently available world list in the world canvas server
00558 '''
00559 get_world_list_srv = self._get_service_handle('list_worlds', world_canvas_msgs.srv.ListWorlds)
00560 response = get_world_list_srv()
00561
00562 return response.names
00563
00564 def _get_service_handle(self, service_name, service_type, timeout=5.0):
00565 '''
00566 Create a service client and wait until the service is available.
00567
00568 @param service_name: ROS service name to get, without namespace.
00569 @param service_type: ROS service type.
00570 @param timeout: Timeout to wait for the service to come up.
00571
00572 @returns: Service handle.
00573 @rtypes: rospy.ServiceProxy.
00574
00575 @raise WCFError: If specified timeout is exceeded or shutdown interrupts wait.
00576 '''
00577 try:
00578 rospy.loginfo("Waiting for '%s' service..." % str(service_name))
00579 rospy.wait_for_service(self._srv_namespace + service_name, timeout)
00580 srv = rospy.ServiceProxy(self._srv_namespace + service_name, service_type)
00581 return srv
00582 except rospy.exceptions.ROSInterruptException as e:
00583 raise WCFError("Wait interrupted by shutdown: %s" % str(e))
00584 except rospy.exceptions.ROSException as e:
00585 raise WCFError("%d seconds elapsed: %s" % (timeout, str(e)))