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))
