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)
def create_master_task(): """ Create an example of a task which we'll copy for other tasks later. This is a good example of creating a task with a variety of arguments. """ # need message store to pass objects around msg_store = MessageStoreProxy() # get the pose of a named object pose_name = "my favourite pose" # get the pose if it's there message, meta = msg_store.query_named(pose_name, Pose._type) # if it's not there, add it in if message == None: message = Pose(Point(0, 1, 2), Quaternion(3, 4, 5, 6)) pose_id = msg_store.insert_named(pose_name, message) else: pose_id = meta["_id"] master_task = Task(action='test_task') task_utils.add_string_argument(master_task, 'hello world') task_utils.add_object_id_argument(master_task, pose_id, Pose) task_utils.add_int_argument(master_task, 24) task_utils.add_float_argument(master_task, 63.678) return master_task
def get_location_pose(name): msg_store = MessageStoreProxy() p = msg_store.query_named(name, Pose._type) if p == None: raise LookupError("Location is not found in scene database", name) return p
def test_replication(self): replication_db = "replication_test" replication_col = "replication_test" # connect to destination for replication try: self.assertTrue(wait_for_mongo(ns="/datacentre2"), "wait for mongodb server") dst_client = import_MongoClient()("localhost", 49163) count = dst_client[replication_db][replication_col].count() self.assertEqual(count, 0, "No entry in destination") except pymongo.errors.ConnectionFailure: self.fail("Failed to connect to destination for replication") # insert an entry to move self.assertTrue(wait_for_mongo(), "wait for mongodb server") msg_store = MessageStoreProxy( database=replication_db, collection=replication_col) msg = Wrench() msg_name = "replication test message" self.assertIsNotNone(msg_store.insert_named(msg_name, msg), "inserted message") # move entries rospy.sleep(3) retcode = subprocess.check_call([ get_script_path(), '--move-before', '0', replication_db, replication_col]) self.assertEqual(retcode, 0, "replicator_client returns code 0") # check if replication was succeeded rospy.sleep(3) count = dst_client[replication_db][replication_col].count() self.assertGreater(count, 0, "entry moved to the destination") # test deletion after move data, meta = msg_store.query_named(msg_name, Wrench._type) self.assertIsNotNone(data, "entry is still in source") retcode = subprocess.check_call([ get_script_path(), '--move-before', '0', '--delete-after-move', replication_db, replication_col]) self.assertEqual(retcode, 0, "replicator_client returns code 0") data, meta = msg_store.query_named("replication test", Wrench._type) self.assertIsNone(data, "moved entry is deleted from source")
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
msg_store = MessageStoreProxy() p = Pose(Point(0, 1, 2), Quaternion(3, 4, 5, 6)) try: # 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
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
rospy.init_node('kcl_turtlebot_init', anonymous=True) msproxy = MessageStoreProxy() try: get_instances = rospy.ServiceProxy( '/rosplan_knowledge_base/state/instances', GetInstanceService) rospy.loginfo("Waiting for the instances service to be ready...") get_instances.wait_for_service(60) instances = get_instances('waypoint').instances except rospy.ServiceException, e: rospy.logerr("Service call failed: %s", e) sys.exit(-1) poses = [] for i in instances: poses.append(msproxy.query_named(i, PoseStamped._type)[0]) d = compute_distances(poses) try: msgs = get_kb_update(instances, d) update_kb = rospy.ServiceProxy('/rosplan_knowledge_base/update_array', KnowledgeUpdateServiceArray) res = update_kb(msgs) if res.success: print "Done!" else: print "Something went wrong!" except rospy.ServiceException, e: print "Service call failed: %s" % e sys.exit(-1)
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
class RosplanSensing: def __init__(self): self.mutex = Lock() ################################################################################################################ # Init clients and publishers rospy.wait_for_service('/rosplan_knowledge_base/update_array') self.update_kb_srv = rospy.ServiceProxy( '/rosplan_knowledge_base/update_array', KnowledgeUpdateServiceArray) rospy.wait_for_service( '/rosplan_knowledge_base/domain/predicate_details') self.get_predicates_srv = rospy.ServiceProxy( '/rosplan_knowledge_base/domain/predicate_details', GetDomainPredicateDetailsService) rospy.wait_for_service('/rosplan_knowledge_base/domain/functions') self.get_functions_srv = rospy.ServiceProxy( '/rosplan_knowledge_base/domain/functions', GetDomainAttributeService) rospy.wait_for_service('/rosplan_knowledge_base/state/instances') self.get_instances_srv = rospy.ServiceProxy( '/rosplan_knowledge_base/state/instances', GetInstanceService) rospy.wait_for_service('/rosplan_knowledge_base/state/propositions') self.get_state_propositions_srv = rospy.ServiceProxy( '/rosplan_knowledge_base/state/propositions', GetAttributeService) rospy.wait_for_service('/rosplan_knowledge_base/state/functions') self.get_state_functions_srv = rospy.ServiceProxy( '/rosplan_knowledge_base/state/functions', GetAttributeService) ################################################################################################################ # Get cfg self.cfg_topics = self.cfg_service = {} self.functions_path = None found_config = False if rospy.has_param('~topics'): self.cfg_topics = rospy.get_param('~topics') found_config = True if rospy.has_param('~predicates/services'): self.cfg_service = rospy.get_param('~services') found_config = True if rospy.has_param('~functions'): self.functions_path = rospy.get_param('~functions')[0] regexp = re.compile('\$\(find (.*)\)') groups = regexp.match(self.functions_path).groups() if len(groups): try: ros_pkg_path = rospkg.RosPack().get_path(groups[0]) self.functions_path = regexp.sub(ros_pkg_path, self.functions_path) except: rospy.logerr( 'KCL: (RosplanSensing) Error: Package %s was not found! Fix configuration file and retry.' % groups[0]) rospy.signal_shutdown('Wrong path in cfg file') return if not found_config: rospy.logerr( 'KCL: (RosplanSensing) Error: configuration file is not defined!' ) rospy.signal_shutdown('Config not found') return # Prepare MessageStoreProxy # Todo check if message_Store is present before creating it? self.msproxy = MessageStoreProxy() ################################################################################################################ # Load scripts if self.functions_path: self.scripts = imp.load_source('sensing_scripts', self.functions_path) # Declare tools in the scripts module: self.scripts.mongodb = self.mongodb self.scripts.get_kb_attribute = self.get_kb_attribute self.scripts.rospy = rospy ################################################################################################################ # Init variables self.sensed_topics = {} self.sensed_services = {} # TODO self.params = {} # Params defined for each predicate # Subscribe to all the topics self.offset = {} # Offset for reading cfg for predicate_name, predicate_info in self.cfg_topics.iteritems(): params_loaded = self.load_params( predicate_name, predicate_info) # If parameters found, add 1 to the indexes if not params_loaded[0]: continue offset = self.offset[predicate_name] = int(params_loaded[1]) if len(predicate_info) < 2 + offset: rospy.logerr( "Error: Wrong configuration file for predicate %s" % predicate_name) continue msg_type = predicate_info[offset + 1] self.import_msg(msg_type) rospy.Subscriber(predicate_info[offset + 0], eval(msg_type[msg_type.rfind('/') + 1:]), self.subs_callback, predicate_name) # self.sensed_topics[predicate_name] = (None, False) rospy.loginfo( 'KCL: (RosplanSensing) Predicate %s: Subscribed to topic %s of type %s', predicate_name, predicate_info[offset + 0], msg_type) # Returns (bool, bool), first tells if the parameters could be loaded, second if parameters were loaded from config file and false if they were taken directly from the kb def load_params(self, predicate_name, predicate_info): # Check if predicate kb_info = None try: kb_info = self.get_predicates_srv.call( GetDomainPredicateDetailsServiceRequest( predicate_name)).predicate except Exception as e: kb_info = self.get_function_params(predicate_name) if not kb_info: rospy.logerr( "KCL: (RosplanSensing) Could not find predicate or function %s" % predicate_name) return (False, False) # Obtain all the instances for each parameter kb_params = [] for p in kb_info.typed_parameters: instances = self.get_instances_srv.call( GetInstanceServiceRequest(p.value)).instances kb_params.append(instances) if predicate_info[0].startswith('params'): params = predicate_info[0].split() params.pop(0) # Remove the 'params' string if len(kb_params) != len(params): rospy.logerr( "KCL: (RosplanSensing) Parameters defined for predicate %s don't match the knowledge base" % predicate_name) rospy.signal_shutdown('Wrong cfg file parameters definition') return (False, True) # Check params wildcard = False for i, p in enumerate(params): if p != '*' and p != '_': if p in kb_params[i]: kb_params[i] = [p] else: rospy.logerr( 'KCL: (RosplanSensing) Unknown parameter instance "%s" of type "%s" for predicate "%s"', p, kb_info.typed_parameters[i].value, predicate_name) rospy.signal_shutdown( 'Wrong cfg file parameters definition') return (False, True) else: wildcard = True # If the params are fully instantiated we store them as a list, else it will be a matrix with a list of # instances per parameter self.params[predicate_name] = kb_params if wildcard else params return (True, True) else: self.params[predicate_name] = kb_params return (True, False) def get_function_params(self, func_name): functions = self.get_functions_srv.call() for i in functions.items: if i.name == func_name: return i return None def subs_callback(self, msg, pred_name): offset = self.offset[pred_name] if len(self.cfg_topics[pred_name]) == 3 + offset: python_string = self.cfg_topics[pred_name][2 + offset] else: # Call the method from the scripts.py file if not pred_name in dir(self.scripts): rospy.logerr( 'KCL: (RosplanSensing) Predicate "%s" does not have either a function or processing information' % pred_name) return None python_string = "self.scripts." + pred_name + "(msg, self.params[pred_name])" result = eval(python_string, globals(), locals()) changed = False if type(result) is list: for params, val in result: self.mutex.acquire( True ) # ROS subscribers are multithreaded in Python FIXME should lock the whole loop? try: if type(val) is bool: changed = self.sensed_topics[pred_name + ':' + params][0] ^ val else: changed = self.sensed_topics[pred_name + ':' + params][0] != val except: # If hasn't been added yet just ignore it changed = True self.sensed_topics[pred_name + ':' + params] = (val, changed) self.mutex.release() else: params = reduce(lambda r, x: r + ':' + x, self.params[pred_name]) if type(params) == list: rospy.logerr( 'KCL: (RosplanSensing) Predicate "%s" needs to have all the parameters defined and fully instantiated' % pred_name) rospy.signal_shutdown('Wrong cfg params') return None self.mutex.acquire( True) # ROS subscribers are multithreaded in Python try: if type(result) is bool: changed = self.sensed_topics[pred_name + ':' + params][0] ^ result else: changed = self.sensed_topics[pred_name + ':' + params][0] != result except: # If hasn't been added yet just ignore it changed = True self.sensed_topics[pred_name + ':' + params] = (result, changed) self.mutex.release() # Import a ros msg type of type pkg_name/MessageName def import_msg(self, ros_msg_string): # msg_string will be something like std_msgs/String -> convert it to from std_msgs.msg import String i = ros_msg_string.find('/') pkg_name = ros_msg_string[:i] msg_name = ros_msg_string[i + 1:] exec('from ' + pkg_name + ".msg import " + msg_name, globals()) def update_kb(self): kus = KnowledgeUpdateServiceArrayRequest() # Get info from KB functions and propositions (to know type of variable) # Fill update self.mutex.acquire(True) sensed_topics_cp = self.sensed_topics.copy() self.mutex.release() for predicate, (val, changed) in sensed_topics_cp.iteritems(): if not changed: continue self.mutex.acquire(True) self.sensed_topics[predicate] = (self.sensed_topics[predicate][0], False) # Set to not changed self.mutex.release() predicate_info = predicate.split(':') ki = KnowledgeItem() ki.attribute_name = predicate_info.pop(0) if type(val) is bool: ki.knowledge_type = ki.FACT ki.is_negative = not val else: ki.knowledge_type = ki.FUNCTION ki.function_value = val # TODO if no parameters iterate over all the instantiated parameters and add it? for i, param in enumerate(predicate_info): kv = KeyValue() kv.key = 'p' + str(i) kv.value = param ki.values.append(kv) kus.update_type += np.array(kus.ADD_KNOWLEDGE).tostring() kus.knowledge.append(ki) # Update the KB with the full array if len(kus.update_type) > 0: try: self.update_kb_srv.call(kus) except Exception as e: rospy.logerr( "KCL (SensingInterface) Failed to update knowledge base: %s" % e.message) # Returns the query to the mongodb messageproxy object def mongodb(self, query, ret_type): return self.msproxy.query_named(query, ret_type) def get_kb_attribute(self, attribute_name): request = GetAttributeServiceRequest(attribute_name) ret = self.get_state_propositions_srv.call(request) if len(ret.attributes) > 0: return ret.attributes return self.get_state_functions_srv.call(request).attributes
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)
p = Pose(Point(0, 1, 2), Quaternion(3, 4, 5, 6)) try: # 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
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))