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
class MessageStorage(object):
def __init__(self):
Logger.log("Connecting with MongoDB Database...")
self.msg_store = MessageStoreProxy()
Logger.log("Connected!")
def store(self, name, message):
try:
return self.msg_store.insert_named(name, message)
except rospy.ServiceException, e:
Logger.error(e)
return None
def get_scene_and_store_in_db():
get_scene_service = rospy.ServiceProxy('/get_scene', GetScene)
result = get_scene_service()
rospy.loginfo("Response from get_scene service: \n{}".format(result))
if result.success:
# clear previous items in scene db and knowledge base
clear_blocks_from_scene_db()
clear_locations_from_scene_db()
clear_service = rospy.ServiceProxy('/rosplan_knowledge_base/clear',
Empty)
result_clear = clear_service()
#print "result_clear = "
#print result_clear
msg_store = MessageStoreProxy()
listener = tf.TransformListener()
for block in result.blocks:
listener.waitForTransform("table", block.pose.header.frame_id,
rospy.Time(), rospy.Duration(4.0))
now = rospy.Time.now()
listener.waitForTransform("table", block.pose.header.frame_id, now,
rospy.Duration(4.0))
tmp_pose = listener.transformPose("table", block.pose)
block.pose = tmp_pose
rospy.loginfo(
"Storing following block in scene db: \n{}".format(block))
msg_store.insert_named('block{}'.format(block.id), block)
return result.success
def add_location(name, x, y, yaw=0):
rospy.loginfo(
"Add location {} in knowledge and scene db at x = {}, y = {}".format(
name, x, y))
if not add_instance("location", name):
return -1
msg_store = MessageStoreProxy()
q = quaternion_from_euler(0, 0, yaw)
p = Pose(Point(x, y, 0), Quaternion(*q))
location_id = msg_store.insert_named(name, p)
return location_id
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")
import roslib
import sys, os
from std_msgs.msg import String, Header, Int32
from mongodb_store.message_store import MessageStoreProxy
# from soma2_msgs.msg import SOMA2Object
if __name__ == "__main__":
rospy.init_node('query_soma')
# connecting
soma_store = MessageStoreProxy(database="message_store",
collection="soma_activity_ids_list")
if len(sys.argv) > 1:
for cnt, arg in enumerate(sys.argv):
if cnt == 0: continue
# print arg
print 'Add an object ID to a msg store list: %s ' % arg
new_obj_id = String(arg)
# putting something in
soma_store.insert_named("object id %s" % arg, new_obj_id)
# # getting it back out
# id,meta = soma_store.query_named("object id %s" %arg, Int32._type)
# print scene, meta
else:
print "Requires a list of SOMA object IDs to add to a database"
import mongodb_store.util as dc_util
from mongodb_store.message_store import MessageStoreProxy
from geometry_msgs.msg import Pose, Point, Quaternion
import StringIO
if __name__ == '__main__':
rospy.init_node("example_message_store_client")
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
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()
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
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from mongodb_store.message_store import MessageStoreProxy
from geometry_msgs.msg import Pose, Point, Quaternion
if __name__ == '__main__':
rospy.init_node("mongo_pose_write")
msg_store = MessageStoreProxy(database="srs", collection="pose1")
p = Pose(Point(0, 1, 2), Quaternion(0, 0, 0, 1))
try:
p_id = msg_store.insert_named("named_pose", p)
p_id = msg_store.insert(p)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
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)
import StringIO
if __name__ == '__main__':
rospy.init_node("example_message_store_client")
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
class InitialViewEvaluationCore():
def __init__(self):
rospy.init_node('initial_surface_view_evaluation_actionserver', anonymous = False)
rospy.loginfo("VIEW EVAL: waiting for services")
self.conv_octomap = rospy.ServiceProxy('/surface_based_object_learning/convert_pcd_to_octomap',ConvertCloudToOctomap)
self.get_normals = rospy.ServiceProxy('/surface_based_object_learning/extract_normals_from_octomap',ExtractNormalsFromOctomap)
self.get_obs = rospy.ServiceProxy('/semantic_map_publisher/SemanticMapPublisher/ObservationService',ObservationService)
self.roi_srv = rospy.ServiceProxy('/check_point_set_in_soma_roi',PointSetInROI)
self.closest_roi_srv = rospy.ServiceProxy('/get_closest_roi_to_robot',GetROIClosestToRobot)
#self.overlap_srv = rospy.ServiceProxy('/surface_based_object_learning/calculate_octree_overlap',CalculateOctreeOverlap)
rospy.loginfo("VIEW EVAL: done")
self.pc_topic = "/head_xtion/depth_registered/points"
#self.pc_topic = "/head"
self.rgb_topic = "/head_xtion/rgb/image_color"
self.ptu_gazer_controller = PTUGazeController()
self.marker_publisher = rospy.Publisher("/initial_surface_view_evaluation/centroid", Marker,queue_size=5)
self.min_z_cutoff = 0.7
self.max_z_cutoff = 1.7
self.obs_resolution = 0.03
self.initial_view_store = MessageStoreProxy(database="initial_surface_views", collection="logged_views")
self.segmentation = SegmentationWrapper()
self.transformation_store = TransformListener()
rospy.sleep(2)
self.action_server = actionlib.SimpleActionServer("/surface_based_object_learning/evaluate_surface", EvaluateSurfaceAction,
execute_cb=self.do_task_cb, auto_start = False)
self.action_server.start()
rospy.loginfo("VIEW EVAL: action server set up")
rospy.spin()
def log_task(self,data):
rospy.loginfo("VIEW EVAL: logging initial view")
waypoint,filtered_octomap,normals,segmented_objects_octomap,sweep_imgs = data
lv = LoggedInitialView()
lv.timestamp = int(rospy.Time.now().to_sec())
lv.meta_data = "{}"
lv.id = str(uuid.uuid4())
lv.waypoint = waypoint
#lv.metaroom_filtered_cloud = filtered_cloud
lv.metaroom_filtered_octomap = filtered_octomap
lv.up_facing_points = normals
lv.segmented_objects_octomap = segmented_objects_octomap
#lv.sweep_clouds = sweep_clouds
lv.sweep_imgs = sweep_imgs
self.initial_view_store.insert_named(lv.id,lv)
rospy.loginfo("VIEW EVAL: done")
def do_task_cb(self,action):
octo = self.do_task(action.waypoint_id)
re = initial_surface_view_evaluation.msg.EvaluateSurfaceResult(octomap=octo)
self.action_server.set_succeeded(re)
def do_task(self,waypoint):
try:
rospy.loginfo("VIEW EVAL: -- Executing initial view evaluation task at waypoint: " + waypoint)
obs = self.get_filtered_obs_from_wp(waypoint)
octo_obs = self.convert_cloud_to_octomap([obs])
normals = self.get_normals_from_octomap(octo_obs)
rospy.loginfo("VIEW EVAL: got: " + str(len(normals)) + " up-facing normal points")
sx = 0
sy = 0
sz = 0
for k in normals:
sx+=k.x
sy+=k.y
sz+=k.z
sx/=len(normals)
sy/=len(normals)
sz/=len(normals)
centroid = [sx,sy,sz]
print("centroid: " + str(centroid))
centroid_marker = Marker()
centroid_marker.header.frame_id = "/map"
centroid_marker.type = Marker.SPHERE
centroid_marker.header.stamp = rospy.Time.now()
centroid_marker.pose.position.x = sx
centroid_marker.pose.position.y = sy
centroid_marker.pose.position.z = sz
centroid_marker.scale.x = 0.1
centroid_marker.scale.y = 0.1
centroid_marker.scale.z = 0.1
centroid_marker.color.a = 1.0
centroid_marker.color.r = 1.0
centroid_marker.color.g = 0.0
centroid_marker.color.b = 0.0
self.marker_publisher.publish(centroid_marker)
fp = []
for p in normals:
fp.append([p.x,p.y,p.z,255])
n_cloud = pc2.create_cloud(obs.header, obs.fields, fp)
#python_pcd.write_pcd("nrmls.pcd",n_cloud,overwrite=True)
pt_s = PointStamped()
pt_s.header.frame_id = "/map"
# behind robot
pt_s.point.x = sx
pt_s.point.y = sy
pt_s.point.z = sz
segmented_clouds,sweep_clouds,sweep_imgs = self.do_view_sweep_from_point(pt_s)
roi_filtered_objects = self.get_filtered_roi_cloud(segmented_clouds)
object_octomap = self.convert_cloud_to_octomap([roi_filtered_objects])
#object_octomap.header = "/map"
# waypoint,filtered_cloud,filtered_octomap,normals,segmented_objects_octomap,sweep_clouds,sweep_imgs
self.log_task([waypoint,octo_obs,n_cloud,object_octomap,sweep_imgs])
return object_octomap
except Exception,e:
rospy.logerr(e)
rospy.sleep(1)
self.ptu_gazer_controller.reset_gaze()
rospy.sleep(1)
rospy.logerr("PTU has been reset")
import sys, os
from std_msgs.msg import String, Header, Int32
from mongodb_store.message_store import MessageStoreProxy
# from soma2_msgs.msg import SOMA2Object
if __name__ == "__main__":
rospy.init_node('query_soma')
# connecting
soma_store = MessageStoreProxy(database="message_store", collection="soma_activity_ids_list")
if len(sys.argv) > 1:
for cnt, arg in enumerate(sys.argv):
if cnt ==0: continue
# print arg
print 'Add an object ID to a msg store list: %s ' %arg
new_obj_id = Int32(int(arg))
# putting something in
soma_store.insert_named("object id %s" %arg, new_obj_id)
# # getting it back out
# id,meta = soma_store.query_named("object id %s" %arg, Int32._type)
# print scene, meta
else:
print "Requires a list of SOMA2 object IDs to add to a database"
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))