def test_add_message(self): msg_store = MessageStoreProxy() POSE_NAME = "__test__pose__" p = Pose(Point(0, 1, 2), Quaternion(0, 0, 0, 1)) # insert a pose object with a name msg_store.insert_named(POSE_NAME, p) # get it back with a name stored, meta = msg_store.query_named(POSE_NAME, Pose._type) self.assertIsInstance(stored, Pose) self.assertEqual(stored.position.x, p.position.x) self.assertEqual(stored.position.y, p.position.y) self.assertEqual(stored.position.z, p.position.z) self.assertEqual(stored.orientation.x, p.orientation.x) self.assertEqual(stored.orientation.y, p.orientation.y) self.assertEqual(stored.orientation.z, p.orientation.z) self.assertEqual(stored.orientation.w, p.orientation.w) p.position.x = 666 msg_store.update_named(POSE_NAME, p) # get it back with a name updated = msg_store.query_named(POSE_NAME, Pose._type)[0] self.assertEqual(updated.position.x, p.position.x) # # try to get it back with an incorrect name wrong_name = "thid name does not exist in the datacentre" none_item = msg_store.query_named(wrong_name, Pose._type)[0] self.assertIsNone(none_item) # # get all non-existant typed objects, so get an empty list back none_query = msg_store.query( "not my type") self.assertEqual(len(none_query), 0) # add 100 query and sort by date inserted. for i in range(100): p = Pose(Point(0, 0, 0), Quaternion(i, 0, 100, 1)) msg_store.insert(p) result = msg_store.query(Pose._type, message_query={ 'orientation.z': {'$gt': 10} }, sort_query=[("$natural", -1)]) self.assertEqual(len(result), 100) self.assertEqual(result[0][0].orientation.x, 99) # must remove the item or unittest only really valid once print meta["_id"] print str(meta["_id"]) deleted = msg_store.delete(str(meta["_id"])) self.assertTrue(deleted)
class ArtDB: def __init__(self): self.db = MessageStoreProxy() self.lock = threading.RLock() self.srv_get_program = rospy.Service('/art/db/program/get', getProgram, self.srv_get_program_cb) self.srv_get_program_headers = rospy.Service( '/art/db/program_headers/get', getProgramHeaders, self.srv_get_program_headers_cb) self.srv_store_program = rospy.Service('/art/db/program/store', storeProgram, self.srv_store_program_cb) self.srv_delete_program = rospy.Service('/art/db/program/delete', ProgramIdTrigger, self.srv_delete_program_cb) self.srv_ro_set_program = rospy.Service('/art/db/program/readonly/set', ProgramIdTrigger, self.srv_ro_set_program_cb) self.srv_ro_clear_program = rospy.Service( '/art/db/program/readonly/clear', ProgramIdTrigger, self.srv_ro_clear_program_cb) self.srv_get_object = rospy.Service('/art/db/object_type/get', getObjectType, self.srv_get_object_cb) self.srv_store_object = rospy.Service('/art/db/object_type/store', storeObjectType, self.srv_store_object_cb) self.srv_get_collision_primitives = rospy.Service( '/art/db/collision_primitives/get', GetCollisionPrimitives, self.srv_get_collision_primitives_cb) self.srv_add_collision_primitive = rospy.Service( '/art/db/collision_primitives/add', AddCollisionPrimitive, self.srv_add_collision_primitive_cb) self.srv_clear_collision_primitive = rospy.Service( '/art/db/collision_primitives/clear', ClearCollisionPrimitives, self.srv_clear_collision_primitives_cb) rospy.loginfo('art_db ready') def srv_clear_collision_primitives_cb(self, req): resp = ClearCollisionPrimitivesResponse(success=False) try: # if any name is given, remove all if not req.names: primitives = self.db.query(CollisionPrimitive._type) rospy.loginfo("Removing " + str(len(primitives)) + " collision primitives.") for prim in primitives: self.db.delete(str(prim[1]["_id"])) else: for name in req.names: if name == "": rospy.logwarn("Ignoring empty name.") continue primitive = self.db.query(CollisionPrimitive._type, message_query={ "name": name, "setup": req.setup }, single=True) if None in primitive: rospy.logwarn("Unknown primitive name: " + name) continue self.db.delete(str(primitive[1]["_id"])) except rospy.ServiceException as e: rospy.logerr("Service call failed: " + str(e)) return resp resp.success = True return resp def srv_add_collision_primitive_cb(self, req): resp = AddCollisionPrimitiveResponse(success=False) if req.primitive.name == "": rospy.logerr("Empty primitive name!") return resp if req.primitive.setup == "": rospy.logerr("Empty setup name!") return resp try: ret = self.db.update_named("collision_primitive_" + req.primitive.name + "_" + req.primitive.setup, req.primitive, upsert=True) except rospy.ServiceException as e: rospy.logerr("Service call failed: " + str(e)) return resp resp.success = ret.success return resp def srv_get_collision_primitives_cb(self, req): resp = GetCollisionPrimitivesResponse() for name in req.names: if name == "": rospy.logwarn("Ignoring empty name.") continue prim = self.db.query(CollisionPrimitive._type, message_query={ "name": name, "setup": req.setup }, single=True) if None in prim: rospy.logwarn("Unknown primitive name: " + name) continue resp.primitives.append(prim[0]) if not req.names: primitives = self.db.query(CollisionPrimitive._type, message_query={"setup": req.setup}) for prim in primitives: resp.primitives.append(prim[0]) return resp def _program_set_ro(self, program_id, ro): with self.lock: name = "program:" + str(program_id) resp = ProgramIdTriggerResponse() resp.success = False try: prog = self.db.query_named(name, Program._type)[0] except rospy.ServiceException as e: resp.error = str(e) return resp if not prog: resp.error = "Program does not exist" return resp prog.header.readonly = ro try: ret = self.db.update_named(name, prog, upsert=True) except rospy.ServiceException as e: resp.error = str(e) return resp resp.success = ret.success return resp def srv_ro_set_program_cb(self, req): return self._program_set_ro(req.program_id, True) def srv_ro_clear_program_cb(self, req): return self._program_set_ro(req.program_id, False) def srv_get_program_headers_cb(self, req): with self.lock: resp = getProgramHeadersResponse() programs = [] try: programs = self.db.query(Program._type) except rospy.ServiceException as e: print "Service call failed: " + str(e) for prog in programs: if len(req.ids) == 0 or prog[0].header.id in req.ids: resp.headers.append(prog[0].header) return resp def srv_delete_program_cb(self, req): with self.lock: resp = ProgramIdTriggerResponse() resp.success = False name = "program:" + str(req.program_id) try: meta = self.db.query_named(name, Program._type)[1] if self.db.delete(str(meta["_id"])): resp.success = True except rospy.ServiceException as e: pass return resp def srv_get_program_cb(self, req): with self.lock: resp = getProgramResponse() resp.success = False name = "program:" + str(req.id) prog = None try: prog = self.db.query_named(name, Program._type)[0] except rospy.ServiceException as e: print "Service call failed: " + str(e) if prog is not None: resp.program = prog resp.success = True return resp def srv_store_program_cb(self, req): with self.lock: resp = storeProgramResponse() resp.success = False name = "program:" + str(req.program.header.id) try: prog = self.db.query_named(name, Program._type)[0] except rospy.ServiceException as e: print "Service call failed: " + str(e) return resp if prog is not None and prog.header.readonly: resp.error = "Readonly program." return resp ph = ProgramHelper() if not ph.load(req.program): resp.error = "Invalid program" return resp try: ret = self.db.update_named(name, req.program, upsert=True) except rospy.ServiceException as e: print "Service call failed: " + str(e) return resp resp.success = ret.success return resp def srv_get_object_cb(self, req): with self.lock: resp = getObjectTypeResponse() resp.success = False name = "object_type:" + str(req.name) try: object_type = self.db.query_named(name, ObjectType._type)[0] except rospy.ServiceException as e: print "Service call failed: " + str(e) return resp if object_type: resp.success = True resp.object_type = object_type return resp rospy.logerr("Unknown object type: " + req.name) return resp def srv_store_object_cb(self, req): with self.lock: resp = storeObjectTypeResponse() name = "object_type:" + str(req.object_type.name) try: ret = self.db.update_named(name, req.object_type, upsert=True) except rospy.ServiceException as e: print "Service call failed: " + str(e) resp.success = False return resp resp.success = ret.success return resp
class DatabasePusher(object): # TF buffer tf2_buffer = None # Database class _msg_store = None def __init__(self): rospy.init_node('configuration_services') # TF self.tf2_buffer = tf2_ros.Buffer() self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer) # Create database proxy self._msg_store = MessageStoreProxy() # Create database reload service try: rospy.wait_for_service('/apps/database/reload', 0.1) self._database_service = rospy.ServiceProxy( '/apps/database/reload', Empty) except Exception as e: rospy.logerr('Could not initialize {}'.format(rospy.get_name())) rospy.loginfo('Exceptions: {}'.format(e)) return 0 # Define the service proxies try: save_tf_srvs = rospy.Service('tf_capture', CaptureTF, self._handle_tf_save) save_joint_srvs = rospy.Service('joint_capture', CaptureJoint, self._handle_joint_save) save_dmp_srvs = rospy.Service('dmp_capture', CaptureDMP, self._handle_dmp_save) except Exception as e: rospy.logerr('Could not initialize {}'.format(rospy.get_name())) rospy.loginfo('Exceptions: {}'.format(e)) return 0 # Notify the user the node is running rospy.loginfo("Hepling hand is running!") rospy.spin() def _handle_joint_save(self, req): try: # Display what you are doing rospy.loginfo( "Storing the provided joint configuration as <{}> into the database ..." .format(req.entry_name)) # Save to DB self._save_to_db(req.joints, req.entry_name) # Return the success return CaptureJointResponse(message='Frame saved', success=True) except Exception as e: # Handle exceptions rospy.logerr( "Failed to handle request with exception:\n{}".format(e)) return CaptureJointResponse( message='Failed with exception:\n{}'.format(e), success=False) def _handle_pose_save(self, req): # Work in progress, not implemented yet pass def _handle_topic_save(self, req): # Work in progress, not implemented yet pass def _handle_tf_save(self, req): try: # Retrieve the transformation from the TF buffer resulting_frame = self.tf2_buffer.lookup_transform( req.from_frame, req.to_frame, rospy.Time()) # Set the tranformation's ID to the desired one resulting_frame.child_frame_id = req.new_frame_name # Display what you are doing rospy.loginfo( "Storing transformation from <{0}> to <{1}> with the new name <{2}> into the database ..." .format(req.from_frame, req.to_frame, req.new_frame_name)) # Save to DB self._save_to_db(resulting_frame, req.new_frame_name) # Return the success return CaptureTFResponse(message='Frame saved', success=True) except Exception as e: # Handle exceptions rospy.logerr( "Failed to handle request with exception:\n{}".format(e)) return CaptureTFResponse( message='Failed with exception:\n{}'.format(e), success=False) def _handle_dmp_save(self, req): try: # Display what you are doing rospy.loginfo( "Storing the provided DMP as <{}> into the database ...". format(req.entry_name)) if len(req.cartesian_dmp.w) == len(req.joint_dmp.w): raise Exception( "Either both DMP are provided or none. Not saving.") if len(req.joint_dmp.w) != 1: self._save_to_db(req.joint_dmp, req.entry_name) if len(req.cartesian_dmp.w) != 1: self._save_to_db(req.cartesian_dmp, req.entry_name) # Return the success return CaptureDMPResponse(message='Frame saved', success=True) except Exception as e: # Handle exceptions rospy.logerr( "Failed to handle request with exception:\n{}".format(e)) return CaptureDMPResponse( message='Failed with exception:\n{}'.format(e), success=False) def _save_to_db(self, entry, name): # If you cannot update an existing entry, make a new one if not (self._msg_store.update_named(name, entry).success): self._msg_store.insert_named(name, entry) rospy.loginfo('Entry <{}> inserted !!'.format(name)) else: rospy.loginfo('Entry <{}> updated !!'.format(name)) # Tell the service to reload all the TFs self._database_service()
# insert a pose object with a name, store the id from db p_id = msg_store.insert_named("my favourite pose", p) # you don't need a name (note that this p_id is different than one above) p_id = msg_store.insert(p) p_id = msg_store.insert(['test1', 'test2']) # get it back with a name print msg_store.query_named("my favourite pose", Pose._type) p.position.x = 666 # update it with a name msg_store.update_named("my favourite pose", p) p.position.y = 2020 # update the other inserted one using the id msg_store.update_id(p_id, p) stored_p, meta = msg_store.query_id(p_id, Pose._type) assert stored_p.position.x == 666 assert stored_p.position.y == 2020 print "stored object ok" print "stored object inserted at %s (UTC rostime) by %s" % ( meta['inserted_at'], meta['inserted_by']) print "stored object last updated at %s (UTC rostime) by %s" % ( meta['last_updated_at'], meta['last_updated_by'])
class KnowledgeBaseInterface(object): '''Defines an interface for interacting with a knowledge base. @author Alex Mitrevski @contact [email protected] ''' def __init__(self): # a client for the /rosplan_knowledge_base/domain/predicates service self.predicate_retrieval_client = None # a client for the /rosplan_knowledge_base/update service self.knowledge_update_client = None # a client for the /rosplan_knowledge_base/state/propositions service self.attribute_retrieval_client = None # a mongodb_store.message_store.MessageStoreProxy instance self.msg_store_client = None # name of the host robot; value read from the knowledge base self.robot_name = '' rospy.loginfo( '[kb_interface] Creating a domain predicate retrieval client') try: rospy.wait_for_service('/rosplan_knowledge_base/domain/predicates', 5.) self.predicate_retrieval_client = rospy.ServiceProxy( '/rosplan_knowledge_base/domain/predicates', rosplan_srvs.GetDomainAttributeService) except (rospy.ServiceException, rospy.ROSException) as exc: rospy.logwarn( 'The service /rosplan_knowledge_base/domain/predicates does not appear to exist.\n' + 'Please spawn rosplan_knowledge_base/knowledgeBase') rospy.loginfo('[kb_interface] Creating a knowledge base update client') try: rospy.wait_for_service('/rosplan_knowledge_base/update', 5.) self.knowledge_update_client = rospy.ServiceProxy( '/rosplan_knowledge_base/update', rosplan_srvs.KnowledgeUpdateService) except (rospy.ServiceException, rospy.ROSException) as exc: rospy.logwarn( 'The service /rosplan_knowledge_base/state/propositions does not appear to exist.\n' + 'Please spawn rosplan_knowledge_base/knowledgeBase') rospy.loginfo('[kb_interface] Creating a knowledge base query client') try: rospy.wait_for_service( '/rosplan_knowledge_base/state/propositions', 5.) self.attribute_retrieval_client = rospy.ServiceProxy( '/rosplan_knowledge_base/state/propositions', rosplan_srvs.GetAttributeService) request = rosplan_srvs.GetAttributeServiceRequest() request.predicate_name = 'robot_name' result = self.attribute_retrieval_client(request) for item in result.attributes: for param in item.values: if param.key == 'bot': self.robot_name = param.value break break except (rospy.ServiceException, rospy.ROSException) as exc: rospy.logwarn( 'The service /rosplan_knowledge_base/state/propositions does not appear to exist.\n' + 'Please spawn rosplan_knowledge_base/knowledgeBase') rospy.loginfo('[kb_interface] Creating a message store client') try: self.msg_store_client = MessageStoreProxy() except: print('Could not create a mongodb_store proxy.\n' + 'Please spawn mongodb_store/message_store_node.py') ############################################################################ #--------------------------- Symbolic knowledge ---------------------------# ############################################################################ def get_predicate_names(self): '''Returns a list with the names of all predicates in the domain. ''' request = rosplan_srvs.GetDomainAttributeServiceRequest() result = self.predicate_retrieval_client(request) predicate_names = [p.name for p in result.items] return predicate_names def get_all_attributes(self, predicate_name): '''Returns a list of all instances of the given predicate in the knowledge base. @param predicate_name -- string representing the name of a predicate ''' request = rosplan_srvs.GetAttributeServiceRequest() request.predicate_name = predicate_name result = self.attribute_retrieval_client(request) return result.attributes def update_kb(self, facts_to_add, facts_to_remove): '''Updates the knowledge base by adding and removing the facts from the given lists. In both input lists, each entry is a tuple of the form (predicate, [(variable, value), ...]), where "predicate" is the predicate name and the (variable, value) tuples are the variable values. Keyword arguments: @param facts_to_add -- a list of facts to add to the knowledge base @param facts_to_remove -- a list of facts to remove from the knowledge base ''' rospy.loginfo('[kb_interface] Inserting facts into the knowledge base') self.insert_facts(facts_to_add) rospy.loginfo('[kb_interface] Removing facts from the knowledge base') self.remove_facts(facts_to_remove) def insert_facts(self, fact_list): '''Inserts the facts in the given list into the knowledge base. Keyword arguments: @param fact_list -- a list in which each entry is a tuple of the form (predicate, [(variable, value), ...]), where "predicate" is the predicate name and the (variable, value) tuples are the variable values ''' self.__update_kb(KnowledgeUpdateTypes.INSERT, fact_list) def remove_facts(self, fact_list): '''Removes the facts in the given list from the knowledge base. Keyword arguments: @param fact_list -- a list in which each entry is a tuple of the form (predicate, [(variable, value), ...]), where "predicate" is the predicate name and the (variable, value) tuples are the variable values ''' self.__update_kb(KnowledgeUpdateTypes.REMOVE, fact_list) def insert_goals(self, goal_list): '''Inserts the goals in the given list into the knowledge base. Keyword arguments: @param goal_list -- a list in which each entry is a tuple of the form (predicate, [(variable, value), ...]), where "predicate" is the predicate name and the (variable, value) tuples are the variable values ''' self.__update_kb(KnowledgeUpdateTypes.INSERT_GOAL, goal_list) def remove_goals(self, goal_list): '''Removes the goals in the given list from the knowledge base. Keyword arguments: @param goal_list -- a list in which each entry is a tuple of the form (predicate, [(variable, value), ...]), where "predicate" is the predicate name and the (variable, value) tuples are the variable values ''' self.__update_kb(KnowledgeUpdateTypes.REMOVE_GOAL, goal_list) def __update_kb(self, update_type, fact_list): '''Updates the knowledge base with the facts in the given list. Keyword arguments: @param update_type -- a KnowledgeUpdateTypes constant describing the update type (add or remove facts) @param fact_list -- a list in which each entry is a tuple of the form (predicate, [(variable, value), ...]), where "predicate" is the predicate name and the (variable, value) tuples are the variable values ''' try: for predicate, var_data in fact_list: # we set up the knowledge update request with the given attribute data request = rosplan_srvs.KnowledgeUpdateServiceRequest() request.update_type = update_type request.knowledge.knowledge_type = 1 request.knowledge.attribute_name = predicate for var_name, var_value in var_data: arg_msg = diag_msgs.KeyValue() arg_msg.key = var_name arg_msg.value = var_value request.knowledge.values.append(arg_msg) # we modify the fact into the knowledge base self.knowledge_update_client(request) except Exception as exc: rospy.logerr('[kb_interface] %s', str(exc)) raise KBException('The knowledge base could not be updated') ############################################################################ #------------------------------ Data storage ------------------------------# ############################################################################ def insert_objects(self, object_list): '''Inserts the objects in the given list into the knowledge base. Keyword arguments: @param object_list -- a list of (obj_name, obj) tuples ''' for obj_name, obj in object_list: self.msg_store_client.insert_named(obj_name, obj) def insert_obj_instance(self, obj_name, obj): '''Inserts a named object instance into the knowledge base. Keyword arguments: @param obj_name -- name of the object instance @param obj -- object to insert ''' self.msg_store_client.insert_named(obj_name, obj) def update_objects(self, object_list): '''Updates the knowledge about the objects in the given list. Keyword arguments: @param object_list -- a list of (obj_name, obj) tuples ''' for obj_name, obj in object_list: self.msg_store_client.update_named(obj_name, obj) def update_obj_instance(self, obj_name, obj): '''Updates the knowledge about the given named object. Keyword arguments: @param obj_name -- name of the object instance @param obj -- object to insert ''' self.msg_store_client.update_named(obj_name, obj) def remove_objects(self, object_list): '''Removes the objects in the given list from the knowledge base. Keyword arguments: @param object_list -- a list of (obj_name, obj_type) tuples ''' for obj_name, obj_type in object_list: self.msg_store_client.delete_named(obj_name, obj_type) def remove_obj_instance(self, obj_name, obj_type): '''Removes a named object instance from the knowledge base. Keyword arguments: @param obj_name -- name of the object instance @param obj_type -- instance type (the value of the object's "_type" field) ''' self.msg_store_client.delete_named(obj_name, obj_type) def get_objects(self, object_names, obj_type): '''Returns a list of named object instances from the knowledge base. @param object_names -- a list of strings representing object names @param obj_type -- type of the objects to be retrieved ''' objects = list() for obj_name in object_names: obj = self.get_obj_instance(obj_name, obj_type) if obj is not None: objects.append(obj) return objects def get_obj_instance(self, obj_name, obj_type): '''Retrieves a named object instance from the knowledge base. Keyword arguments: @param obj_name -- name of the object instance @param obj_type -- instance type (the value of the object's "_type" field) ''' try: obj = self.msg_store_client.query_named(obj_name, obj_type)[0] return obj except: rospy.logerr('[kb_interface] Error retriving knowledge about %s', obj_name) return None
class ArtDB: def __init__(self): self.db = MessageStoreProxy() self.lock = threading.RLock() self.srv_get_program = rospy.Service('/art/db/program/get', getProgram, self.srv_get_program_cb) self.srv_get_program_headers = rospy.Service('/art/db/program_headers/get', getProgramHeaders, self.srv_get_program_headers_cb) self.srv_store_program = rospy.Service('/art/db/program/store', storeProgram, self.srv_store_program_cb) self.srv_delete_program = rospy.Service('/art/db/program/delete', ProgramIdTrigger, self.srv_delete_program_cb) self.srv_ro_set_program = rospy.Service('/art/db/program/readonly/set', ProgramIdTrigger, self.srv_ro_set_program_cb) self.srv_ro_clear_program = rospy.Service('/art/db/program/readonly/clear', ProgramIdTrigger, self.srv_ro_clear_program_cb) self.srv_get_object = rospy.Service('/art/db/object_type/get', getObjectType, self.srv_get_object_cb) self.srv_store_object = rospy.Service('/art/db/object_type/store', storeObjectType, self.srv_store_object_cb) rospy.loginfo('art_db ready') def _program_set_ro(self, program_id, ro): with self.lock: name = "program:" + str(program_id) resp = ProgramIdTriggerResponse() resp.success = False try: prog = self.db.query_named(name, Program._type)[0] except rospy.ServiceException as e: resp.error = str(e) return resp prog.header.readonly = ro try: ret = self.db.update_named(name, prog, upsert=True) except rospy.ServiceException as e: resp.error = str(e) return resp resp.success = ret.success return resp def srv_ro_set_program_cb(self, req): return self._program_set_ro(req.program_id, True) def srv_ro_clear_program_cb(self, req): return self._program_set_ro(req.program_id, False) def srv_get_program_headers_cb(self, req): with self.lock: resp = getProgramHeadersResponse() programs = [] try: programs = self.db.query(Program._type) except rospy.ServiceException as e: print "Service call failed: " + str(e) for prog in programs: if len(req.ids) == 0 or prog[0].header.id in req.ids: resp.headers.append(prog[0].header) return resp def srv_delete_program_cb(self, req): with self.lock: resp = ProgramIdTriggerResponse() resp.success = False name = "program:" + str(req.program_id) try: meta = self.db.query_named(name, Program._type)[1] if self.db.delete(str(meta["_id"])): resp.success = True except rospy.ServiceException as e: pass return resp def srv_get_program_cb(self, req): with self.lock: resp = getProgramResponse() resp.success = False name = "program:" + str(req.id) prog = None try: prog = self.db.query_named(name, Program._type)[0] except rospy.ServiceException as e: print "Service call failed: " + str(e) if prog is not None: resp.program = prog resp.success = True return resp def srv_store_program_cb(self, req): with self.lock: resp = storeProgramResponse() resp.success = False name = "program:" + str(req.program.header.id) try: prog = self.db.query_named(name, Program._type)[0] except rospy.ServiceException as e: print "Service call failed: " + str(e) return resp if prog is not None and prog.header.readonly: resp.error = "Readonly program." return resp ph = ProgramHelper() if not ph.load(req.program): resp.error = "Invalid program" return resp try: ret = self.db.update_named(name, req.program, upsert=True) except rospy.ServiceException as e: print "Service call failed: " + str(e) return resp resp.success = ret.success return resp def srv_get_object_cb(self, req): with self.lock: resp = getObjectTypeResponse() resp.success = False name = "object_type:" + str(req.name) try: object_type = self.db.query_named(name, ObjectType._type)[0] except rospy.ServiceException as e: print "Service call failed: " + str(e) return resp if object_type: resp.success = True resp.object_type = object_type return resp rospy.logerr("Unknown object type: " + req.name) return resp def srv_store_object_cb(self, req): with self.lock: resp = storeObjectTypeResponse() name = "object_type:" + str(req.object_type.name) try: ret = self.db.update_named(name, req.object_type, upsert=True) except rospy.ServiceException as e: print "Service call failed: " + str(e) resp.success = False return resp resp.success = ret.success return resp
def test_add_message(self): msg_store = MessageStoreProxy() POSE_NAME = "__test__pose__" p = Pose(Point(0, 1, 2), Quaternion(0, 0, 0, 1)) # insert a pose object with a name msg_store.insert_named(POSE_NAME, p) # get it back with a name stored, meta = msg_store.query_named(POSE_NAME, Pose._type) self.assertIsInstance(stored, Pose) self.assertEqual(stored.position.x, p.position.x) self.assertEqual(stored.position.y, p.position.y) self.assertEqual(stored.position.z, p.position.z) self.assertEqual(stored.orientation.x, p.orientation.x) self.assertEqual(stored.orientation.y, p.orientation.y) self.assertEqual(stored.orientation.z, p.orientation.z) self.assertEqual(stored.orientation.w, p.orientation.w) p.position.x = 666 msg_store.update_named(POSE_NAME, p) # get it back with a name updated = msg_store.query_named(POSE_NAME, Pose._type)[0] self.assertEqual(updated.position.x, p.position.x) # # try to get it back with an incorrect name wrong_name = "thid name does not exist in the datacentre" none_item = msg_store.query_named(wrong_name, Pose._type)[0] self.assertIsNone(none_item) # # get all non-existant typed objects, so get an empty list back none_query = msg_store.query("not my type") self.assertEqual(len(none_query), 0) # add 100 query and sort by date inserted. for i in range(100): p = Pose(Point(0, 0, 0), Quaternion(i, 0, 100, 1)) msg_store.insert(p) result = msg_store.query(Pose._type, message_query={'orientation.z': { '$gt': 10 }}, sort_query=[("$natural", -1)]) self.assertEqual(len(result), 100) self.assertEqual(result[0][0].orientation.x, 99) # get documents with limit result_limited = msg_store.query( Pose._type, message_query={'orientation.z': { '$gt': 10 }}, sort_query=[("$natural", 1)], limit=10) self.assertEqual(len(result_limited), 10) self.assertListEqual( [int(doc[0].orientation.x) for doc in result_limited], range(10)) #get documents without "orientation" field result_no_id = msg_store.query(Pose._type, message_query={}, projection_query={"orientation": 0}) for doc in result_no_id: self.assertEqual(int(doc[0].orientation.z), 0) # must remove the item or unittest only really valid once print meta["_id"] print str(meta["_id"]) deleted = msg_store.delete(str(meta["_id"])) self.assertTrue(deleted)
# insert a pose object with a name, store the id from db p_id = msg_store.insert_named("my favourite pose", p) # you don't need a name (note that this p_id is different than one above) p_id = msg_store.insert(p) p_id = msg_store.insert(['test1', 'test2']) # get it back with a name print msg_store.query_named("my favourite pose", Pose._type) p.position.x = 666 # update it with a name msg_store.update_named("my favourite pose", p) p.position.y = 2020 # update the other inserted one using the id msg_store.update_id(p_id, p) stored_p, meta = msg_store.query_id(p_id, Pose._type) assert stored_p.position.x == 666 assert stored_p.position.y == 2020 print "stored object ok" print "stored object inserted at %s (UTC rostime) by %s" % (meta['inserted_at'], meta['inserted_by']) print "stored object last updated at %s (UTC rostime) by %s" % (meta['last_updated_at'], meta['last_updated_by']) # some other things you can do...
class Navigator(): def __init__(self): print("Started init (Navigator)") #Latched position topic for keeping web up to date. self.Latched_Positions = rospy.Publisher( "/web_navigation/latched_position_names", NameList, latch=True, queue_size=1) self.last_recieved_position = None #Keeps track of last known position self.sub = rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, self.last_position_callback) #ActionClient for publishing navigation targets self.goal_client = actionlib.SimpleActionClient( 'move_base', MoveBaseAction) #Free to change rws_nav_position to whatever self.Mongo_Storer = MessageStoreProxy(database='rws_nav_positions', collection='web_positions') self.update_web() def last_position_callback(self, data): self.last_recieved_position = data def display_current_position(self): print(self.last_recieved_position) #Makes a PoseStamped with x, y, and w. def make_PoseStamped_xy(self, x, y, w=1.0): """Makes a PoseStamped with position x, y, and rotation w.""" #Wrong way to construct things - use default MoveBaseGoal() then MoveBaseGoal.target_pose.pose.x = 1... h = Header(seq=1, stamp=rospy.Time.now(), frame_id="map") p = Point(float(x), float(y), 0.0) q = Quaternion(0.0, 0.0, 0.0, w) return PoseStamped(header=h, pose=Pose(position=p, orientation=q)) def make_PoseStamped_from_Pose(self, p): """Adds a header to the Pose p.""" #Wrong way to construct things - use default MoveBaseGoal() then MoveBaseGoal.target_pose.pose.x = 1... h = Header(seq=1, stamp=rospy.Time.now(), frame_id="map") return PoseStamped(header=h, pose=p) def save_position(self, position, name): if self.get_position(name): print("Updating position: " + name) self.Mongo_Storer.update_named(name, position) else: print("Inserting new position: " + name) temp_id = self.Mongo_Storer.insert_named(name, position) self.Mongo_Storer.update_named(name, position, meta={"_id": temp_id}) self.update_web() def get_position(self, name): """Returns a PoseStamped saved position""" return self.Mongo_Storer.query_named(name, "geometry_msgs/PoseStamped")[0] def get_id(self, name): """Returns id of position named name in database""" print self.Mongo_Storer.query_named( name, "geometry_msgs/PoseStamped")[1]["_id"] return self.Mongo_Storer.query_named( name, "geometry_msgs/PoseStamped")[1]["_id"].__str__() def save_current_position(self, name): """If known, saves the robot's current position to the database""" if (self.last_recieved_position != None): new_pose = PoseStamped(header=self.last_recieved_position.header, pose=self.last_recieved_position.pose.pose) self.save_position(new_pose, name) else: print("Current position not known! Launch amcl/robot?") def publish_PoseStamped(self, pose): """Sends a navigation goal with pose""" print "Publishing pose:", pose to_pub = MoveBaseGoal(target_pose=pose) self.goal_client.send_goal(to_pub) def publish_saved_position(self, name): """Sends a navigation goal with the pose saved as name""" to_pub = self.get_position(name) if to_pub: self.publish_PoseStamped(to_pub) def publish_position_xy(self, x, y): """Sends a navigation goal with the position (x, y)""" self.publish_PoseStamped(self.make_PoseStamped_xy(x, y)) def read_saved_position(self, name): """Outputs the stored position named name to console""" print self.get_position(name) def delete_position(self, name): if self.get_position(name): print("Deleting: " + name) print "ID: ", self.get_id(name) self.Mongo_Storer.delete(self.get_id(name)) self.update_web() else: print("'" + name + "' is not in database!") def list_positions(self): print "Positions:" positions = self.Mongo_Storer.query("geometry_msgs/PoseStamped") for p in positions: print p[1]["name"] + ":" pos = p[0] # Heh print "\t(", pos.pose.position.x, ",", pos.pose.position.y, ")" def update_web(self): """Publishes to a latched topic to keep the web interface updated. Called whenever the stored positions are changed.""" position_list = [ p for p, m in self.Mongo_Storer.query("geometry_msgs/PoseStamped") ] position_name_list = [ m["name"] for p, m in self.Mongo_Storer.query("geometry_msgs/PoseStamped") ] print "Updating latched names..." self.Latched_Positions.publish(NameList(position_name_list))