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