def remove_node(self, node_name) :
rospy.loginfo('Removing Node: '+node_name)
msg_store = MessageStoreProxy(collection='topological_maps')
query = {"name" : node_name, "pointset": self.name}
query_meta = {}
query_meta["pointset"] = self.name
query_meta["map"] = self.map
available = msg_store.query(TopologicalNode._type, query, query_meta)
node_found = False
if len(available) == 1 :
node_found = True
rm_id = str(available[0][1]['_id'])
print rm_id
else :
rospy.logerr("Node not found "+str(len(available))+" waypoints found after query")
#rospy.logerr("Available data: "+str(available))
if node_found :
query_meta = {}
query_meta["pointset"] = self.name
edges_to_rm = []
message_list = msg_store.query(TopologicalNode._type, {}, query_meta)
for i in message_list:
for j in i[0].edges :
if j.node == node_name :
edge_rm = i[0].name+'_'+node_name
edges_to_rm.append(edge_rm)
for k in edges_to_rm :
print 'remove: '+k
self.remove_edge(k)
msg_store.delete(rm_id)
def clear_blocks_from_scene_db():
# clear stored pose informations in scene database (mongo db)
rospy.loginfo("Clearing blocks from scene database")
msg_store = MessageStoreProxy()
res = msg_store.query(Block._type)
for (block, meta) in res:
#print meta
block_id = meta['_id']
msg_store.delete(str(block_id))
def clear_locations_from_scene_db():
# clear stored location informations in scene database (mongo db)
rospy.loginfo("Clearing locations from scene database")
msg_store = MessageStoreProxy()
res = msg_store.query(Pose._type)
for (pose, meta) in res:
#print meta
location_id = meta['_id']
msg_store.delete(str(location_id))
def delete_map(self) :
rospy.loginfo('Deleting map: '+self.name)
msg_store = MessageStoreProxy(collection='topological_maps')
query_meta = {}
query_meta["pointset"] = self.name
message_list = msg_store.query(TopologicalNode._type, {}, query_meta)
for i in message_list:
rm_id = str(i[1]['_id'])
msg_store.delete(rm_id)
def delete_map(self):
rospy.loginfo('Deleting map: ' + self.name)
msg_store = MessageStoreProxy(collection='topological_maps')
query_meta = {}
query_meta["pointset"] = self.name
message_list = msg_store.query(TopologicalNode._type, {}, query_meta)
for i in message_list:
rm_id = str(i[1]['_id'])
msg_store.delete(rm_id)
def update_maps(to_update, client):
db=client.message_store
collection=db["topological_maps"]
# available = collection.find().distinct("_meta.pointset")
msg_store = MessageStoreProxy(collection='topological_maps')
del_ids = []
for pointset in to_update:
#pointsetb='%s_b'%pointset
#print pointsetb
search = {"_meta.pointset": pointset}
av =collection.find(search)
#lnodes=[]
eids=[] #list of known edge id's
for a in av:
#print a
bc = update_node(a, pointset)
nna = a['name']
nma = a['map']
vt = a['verts']
if a['edges']:
es = a['edges']
for i in es:
ed, eids = update_edge(i, nna, nma, eids)
bc.edges.append(ed)
for i in vt:
v = update_vert(i)
bc.verts.append(v)
meta = update_meta(a['_meta'], pointset)
#print bc
#print meta
del_ids.append(a['_id'])
#lnodes.append(bc)
msg_store.insert(bc,meta)
#print lnodes
print "Deleting updated nodes"
for i in del_ids:
msg_store.delete(str(i))
print "done"
def update_maps(to_update, client):
db = client.message_store
collection = db["topological_maps"]
# available = collection.find().distinct("_meta.pointset")
msg_store = MessageStoreProxy(collection='topological_maps')
del_ids = []
for pointset in to_update:
#pointsetb='%s_b'%pointset
#print pointsetb
search = {"_meta.pointset": pointset}
av = collection.find(search)
#lnodes=[]
eids = [] #list of known edge id's
for a in av:
#print a
bc = update_node(a, pointset)
nna = a['name']
nma = a['map']
vt = a['verts']
if a['edges']:
es = a['edges']
for i in es:
ed, eids = update_edge(i, nna, nma, eids)
bc.edges.append(ed)
for i in vt:
v = update_vert(i)
bc.verts.append(v)
meta = update_meta(a['_meta'], pointset)
#print bc
#print meta
del_ids.append(a['_id'])
#lnodes.append(bc)
msg_store.insert(bc, meta)
#print lnodes
print "Deleting updated nodes"
for i in del_ids:
msg_store.delete(str(i))
print "done"
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 remove_node(self, node_name):
rospy.loginfo('Removing Node: ' + node_name)
msg_store = MessageStoreProxy(collection='topological_maps')
query = {"name": node_name, "pointset": self.nodes.name}
query_meta = {}
query_meta["pointset"] = self.nodes.name
query_meta["map"] = self.nodes.map
available = msg_store.query(TopologicalNode._type, query, query_meta)
node_found = False
if len(available) == 1:
node_found = True
rm_id = str(available[0][1]['_id'])
print rm_id
else:
rospy.logerr("Node not found " + str(len(available)) +
" waypoints found after query")
#rospy.logerr("Available data: "+str(available))
if node_found:
query_meta = {}
query_meta["pointset"] = self.nodes.name
edges_to_rm = []
message_list = msg_store.query(TopologicalNode._type, {},
query_meta)
for i in message_list:
for j in i[0].edges:
if j.node == node_name:
edge_rm = j.edge_id
edges_to_rm.append(edge_rm)
for k in edges_to_rm:
print 'remove: ' + k
self.remove_edge(k)
msg_store.delete(rm_id)
return True
else:
return False
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 SOMAROIManager():
def __init__(self, soma_map, soma_conf, config_file=None):
self.soma_map = soma_map
self.soma_conf = soma_conf
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_roi_manager') + '/config/'
filename = 'default.json'
self._config_file=path+filename
self._soma_obj_ids = dict()
self._soma_obj_msg = dict()
self._soma_obj_roi_ids = dict()
self._soma_obj_roi = dict()
self._soma_obj_type = dict()
self._soma_obj_pose = dict()
self._interactive = True
self._msg_store=MessageStoreProxy(collection="soma_roi")
self._gs_store=GeoSpatialStoreProxy(db="geospatial_store", collection="soma")
self._server = InteractiveMarkerServer("soma_roi")
self._init_types()
self._init_menu()
self.load_objects()
rospy.spin()
def _init_types(self):
# read from config in soma_objects
with open(self._config_file) as config_file:
config = json.load(config_file)
self.mesh = dict()
for k, v in config['roi'].iteritems():
self.mesh[k] = v
def _init_menu(self):
self.menu_handler = MenuHandler()
add_point_entry = self.menu_handler.insert( "Add Point", callback=self._add_point_cb)
del_point_entry = self.menu_handler.insert( "Delete Point", callback=self._del_point_cb)
add_entry = self.menu_handler.insert( "Add ROI" )
self.menu_item = dict()
for k in sorted(self.mesh.keys()):
entry = self.menu_handler.insert(k, parent=add_entry, callback=self._add_cb)
self.menu_item[entry] = k
del_entry = self.menu_handler.insert( "Delete ROI", callback=self._del_cb)
enable_entry = self.menu_handler.insert( "Movement control", callback=self._enable_cb )
self.menu_handler.setCheckState( enable_entry, MenuHandler.CHECKED )
def _add_cb(self, feedback):
rospy.loginfo("Add ROI: %s", self.menu_item[feedback.menu_entry_id])
self.add_object(self.menu_item[feedback.menu_entry_id], feedback.pose)
def _del_cb(self, feedback):
rospy.loginfo("Delete ROI: %s", feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
for r in self._soma_obj_roi_ids[roi]:
self.delete_object(r)
self.undraw_roi(roi)
def _add_point_cb(self, feedback):
rospy.loginfo("Add point: %s", feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
t = self._soma_obj_type[feedback.marker_name]
self.add_object(t, feedback.pose, roi)
#self.draw_roi(roi)
def _del_point_cb(self, feedback):
rospy.loginfo("Delete point: %s", feedback.marker_name)
self.delete_object(feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
self.draw_roi(roi)
def _update_poly(self, feedback):
return
def _update_cb(self, feedback):
p = feedback.pose.position
#print "Marker " + feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y)
self._soma_obj_pose[feedback.marker_name] = feedback.pose
roi = self._soma_obj_roi[feedback.marker_name]
self.draw_roi(roi)
if hasattr(self, "vp_timer_"+feedback.marker_name):
getattr(self, "vp_timer_"+feedback.marker_name).cancel()
setattr(self, "vp_timer_"+feedback.marker_name,
Timer(3, self.update_object, [feedback]))
getattr(self, "vp_timer_"+feedback.marker_name).start()
def _enable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
if state == MenuHandler.CHECKED:
self.menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
self._interactive = False
else:
self.menu_handler.setCheckState( handle, MenuHandler.CHECKED )
self._interactive = True
self.menu_handler.reApply( self._server )
self.load_objects()
self._server.applyChanges()
def _next_id(self):
self._soma_id += 1
return self._soma_id
def _next_roi_id(self):
self._soma_roi_id += 1
return self._soma_roi_id
def _retrieve_objects(self):
objs = self._msg_store.query(SOMAROIObject._type, message_query={"map": self.soma_map,
"config": self.soma_conf})
max_id = 0
max_roi_id = 0
for o,om in objs:
if int(o.id) > max_id:
max_id = int(o.id)
if int(o.roi_id) > max_roi_id:
max_roi_id = int(o.roi_id)
self._soma_id = max_id
self._soma_roi_id = max_roi_id
return objs
def load_objects(self):
self._soma_obj_roi_ids = dict()
objs = self._retrieve_objects()
# if collection is empty insert initial object
if not objs:
pose = Pose()
self.add_object('Office', pose)
return
# otherwise, load all object from collection
for o, om in objs:
self._soma_obj_ids[o.id] = om['_id']
self._soma_obj_msg[o.id] = o
if o.roi_id in self._soma_obj_roi_ids:
self._soma_obj_roi_ids[o.roi_id].append(o.id)
else:
self._soma_obj_roi_ids[o.roi_id] = list()
self._soma_obj_roi_ids[o.roi_id].append(o.id)
self._soma_obj_roi[o.id] = o.roi_id
self._soma_obj_type[o.id] = o.type
self._soma_obj_pose[o.id] = o.pose
self.load_object(o.id, o.roi_id, o.type, o.pose)
self.draw_all_roi()
def draw_all_roi(self):
for key in self._soma_obj_roi_ids:
self.draw_roi(key)
def undraw_all_roi(self):
for key in self._soma_obj_roi_ids:
self.undraw_roi(key)
def draw_roi(self, roi):
v = self._soma_obj_roi_ids[roi]
t = self._soma_obj_type[v[0]]
p = self._soma_obj_pose[v[0]]
int_marker = self.create_roi_marker(roi, t, p, v)
self._server.erase("ROI-" + roi)
self._server.applyChanges()
self._server.insert(int_marker, self._update_poly)
self._server.applyChanges()
def undraw_roi(self, roi):
self._server.erase("ROI-" + roi)
self._server.applyChanges()
def load_object(self, soma_id, roi, soma_type, pose):
int_marker = self.create_object_marker(soma_id, roi, soma_type, pose)
self._server.insert(int_marker, self._update_cb)
self.menu_handler.apply( self._server, soma_id )
self._server.applyChanges()
def add_object(self, soma_type, pose, roi_id=None):
# todo: add to mongodb
soma_id = self._next_id()
if roi_id == None:
soma_roi_id = self._next_roi_id()
self._soma_obj_roi_ids[str(soma_roi_id)] = list()
else:
soma_roi_id = roi_id
soma_obj = SOMAROIObject()
soma_obj.id = str(soma_id)
soma_obj.roi_id = str(soma_roi_id)
soma_obj.map = str(self.soma_map)
soma_obj.config = str(self.soma_conf)
soma_obj.type = soma_type
soma_obj.pose = pose
soma_obj.frame = 'map'
_id = self._msg_store.insert(soma_obj)
self._soma_obj_ids[soma_obj.id] = _id
self._soma_obj_msg[soma_obj.id] = soma_obj
self._soma_obj_roi_ids[soma_obj.roi_id].append(soma_obj.id)
self._soma_obj_roi[soma_obj.id] = soma_obj.roi_id
self._soma_obj_type[soma_obj.id] = soma_type
self._soma_obj_pose[soma_obj.id] = pose
self.load_object(str(soma_id), soma_obj.roi_id, soma_type, pose)
# geospatial store
# delete old message
res = self._gs_store.find_one({'soma_roi_id': soma_roi_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(soma_obj)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" % (soma_obj.type, soma_obj.roi_id) )
except:
rospy.logerr("The polygon of %s %s is malformed (self-intersecting) => Please update geometry." % (soma_type, soma_roi_id))
def geo_json_from_soma_obj(self, soma_obj):
geo_json = {}
geo_json['soma_roi_id'] = soma_obj.roi_id
geo_json['soma_map'] = soma_obj.map
geo_json['soma_config'] = soma_obj.config
geo_json['type'] = soma_obj.type
if len(self._soma_obj_roi_ids[soma_obj.roi_id]) < 3:
rospy.logerr("GS Store: %s %s, less then 3 points => Add more points or delete ROI." % (soma_obj.type, soma_obj.roi_id) )
return None
coordinates = []
for obj_id in self._soma_obj_roi_ids[soma_obj.roi_id]:
p = self._soma_obj_pose[obj_id]
coordinates.append(self._gs_store.coords_to_lnglat(p.position.x, p.position.y))
p = self._soma_obj_pose[self._soma_obj_roi_ids[soma_obj.roi_id][0]]
coordinates.append(self._gs_store.coords_to_lnglat(p.position.x, p.position.y))
geo_json['loc'] = {'type': 'Polygon',
'coordinates': [coordinates]}
return geo_json
def delete_object(self, soma_id):
# todo: delete from mongodb
_id = self._soma_obj_ids[str(soma_id)]
self._msg_store.delete(str(_id))
self._server.erase(soma_id)
self._server.applyChanges()
roi = self._soma_obj_roi[str(soma_id)]
nodes = self._soma_obj_roi_ids[roi]
new_nodes = []
for n in nodes:
if n != str(soma_id):
new_nodes.append(n)
self._soma_obj_roi_ids[roi] = new_nodes
# geospatial store
# delete old message
old_msg = self._soma_obj_msg[soma_id]
res = self._gs_store.find_one({'soma_roi_id': roi,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(old_msg)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" % (old_msg.type, old_msg.roi_id) )
except:
rospy.logerr("The polygon of %s %s is malformed (self-intersecting) => Please update geometry." % (old_msg.type, old_msg.roi_id))
def update_object(self, feedback):
rospy.loginfo("Updated marker: %s", feedback.marker_name)
_id = self._soma_obj_ids[feedback.marker_name]
msg = self._soma_obj_msg[feedback.marker_name]
new_msg = copy.deepcopy(msg)
new_msg.pose = feedback.pose
self._soma_obj_pose[feedback.marker_name] = feedback.pose
self._msg_store.update_id(_id, new_msg)
# geospatial store
# delete old message
res = self._gs_store.find_one({'soma_roi_id': new_msg.roi_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(new_msg)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" % (new_msg.type, new_msg.roi_id) )
except:
rospy.logerr("The polygon of %s %s is malformed (self-intersecting) => Please update geometry." % (new_msg.type, new_msg.roi_id))
def create_object_marker(self, soma_obj, roi, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = soma_obj
int_marker.description = soma_type + ' (' + roi + ')'
int_marker.pose = pose
int_marker.pose.position.z = 0.01
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
int_marker.pose.position.z = (marker.scale.z / 2)
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
#marker.pose = pose
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
if self._interactive:
int_marker.controls.append(copy.deepcopy(control))
# add the control to the interactive marker
int_marker.controls.append(control);
# add menu control
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.markers.append( marker) #makeBox(int_marker) )
int_marker.controls.append(menu_control)
return int_marker
def create_roi_marker(self, roi, soma_type, pose, points):
#print "POINTS: " + str(points)
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = "ROI-" + roi
int_marker.description = roi
int_marker.pose = pose
marker = Marker()
marker.type = Marker.LINE_STRIP
marker.scale.x = 0.1
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append( marker )
int_marker.controls.append(control )
marker.points = []
for point in points:
p = Point()
pose = self._soma_obj_pose[point]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
p = Point()
pose = self._soma_obj_pose[points[0]]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
return int_marker
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 SOMAManager():
def __init__(self, soma_map, soma_conf, config_file=None):
self.soma_map = soma_map
self.soma_conf = soma_conf
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_objects') + '/config/'
filename = 'default.json'
self._config_file=path+filename
self._soma_obj_ids = dict()
self._soma_obj_msg = dict()
self._interactive = True
self._msg_store=MessageStoreProxy(collection="soma")
self._gs_store=GeoSpatialStoreProxy(db="geospatial_store", collection="soma")
self._server = InteractiveMarkerServer("soma")
self._init_types()
self._init_menu()
self.load_objects()
rospy.spin()
def _init_types(self):
# read from config in soma_objects
with open(self._config_file) as config_file:
config = json.load(config_file)
self.mesh = dict()
self.marker = dict()
if '2D' in config:
for k, v in config['2D'].iteritems():
self.mesh[k] = v
self.marker[k] = '2D'
if '3D' in config:
for k, v in config['3D'].iteritems():
self.mesh[k] = v
self.marker[k] = '3D'
def _init_menu(self):
self.menu_handler = MenuHandler()
add_entry = self.menu_handler.insert( "Add object" )
self.menu_item = dict()
for k in sorted(self.mesh.keys()):
entry = self.menu_handler.insert(k, parent=add_entry, callback=self._add_cb)
self.menu_item[entry] = k
del_entry = self.menu_handler.insert( "Delete object", callback=self._del_cb)
enable_entry = self.menu_handler.insert( "Movement control", callback=self._enable_cb )
self.menu_handler.setCheckState( enable_entry, MenuHandler.CHECKED )
def _add_cb(self, feedback):
rospy.loginfo("Add marker: %s", self.menu_item[feedback.menu_entry_id])
self.add_object(self.menu_item[feedback.menu_entry_id], feedback.pose)
def _del_cb(self, feedback):
rospy.loginfo("Delete marker: %s", feedback.marker_name)
self.delete_object(feedback.marker_name)
def _update_cb(self, feedback):
p = feedback.pose.position
print "Marker " + feedback.marker_name + " position: " + str(round(p.x,2)) + ", " + str(round(p.y,2)) + ", " + str(round(p.z,2))
if hasattr(self, "vp_timer_"+feedback.marker_name):
getattr(self, "vp_timer_"+feedback.marker_name).cancel()
setattr(self, "vp_timer_"+feedback.marker_name,
Timer(3, self.update_object, [feedback]))
getattr(self, "vp_timer_"+feedback.marker_name).start()
def _enable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
if state == MenuHandler.CHECKED:
self.menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
self._interactive = False
else:
self.menu_handler.setCheckState( handle, MenuHandler.CHECKED )
self._interactive = True
self.menu_handler.reApply( self._server )
self.load_objects()
self._server.applyChanges()
def _next_id(self):
self._soma_id += 1
return self._soma_id
def _retrieve_objects(self):
objs = self._msg_store.query(SOMAObject._type, message_query={"map": self.soma_map,
"config": self.soma_conf})
max_id = 0
for o,om in objs:
if int(o.id) > max_id:
max_id = int(o.id)
self._soma_id = max_id
return objs
def load_objects(self):
objs = self._retrieve_objects()
# if collection is empty insert initial object
if not objs:
pose = Pose()
self.add_object('Table', pose)
return
# otherwise, load all object from collection
for o, om in objs:
self._soma_obj_ids[o.id] = om['_id']
self._soma_obj_msg[o.id] = o
self.load_object(o.id, o.type, o.pose)
def load_object(self, soma_id, soma_type, pose):
int_marker = self.create_object_marker(soma_id, soma_type, pose)
self._server.insert(int_marker, self._update_cb)
self.menu_handler.apply( self._server, soma_id )
self._server.applyChanges()
def add_object(self, soma_type, pose):
# todo: add to mongodb
soma_id = self._next_id()
soma_obj = SOMAObject()
soma_obj.id = str(soma_id)
soma_obj.map = str(self.soma_map)
soma_obj.config = str(self.soma_conf)
soma_obj.type = soma_type
soma_obj.pose = pose
soma_obj.pose.position.z = 0.0
soma_obj.frame = 'map'
soma_obj.mesh = self.mesh[soma_type]
_id = self._msg_store.insert(soma_obj)
self._soma_obj_ids[soma_obj.id] = _id
self._soma_obj_msg[soma_obj.id] = soma_obj
# add object to geospatial store
self._gs_store.insert(self.geo_json_from_soma_obj(soma_obj))
print "GS Store: added obj"
self.load_object(str(soma_id), soma_type, soma_obj.pose)
def geo_json_from_soma_obj(self, soma_obj):
geo_json = {}
geo_json['soma_id'] = soma_obj.id
geo_json['soma_map'] = soma_obj.map
geo_json['soma_config'] = soma_obj.config
geo_json['type'] = soma_obj.type
geo_json['loc'] = {'type': 'Point',
'coordinates': self._gs_store.coords_to_lnglat(soma_obj.pose.position.x,
soma_obj.pose.position.y)}
return geo_json
def delete_object(self, soma_id):
# geospatial store
res = self._gs_store.find_one({'soma_id': soma_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
print "GS Store: deleted obj"
# message store
_id = self._soma_obj_ids[str(soma_id)]
self._msg_store.delete(str(_id))
self._server.erase(soma_id)
self._server.applyChanges()
def update_object(self, feedback):
print "Updated marker " + feedback.marker_name
_id = self._soma_obj_ids[feedback.marker_name]
msg = self._soma_obj_msg[feedback.marker_name]
new_msg = copy.deepcopy(msg)
new_msg.pose = feedback.pose
self._msg_store.update_id(_id, new_msg)
# geospatial store
# delete old message
res = self._gs_store.find_one({'soma_id': new_msg.id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
print "GS Store: deleted obj"
# add new object to geospatial store
self._gs_store.insert(self.geo_json_from_soma_obj(new_msg))
print "GS Store: added obj"
def create_object_marker(self, soma_obj, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = soma_obj
int_marker.description = "id" + soma_obj
int_marker.pose = pose
int_marker.pose.position.z = 0.01
mesh_marker = Marker()
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.scale.x = 1
mesh_marker.scale.y = 1
mesh_marker.scale.z = 1
random.seed(soma_type)
val = random.random()
mesh_marker.color.r = r_func(val)
mesh_marker.color.g = g_func(val)
mesh_marker.color.b = b_func(val)
mesh_marker.color.a = 1.0
#mesh_marker.pose = pose
mesh_marker.mesh_resource = self.mesh[soma_type]
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
if self._interactive:
int_marker.controls.append(copy.deepcopy(control))
# add the control to the interactive marker
if self.marker[soma_type] == '3D':
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
# add menu control
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.markers.append( mesh_marker) #makeBox(int_marker) )
int_marker.controls.append(menu_control)
return int_marker
class skeleton_cpm():
"""docstring for cpm"""
def __init__(self, cam, rem, pub, sav):
# read camera calib
#print '>>>>>>>>>',cam
self.camera_calib = util.read_yaml_calib(cam)
# remove rgb images
self.rgb_remove = rem
if self.rgb_remove:
rospy.loginfo("remove rgb images from the dataset.")
# save cpm images
self.save_cpm_img = sav
if self.save_cpm_img:
rospy.loginfo("save cpm images.")
# initialize published
self.pub = pub
if self.pub:
rospy.loginfo("publish cpm images")
self.image_pub = rospy.Publisher("/cpm_skeleton_image",
Image,
queue_size=1)
self.bridge = CvBridge()
else:
rospy.loginfo("don't publish cpm images")
# mongo stuff
self.msg_store = MessageStoreProxy(database='message_store',
collection='cpm_stats')
self.msg_store_learning = MessageStoreProxy(
database='message_store', collection='activity_learning')
# open dataset folder
self.directory = '/home/' + getpass.getuser(
) + '/SkeletonDataset/no_consent/'
if not os.path.isdir(self.directory):
rospy.loginfo(
self.directory +
" does not exist. Please make sure there is a dataset on this pc"
)
sys.exit(1)
self.dates = sorted(os.listdir(self.directory))
#self.delete_empty_dates()
self.get_dates_to_process()
self.empty_date = 0 # just a flag for empty date folder
if not self.read_mongo_success:
self.folder = 0
self.userid = 0
self._read_files()
# cpm init stuff
self.rospack = rospkg.RosPack()
self.cpm_path = self.rospack.get_path('cpm_skeleton')
self.conf = 1 # using config file 1, for the full body detector
self.param, self.model = config_reader(self.conf)
self.boxsize = self.model['boxsize']
self.npart = self.model['np']
self.limbs_names = [
'head', 'neck', 'right_shoulder', 'right_elbow', 'right_hand',
'left_shoulder', 'left_elbow', 'left_hand', 'right_hip',
'right_knee', 'right_foot', 'left_hip', 'left_knee', 'left_foot'
]
self.colors = [[0, 0, 255], [0, 170, 255], [0, 255, 170], [0, 255, 0],
[170, 255, 0], [255, 170, 0], [255, 0, 0],
[255, 0, 170], [170, 0, 255]] # note BGR ...
self.stickwidth = 6
self.dist_threshold = 1.5 # less than 1.5 meters ignore the skeleton
self.depth_thresh = .35 # any more different in depth than this with openni, use openni
self.finished_processing = 0 # a flag to indicate that we finished processing allavailable data
self.threshold = 10 # remove any folder <= 10 detections
self.cpm_stats_file = '/home/' + getpass.getuser(
) + '/SkeletonDataset/cpm_stats.txt'
# action server
self._as = actionlib.SimpleActionServer("cpm_action", cpmAction, \
execute_cb=self.execute_cb, auto_start=False)
self._as.start()
def _initiliase_cpm(self):
sys.stdout = open(os.devnull, "w")
if self.param['use_gpu']:
caffe.set_mode_gpu()
else:
caffe.set_mode_cpu()
caffe.set_device(self.param['GPUdeviceNumber']) # set to your device!
self.person_net = caffe.Net(self.model['deployFile_person'],
self.model['caffemodel_person'],
caffe.TEST)
self.pose_net = caffe.Net(self.model['deployFile'],
self.model['caffemodel'], caffe.TEST)
sys.stdout = sys.__stdout__
def _cpm_stats(self, start, duration, stop_flag_pre, stop_flag_dur):
f1 = open(self.cpm_stats_file, 'a')
f1.write('date=' + start.split(' ')[0])
f1.write(', start=' + start.split(' ')[1])
f1.write(', end=' + time.strftime("%H:%M:%S"))
f1.write(', duration=' + str(duration))
f1.write(', processed=' + str(self.processed))
f1.write(', removed=' + str(self.removed))
f1.write(', images=' + str(self.img_processed))
if self.finished_processing:
f1.write(', stopped=finisehd all data\n')
elif stop_flag_pre:
f1.write(', stopped=preempted\n')
elif stop_flag_dur:
f1.write(', stopped=duration\n')
def execute_cb(self, goal):
#print '>>-',self.empty_date
self.processed = 0
self.removed = 0
self.img_processed = 0 # stats counter
self._initiliase_cpm()
stats_start = time.strftime("%d-%b-%Y %H:%M:%S")
start = rospy.Time.now()
end = rospy.Time.now()
stop_flag_pre = 0
stop_flag_dur = 0 # stop flags preempt and duration
duration = goal.duration.secs
while not self.finished_processing and not stop_flag_pre and not stop_flag_dur:
#print self.empty_date
if self.empty_date or self.rgb_files == []:
rospy.loginfo("found an empty date folder")
self.next()
else:
self.person_found_flag = 0
for rgb, depth, skl in zip(self.rgb_files, self.dpt_files,
self.skl_files):
if self._as.is_preempt_requested():
stop_flag_pre = 1
break
if (end - start).secs > duration:
stop_flag_dur = 1
break
self._process_images(rgb, depth, skl)
end = rospy.Time.now()
self.img_processed += 1 # counts the number of processed images
if not stop_flag_pre and not stop_flag_dur:
self.processed += 1 # stats counter
if self.person_found_flag > self.threshold:
self.update_last_learning()
self.update_last_cpm_date()
if self.rgb_remove:
self._remove_rgb_images(
) # remove rgb images from directory
self.next() # stats counter
else:
rospy.loginfo(
'nothing interesting was detected, I will delete this folder!'
)
self.delete_last_learning()
self.remove_uuid_folder()
self.removed += 1
self.next()
# after the action reset everything
self._cpm_stats(stats_start, duration, stop_flag_pre, stop_flag_dur)
self._as.set_succeeded(cpmActionResult())
def _remove_rgb_images(self):
rospy.loginfo('removing: ' + self.directory + self.dates[self.folder] +
'/' + self.files[self.userid] + '/rgb')
shutil.rmtree(self.directory + self.dates[self.folder] + '/' +
self.files[self.userid] + '/rgb')
def remove_uuid_folder(self):
#print len(self.files)
rospy.loginfo('removing: ' + self.directory + self.dates[self.folder] +
'/' + self.files[self.userid])
shutil.rmtree(self.directory + self.dates[self.folder] + '/' +
self.files[self.userid])
self.files = sorted(
os.listdir(self.directory + self.dates[self.folder]))
self.userid -= 1
#print len(self.files)
#sys.exit(1)
def delete_last_learning(self):
uuid = self.files[self.userid].split('_')[-1]
query = {"uuid": uuid}
result = self.msg_store_learning.query(type=HumanActivities._type,
message_query=query)
rospy.loginfo("I removed id from mongodb: " + uuid)
for (ret, meta) in result:
self.msg_store_learning.delete(message_id=str(meta['_id']))
def update_last_cpm_date(self):
msg = cpm_pointer()
msg.type = "cpm_skeleton"
msg.date_ran = time.strftime("%Y-%m-%d")
msg.last_date_used = self.dates[self.folder]
msg.uuid = self.files[self.userid]
print "adding %s to cpm stats store" % msg.uuid
query = {"type": msg.type}
self.msg_store.update(message=msg, message_query=query, upsert=True)
def update_last_learning(self):
uuid = self.files[self.userid].split('_')[-1]
query = {"uuid": uuid}
result = self.msg_store_learning.query(type=HumanActivities._type,
message_query=query)
for cnt, (msg, meta) in enumerate(result):
msg.cpm = True
print "updating %s to activity learning store" % msg.uuid
self.msg_store_learning.update(message=msg,
message_query=query,
upsert=True)
if len(result) == 0:
msg = HumanActivities()
msg.date = self.dates[self.folder]
msg.uuid = self.files[self.userid].split('_')[-1]
msg.time = self.files[self.userid].split('_')[-2]
msg.cpm = True
self.msg_store_learning.insert(message=msg)
print "adding %s to activity learning store" % msg.uuid
#def update_last_learning(self):
# msg = HumanActivities()
# msg.date = self.dates[self.folder]
# msg.uuid = self.files[self.userid].split('_')[-1]
# msg.time = self.files[self.userid].split('_')[-2]
# msg.cpm = True
# print "adding %s to activity learning store" % msg.uuid
# query = {"uuid" : msg.uuid}
# self.msg_store_learning.update(message=msg, message_query=query, upsert=True)
def get_dates_to_process(self):
""" Find the sequence of date folders (on disc) which have not been processed into QSRs.
ret: self.not_processed_dates - List of date folders to use
"""
self.read_mongo_success = 0
for (ret, meta) in self.msg_store.query(cpm_pointer._type):
if ret.type != "cpm_skeleton": continue
self.read_mongo_success = 1
self.folder = self.dates.index(ret.last_date_used)
self.files = sorted(
os.listdir(self.directory + self.dates[self.folder]))
self.userid = self.files.index(ret.uuid)
rospy.loginfo("cpm progress date: " + ret.last_date_used + "," +
ret.uuid)
self.next()
def _read_files(self):
self.empty_date = 0
self.rgb_files = []
self.files = sorted(
os.listdir(self.directory + self.dates[self.folder]))
if self.files != []: # empty folders
#print ">", self.folder
#print ">>", self.directory+self.dates[0]
#print ">", self.directory, self.folder, self.userid
#print ">>", self.dates, self.files
self.rgb_dir = self.directory + self.dates[
self.folder] + '/' + self.files[self.userid] + '/rgb/'
self.dpt_dir = self.directory + self.dates[
self.folder] + '/' + self.files[self.userid] + '/depth/'
self.skl_dir = self.directory + self.dates[
self.folder] + '/' + self.files[self.userid] + '/skeleton/'
self.cpm_dir = self.directory + self.dates[
self.folder] + '/' + self.files[self.userid] + '/cpm_skeleton/'
self.cpm_img_dir = self.directory + self.dates[
self.folder] + '/' + self.files[self.userid] + '/cpm_images/'
self.rgb_files = sorted(glob.glob(self.rgb_dir + "*.jpg"))
self.dpt_files = sorted(glob.glob(self.dpt_dir + "*.jpg"))
self.skl_files = sorted(glob.glob(self.skl_dir + "*.txt"))
rospy.loginfo('Processing userid: ' + self.files[self.userid])
if not os.path.isdir(self.cpm_dir):
os.mkdir(self.cpm_dir)
if not os.path.isdir(self.cpm_img_dir) and self.save_cpm_img:
os.mkdir(self.cpm_img_dir)
else:
self.empty_date = 1
rospy.loginfo(
"I found an empty date folder, this should not happen.")
#sys.exit(1)
def next(self):
self.userid += 1
if self.userid >= len(self.files):
self.userid = 0
self.folder += 1
if self.folder >= len(self.dates):
rospy.loginfo("cpm finished processing all folders")
self.finished_processing = 1
else:
self._read_files()
#print '>>',self.userid
#print '>>',self.folder
#print '>>> test',self.empty_date
def _process_images(self, test_image, test_depth, test_skl):
# block 1
self.test_skl = test_skl
self.name = test_image.split('.')[0].split('/')[-1]
start_time = time.time()
img = cv.imread(test_image)
depth = cv.imread(test_depth)
self.scale = self.boxsize / (img.shape[0] * 1.0)
imageToTest = cv.resize(img, (0, 0),
fx=self.scale,
fy=self.scale,
interpolation=cv.INTER_CUBIC)
depthToTest = cv.resize(depth, (0, 0),
fx=self.scale,
fy=self.scale,
interpolation=cv.INTER_CUBIC)
imageToTest_padded, pad = util.padRightDownCorner(imageToTest)
# check distance threshold
f1 = open(self.test_skl, 'r')
self.openni_values, self.openni_time = util.get_openni_values(f1)
x = []
y = []
if self.openni_values['torso']['z'] >= self.dist_threshold:
# block 2
self.person_net.blobs['image'].reshape(
*(1, 3, imageToTest_padded.shape[0],
imageToTest_padded.shape[1]))
self.person_net.reshape()
#person_net.forward(); # dry run to avoid GPU synchronization later in caffe
# block 3
self.person_net.blobs['image'].data[...] = np.transpose(
np.float32(imageToTest_padded[:, :, :, np.newaxis]),
(3, 2, 0, 1)) / 256 - 0.5
output_blobs = self.person_net.forward()
person_map = np.squeeze(
self.person_net.blobs[output_blobs.keys()[0]].data)
# block 4
person_map_resized = cv.resize(person_map, (0, 0),
fx=8,
fy=8,
interpolation=cv.INTER_CUBIC)
data_max = scipy.ndimage.filters.maximum_filter(
person_map_resized, 3)
maxima = (person_map_resized == data_max)
diff = (data_max > 0.5)
maxima[diff == 0] = 0
x = np.nonzero(maxima)[1]
y = np.nonzero(maxima)[0]
# get the right person from openni
x, y = util.get_correct_person(self.openni_values, self.scale,
self.camera_calib, x, y)
self.x = x
self.y = y
# block 5
num_people = len(x)
person_image = np.ones((self.model['boxsize'], self.model['boxsize'],
3, num_people)) * 128
for p in range(num_people):
x_max = x[p] + self.model['boxsize'] / 2
x_min = x[p] - self.model['boxsize'] / 2
y_max = y[p] + self.model['boxsize'] / 2
y_min = y[p] - self.model['boxsize'] / 2
if x_min < 0:
xn_min = x_min * -1
x_min = 0
else:
xn_min = 0
if x_max > imageToTest.shape[1]:
xn_max = self.model['boxsize'] - (x_max - imageToTest.shape[1])
x_max = imageToTest.shape[1]
else:
xn_max = self.model['boxsize']
if y_min < 0:
yn_min = y_min * -1
y_min = 0
else:
yn_min = 0
if y_max > imageToTest.shape[0]:
yn_max = self.model['boxsize'] - (y_max - imageToTest.shape[0])
y_max = imageToTest.shape[0]
else:
yn_max = self.model['boxsize']
person_image[yn_min:yn_max, xn_min:xn_max, :,
p] = imageToTest[y_min:y_max, x_min:x_max, :]
# block 6
gaussian_map = np.zeros((self.model['boxsize'], self.model['boxsize']))
x_p = np.arange(self.model['boxsize'])
y_p = np.arange(self.model['boxsize'])
part1 = [(x_p - self.model['boxsize'] / 2) *
(x_p - self.model['boxsize'] / 2),
np.ones(self.model['boxsize'])]
part2 = [
np.ones(self.model['boxsize']), (y_p - self.model['boxsize'] / 2) *
(y_p - self.model['boxsize'] / 2)
]
dist_sq = np.transpose(np.matrix(part1)) * np.matrix(part2)
exponent = dist_sq / 2.0 / self.model['sigma'] / self.model['sigma']
gaussian_map = np.exp(-exponent)
# block 7
#pose_net.forward() # dry run to avoid GPU synchronization later in caffe
output_blobs_array = [dict() for dummy in range(num_people)]
for p in range(num_people):
input_4ch = np.ones(
(self.model['boxsize'], self.model['boxsize'], 4))
input_4ch[:, :, 0:
3] = person_image[:, :, :,
p] / 256.0 - 0.5 # normalize to [-0.5, 0.5]
input_4ch[:, :, 3] = gaussian_map
self.pose_net.blobs['data'].data[...] = np.transpose(
np.float32(input_4ch[:, :, :, np.newaxis]), (3, 2, 0, 1))
if self.conf == 4:
output_blobs_array[p] = copy.deepcopy(
self.pose_net.forward()['Mconv5_stage4'])
else:
output_blobs_array[p] = copy.deepcopy(
self.pose_net.forward()['Mconv7_stage6'])
# block 8
for p in range(num_people):
for part in [
0, 3, 7, 10, 12
]: # sample 5 body parts: [head, right elbow, left wrist, right ankle, left knee]
part_map = output_blobs_array[p][0, part, :, :]
part_map_resized = cv.resize(
part_map, (0, 0), fx=4, fy=4,
interpolation=cv.INTER_CUBIC) #only for displaying
# block 9
prediction = np.zeros((14, 2, num_people))
for p in range(num_people):
for part in range(14):
part_map = output_blobs_array[p][0, part, :, :]
part_map_resized = cv.resize(part_map, (0, 0),
fx=8,
fy=8,
interpolation=cv.INTER_CUBIC)
prediction[part, :,
p] = np.unravel_index(part_map_resized.argmax(),
part_map_resized.shape)
# mapped back on full image
prediction[:, 0,
p] = prediction[:, 0,
p] - (self.model['boxsize'] / 2) + y[p]
prediction[:, 1,
p] = prediction[:, 1,
p] - (self.model['boxsize'] / 2) + x[p]
# block 10
limbs = self.model['limbs']
canvas = imageToTest.copy()
#canvas *= .5 # for transparency
canvas = np.multiply(canvas, 0.2, casting="unsafe")
if num_people:
self.person_found_flag += 1 # this is used to prevent the deletion of the entire folder if noe person is found
self._get_depth_data(prediction, depthToTest)
for p in range(num_people):
cur_canvas = imageToTest.copy(
) #np.zeros(canvas.shape,dtype=np.uint8)
for l in range(limbs.shape[0]):
X = prediction[limbs[l, :] - 1, 0, p]
Y = prediction[limbs[l, :] - 1, 1, p]
mX = np.mean(X)
mY = np.mean(Y)
length = ((X[0] - X[1])**2 + (Y[0] - Y[1])**2)**0.5
angle = math.degrees(math.atan2(X[0] - X[1], Y[0] - Y[1]))
polygon = cv.ellipse2Poly(
(int(mY), int(mX)), (int(length / 2), self.stickwidth),
int(angle), 0, 360, 1)
cv.fillConvexPoly(cur_canvas, polygon, self.colors[l])
cv.fillConvexPoly(depthToTest, polygon, self.colors[l])
cv.circle(cur_canvas, (int(self.x[0]), int(self.y[0])), 3,
(250, 0, 210), -1)
canvas = np.add(canvas,
np.multiply(cur_canvas, 0.8, casting="unsafe"),
casting="unsafe") # for transparency
print 'image ' + self.name + ' processed in: %2.3f' % (
time.time() - start_time), "person found"
else:
print 'image ' + self.name + ' processed in: %2.3f' % (
time.time() - start_time), "person not found"
vis = np.concatenate((canvas.astype(np.uint8), depthToTest), axis=1)
# saving cpm images
if self.save_cpm_img:
cv.imwrite(self.cpm_img_dir + self.name + '.jpg', vis)
# publishing cpm images
if self.pub:
sys.stdout = open(os.devnull, "w")
msg = self.bridge.cv2_to_imgmsg(vis, "bgr8")
sys.stdout = sys.__stdout__
self.image_pub.publish(msg)
#cv.imwrite('/home/strands/SkeletonDataset/cpm_images/cpm_'+self.name+'.jpg',vis)
#### Create CompressedIamge ####
#msg = CompressedImage()
#msg.header.stamp = rospy.Time.now()
#msg.format = "jpeg"
#msg.data = vis # np.array(cv.imencode('.jpg', vis)[1]).tostring()
#### Publish new image
#self.image_pub.publish(msg)
def _get_depth_data(self, prediction, depthToTest):
[fx, fy, cx, cy] = self.camera_calib
cpm_file = self.cpm_dir + 'cpm_' + self.test_skl.split('/')[-1]
f1 = open(cpm_file, 'w')
f1.write(self.openni_time)
for part, jname in enumerate(self.limbs_names):
x2d = np.min([int(prediction[part, 0, 0]), 367])
y2d = np.min([int(prediction[part, 1, 0]), 490])
depth_val = depthToTest[x2d, y2d, 0]
z = (.4) / (20.0) * (depth_val - 60.0) + 2.7
if np.abs(z - self.openni_values[jname]['z']) > self.depth_thresh:
z = self.openni_values[jname]['z']
x = (y2d / self.scale - cx) * z / fx
y = (x2d / self.scale - cy) * z / fy
f1.write(jname + ',' + str(x2d) + ',' + str(y2d) + ',' + str(x) +
',' + str(y) + ',' + str(z) + '\n')
# add the torso position
x2d = np.min([int(self.y[0]), 367])
y2d = np.min([int(self.x[0]), 490])
depth_val = depthToTest[x2d, y2d, 0]
z = (.4) / (20.0) * (depth_val - 60.0) + 2.7
if np.abs(z - self.openni_values[jname]['z']) > self.depth_thresh:
z = self.openni_values[jname]['z']
x = (y2d - cx) * z / fx
y = (x2d - cy) * z / fy
f1.write('torso' + ',' + str(y2d) + ',' + str(x2d) + ',' + str(x) +
',' + str(y) + ',' + str(z) + '\n')
f1.close()
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))
class SOMAROIManager():
def __init__(self, soma_map, soma_conf, config_file=None):
self.soma_map = soma_map
self.soma_conf = soma_conf
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_roi_manager') + '/config/'
filename = 'default.json'
self._config_file = path + filename
self._soma_obj_ids = dict()
self._soma_obj_msg = dict()
self._soma_obj_roi_ids = dict()
self._soma_obj_roi = dict()
self._soma_obj_type = dict()
self._soma_obj_pose = dict()
self._interactive = True
self._msg_store = MessageStoreProxy(collection="soma_roi")
self._gs_store = GeoSpatialStoreProxy(db="geospatial_store",
collection="soma")
self._server = InteractiveMarkerServer("soma_roi")
self._init_types()
self._init_menu()
self.load_objects()
rospy.spin()
def _init_types(self):
# read from config in soma_objects
with open(self._config_file) as config_file:
config = json.load(config_file)
self.mesh = dict()
for k, v in config['roi'].iteritems():
self.mesh[k] = v
def _init_menu(self):
self.menu_handler = MenuHandler()
add_point_entry = self.menu_handler.insert("Add Point",
callback=self._add_point_cb)
del_point_entry = self.menu_handler.insert("Delete Point",
callback=self._del_point_cb)
add_entry = self.menu_handler.insert("Add ROI")
self.menu_item = dict()
for k in sorted(self.mesh.keys()):
entry = self.menu_handler.insert(k,
parent=add_entry,
callback=self._add_cb)
self.menu_item[entry] = k
del_entry = self.menu_handler.insert("Delete ROI",
callback=self._del_cb)
enable_entry = self.menu_handler.insert("Movement control",
callback=self._enable_cb)
self.menu_handler.setCheckState(enable_entry, MenuHandler.CHECKED)
def _add_cb(self, feedback):
rospy.loginfo("Add ROI: %s", self.menu_item[feedback.menu_entry_id])
self.add_object(self.menu_item[feedback.menu_entry_id], feedback.pose)
def _del_cb(self, feedback):
rospy.loginfo("Delete ROI: %s", feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
for r in self._soma_obj_roi_ids[roi]:
self.delete_object(r)
self.undraw_roi(roi)
def _add_point_cb(self, feedback):
rospy.loginfo("Add point: %s", feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
t = self._soma_obj_type[feedback.marker_name]
self.add_object(t, feedback.pose, roi)
#self.draw_roi(roi)
def _del_point_cb(self, feedback):
rospy.loginfo("Delete point: %s", feedback.marker_name)
self.delete_object(feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
self.draw_roi(roi)
def _update_poly(self, feedback):
return
def _update_cb(self, feedback):
p = feedback.pose.position
#print "Marker " + feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y)
self._soma_obj_pose[feedback.marker_name] = feedback.pose
roi = self._soma_obj_roi[feedback.marker_name]
self.draw_roi(roi)
if hasattr(self, "vp_timer_" + feedback.marker_name):
getattr(self, "vp_timer_" + feedback.marker_name).cancel()
setattr(self, "vp_timer_" + feedback.marker_name,
Timer(3, self.update_object, [feedback]))
getattr(self, "vp_timer_" + feedback.marker_name).start()
def _enable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.menu_handler.setCheckState(handle, MenuHandler.UNCHECKED)
self._interactive = False
else:
self.menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self._interactive = True
self.menu_handler.reApply(self._server)
self.load_objects()
self._server.applyChanges()
def _next_id(self):
self._soma_id += 1
return self._soma_id
def _next_roi_id(self):
self._soma_roi_id += 1
return self._soma_roi_id
def _retrieve_objects(self):
objs = self._msg_store.query(SOMAROIObject._type,
message_query={
"map": self.soma_map,
"config": self.soma_conf
})
max_id = 0
max_roi_id = 0
for o, om in objs:
if int(o.id) > max_id:
max_id = int(o.id)
if int(o.roi_id) > max_roi_id:
max_roi_id = int(o.roi_id)
self._soma_id = max_id
self._soma_roi_id = max_roi_id
return objs
def load_objects(self):
self._soma_obj_roi_ids = dict()
objs = self._retrieve_objects()
# if collection is empty insert initial object
if not objs:
pose = Pose()
self.add_object('Office', pose)
return
# otherwise, load all object from collection
for o, om in objs:
self._soma_obj_ids[o.id] = om['_id']
self._soma_obj_msg[o.id] = o
if o.roi_id in self._soma_obj_roi_ids:
self._soma_obj_roi_ids[o.roi_id].append(o.id)
else:
self._soma_obj_roi_ids[o.roi_id] = list()
self._soma_obj_roi_ids[o.roi_id].append(o.id)
self._soma_obj_roi[o.id] = o.roi_id
self._soma_obj_type[o.id] = o.type
self._soma_obj_pose[o.id] = o.pose
self.load_object(o.id, o.roi_id, o.type, o.pose)
self.draw_all_roi()
def draw_all_roi(self):
for key in self._soma_obj_roi_ids:
self.draw_roi(key)
def undraw_all_roi(self):
for key in self._soma_obj_roi_ids:
self.undraw_roi(key)
def draw_roi(self, roi):
v = self._soma_obj_roi_ids[roi]
t = self._soma_obj_type[v[0]]
p = self._soma_obj_pose[v[0]]
int_marker = self.create_roi_marker(roi, t, p, v)
self._server.erase("ROI-" + roi)
self._server.applyChanges()
self._server.insert(int_marker, self._update_poly)
self._server.applyChanges()
def undraw_roi(self, roi):
self._server.erase("ROI-" + roi)
self._server.applyChanges()
def load_object(self, soma_id, roi, soma_type, pose):
int_marker = self.create_object_marker(soma_id, roi, soma_type, pose)
self._server.insert(int_marker, self._update_cb)
self.menu_handler.apply(self._server, soma_id)
self._server.applyChanges()
def add_object(self, soma_type, pose, roi_id=None):
# todo: add to mongodb
soma_id = self._next_id()
if roi_id == None:
soma_roi_id = self._next_roi_id()
self._soma_obj_roi_ids[str(soma_roi_id)] = list()
else:
soma_roi_id = roi_id
soma_obj = SOMAROIObject()
soma_obj.id = str(soma_id)
soma_obj.roi_id = str(soma_roi_id)
soma_obj.map = str(self.soma_map)
soma_obj.config = str(self.soma_conf)
soma_obj.type = soma_type
soma_obj.pose = pose
soma_obj.frame = 'map'
_id = self._msg_store.insert(soma_obj)
self._soma_obj_ids[soma_obj.id] = _id
self._soma_obj_msg[soma_obj.id] = soma_obj
self._soma_obj_roi_ids[soma_obj.roi_id].append(soma_obj.id)
self._soma_obj_roi[soma_obj.id] = soma_obj.roi_id
self._soma_obj_type[soma_obj.id] = soma_type
self._soma_obj_pose[soma_obj.id] = pose
self.load_object(str(soma_id), soma_obj.roi_id, soma_type, pose)
# geospatial store
# delete old message
res = self._gs_store.find_one({
'soma_roi_id': soma_roi_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf
})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(soma_obj)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" %
(soma_obj.type, soma_obj.roi_id))
except:
rospy.logerr(
"The polygon of %s %s is malformed (self-intersecting) => Please update geometry."
% (soma_type, soma_roi_id))
def geo_json_from_soma_obj(self, soma_obj):
geo_json = {}
geo_json['soma_roi_id'] = soma_obj.roi_id
geo_json['soma_map'] = soma_obj.map
geo_json['soma_config'] = soma_obj.config
geo_json['type'] = soma_obj.type
if len(self._soma_obj_roi_ids[soma_obj.roi_id]) < 3:
rospy.logerr(
"GS Store: %s %s, less then 3 points => Add more points or delete ROI."
% (soma_obj.type, soma_obj.roi_id))
return None
coordinates = []
for obj_id in self._soma_obj_roi_ids[soma_obj.roi_id]:
p = self._soma_obj_pose[obj_id]
coordinates.append(
self._gs_store.coords_to_lnglat(p.position.x, p.position.y))
p = self._soma_obj_pose[self._soma_obj_roi_ids[soma_obj.roi_id][0]]
coordinates.append(
self._gs_store.coords_to_lnglat(p.position.x, p.position.y))
geo_json['loc'] = {'type': 'Polygon', 'coordinates': [coordinates]}
return geo_json
def delete_object(self, soma_id):
# todo: delete from mongodb
_id = self._soma_obj_ids[str(soma_id)]
self._msg_store.delete(str(_id))
self._server.erase(soma_id)
self._server.applyChanges()
roi = self._soma_obj_roi[str(soma_id)]
nodes = self._soma_obj_roi_ids[roi]
new_nodes = []
for n in nodes:
if n != str(soma_id):
new_nodes.append(n)
self._soma_obj_roi_ids[roi] = new_nodes
# geospatial store
# delete old message
old_msg = self._soma_obj_msg[soma_id]
res = self._gs_store.find_one({
'soma_roi_id': roi,
'soma_map': self.soma_map,
'soma_config': self.soma_conf
})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(old_msg)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" %
(old_msg.type, old_msg.roi_id))
except:
rospy.logerr(
"The polygon of %s %s is malformed (self-intersecting) => Please update geometry."
% (old_msg.type, old_msg.roi_id))
def update_object(self, feedback):
rospy.loginfo("Updated marker: %s", feedback.marker_name)
_id = self._soma_obj_ids[feedback.marker_name]
msg = self._soma_obj_msg[feedback.marker_name]
new_msg = copy.deepcopy(msg)
new_msg.pose = feedback.pose
self._soma_obj_pose[feedback.marker_name] = feedback.pose
self._msg_store.update_id(_id, new_msg)
# geospatial store
# delete old message
res = self._gs_store.find_one({
'soma_roi_id': new_msg.roi_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf
})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(new_msg)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" %
(new_msg.type, new_msg.roi_id))
except:
rospy.logerr(
"The polygon of %s %s is malformed (self-intersecting) => Please update geometry."
% (new_msg.type, new_msg.roi_id))
def create_object_marker(self, soma_obj, roi, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = soma_obj
int_marker.description = soma_type + ' (' + roi + ')'
int_marker.pose = pose
int_marker.pose.position.z = 0.01
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
int_marker.pose.position.z = (marker.scale.z / 2)
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
#marker.pose = pose
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
if self._interactive:
int_marker.controls.append(copy.deepcopy(control))
# add the control to the interactive marker
int_marker.controls.append(control)
# add menu control
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.markers.append(marker) #makeBox(int_marker) )
int_marker.controls.append(menu_control)
return int_marker
def create_roi_marker(self, roi, soma_type, pose, points):
#print "POINTS: " + str(points)
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = "ROI-" + roi
int_marker.description = roi
int_marker.pose = pose
marker = Marker()
marker.type = Marker.LINE_STRIP
marker.scale.x = 0.1
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(marker)
int_marker.controls.append(control)
marker.points = []
for point in points:
p = Point()
pose = self._soma_obj_pose[point]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
p = Point()
pose = self._soma_obj_pose[points[0]]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
return int_marker
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)
