def rm_tag_cb(self, msg):
#rospy.loginfo('Adding Tag '+msg.tag+' to '+str(msg.node))
succeded = True
for j in msg.node:
msg_store = MessageStoreProxy(collection='topological_maps')
query = {"name" : j, "pointset": self.nodes.name}
query_meta = {}
query_meta["pointset"] = self.nodes.name
query_meta["map"] = self.nodes.map
#print query, query_meta
available = msg_store.query(strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta)
#print len(available)
succeded = False
for i in available:
msgid= i[1]['_id']
if 'tag' in i[1]:
if msg.tag in i[1]['tag']:
print 'removing tag'
i[1]['tag'].remove(msg.tag)
print 'new list of tags'
print i[1]['tag']
msg_store.update_id(msgid, i[0], i[1], upsert = False)
succeded = True
meta_out = str(i[1])
return succeded, meta_out
def modify_tag_cb(self, msg):
succeded = True
meta_out = None
for node in msg.node:
msg_store = MessageStoreProxy(collection='topological_maps')
query = {"name": node, "pointset": self.nodes.name}
query_meta = {}
query_meta["pointset"] = self.nodes.name
query_meta["map"] = self.nodes.map
#print query, query_meta
available = msg_store.query(
strands_navigation_msgs.msg.TopologicalNode._type, query,
query_meta)
#print len(available)
for node_plus_meta in available:
msgid = node_plus_meta[1]['_id']
if 'tag' in node_plus_meta[1]:
if not msg.tag in node_plus_meta[1]['tag']:
continue
else:
tag_ind = node_plus_meta[1]['tag'].index(msg.tag)
node_plus_meta[1]['tag'][tag_ind] = msg.new_tag
meta_out = str(node_plus_meta[1])
msg_store.update_id(msgid,
node_plus_meta[0],
node_plus_meta[1],
upsert=True)
if len(available) == 0:
succeded = False
return succeded, meta_out
def add_tag_cb(self, msg):
#rospy.loginfo('Adding Tag '+msg.tag+' to '+str(msg.node))
succeded = True
for j in msg.node:
msg_store = MessageStoreProxy(collection='topological_maps')
query = {"name" : j, "pointset": self.nodes.name}
query_meta = {}
query_meta["pointset"] = self.nodes.name
query_meta["map"] = self.nodes.map
#print query, query_meta
available = msg_store.query(strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta)
#print len(available)
for i in available:
msgid= i[1]['_id']
if 'tag' in i[1]:
if not msg.tag in i[1]['tag']:
i[1]['tag'].append(msg.tag)
else:
a=[]
a.append(msg.tag)
i[1]['tag']=a
meta_out = str(i[1])
msg_store.update_id(msgid, i[0], i[1], upsert = False)
#print trstr
if len(available) == 0:
succeded = False
return succeded, meta_out
def add_localise_by_topic(tmap, node, json_str):
#print req
#data = json.loads(req.content)
#print data
msg_store = MessageStoreProxy(collection='topological_maps')
query = {"name" : node, "pointset": tmap}
query_meta = {}
#query_meta["pointset"] = tmap
#query_meta["map"] = self.nodes.map
#print query, query_meta
available = msg_store.query(strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta)
#print len(available)
if len(available) != 1:
#succeded = False
print 'there are no nodes or more than 1 with that name'
else:
#succeded = True
for i in available:
if not i[0].localise_by_topic:
msgid= i[1]['_id']
i[0].localise_by_topic=json_str
#print i[0]
print "Updating %s--%s" %(i[0].pointset, i[0].name)
msg_store.update_id(msgid, i[0], i[1], upsert = False)
def add_tag_to_mongo(self, msg):
succeded = True
meta_out = None
for j in msg.node:
msg_store = MessageStoreProxy(collection='topological_maps')
query = {"name": j, "pointset": self.nodes.name}
query_meta = {}
query_meta["pointset"] = self.nodes.name
query_meta["map"] = self.nodes.map
#print query, query_meta
available = msg_store.query(
strands_navigation_msgs.msg.TopologicalNode._type, query,
query_meta)
#print len(available)
for i in available:
msgid = i[1]['_id']
if 'tag' in i[1]:
if not msg.tag in i[1]['tag']:
i[1]['tag'].append(msg.tag)
else:
a = []
a.append(msg.tag)
i[1]['tag'] = a
meta_out = str(i[1])
msg_store.update_id(msgid, i[0], i[1], upsert=False)
#print trstr
if len(available) == 0:
succeded = False
return succeded, meta_out
def rm_tag_cb(self, msg):
#rospy.loginfo('Adding Tag '+msg.tag+' to '+str(msg.node))
succeded = True
for j in msg.node:
msg_store = MessageStoreProxy(collection='topological_maps')
query = {"name": j, "pointset": self.nodes.name}
query_meta = {}
query_meta["pointset"] = self.nodes.name
query_meta["map"] = self.nodes.map
#print query, query_meta
available = msg_store.query(
strands_navigation_msgs.msg.TopologicalNode._type, query,
query_meta)
#print len(available)
succeded = False
meta_out = None
for i in available:
msgid = i[1]['_id']
if 'tag' in i[1]:
if msg.tag in i[1]['tag']:
print 'removing tag'
i[1]['tag'].remove(msg.tag)
print 'new list of tags'
print i[1]['tag']
msg_store.update_id(msgid, i[0], i[1], upsert=False)
succeded = True
meta_out = str(i[1])
return succeded, meta_out
def add_localise_by_topic(tmap, node, json_str):
#print req
#data = json.loads(req.content)
#print data
msg_store = MessageStoreProxy(collection='topological_maps')
query = {"name": node, "pointset": tmap}
query_meta = {}
#query_meta["pointset"] = tmap
#query_meta["map"] = self.nodes.map
#print query, query_meta
available = msg_store.query(
topological_navigation_msgs.msg.TopologicalNode._type, query,
query_meta)
#print len(available)
if len(available) != 1:
#succeded = False
print 'there are no nodes or more than 1 with that name'
else:
#succeded = True
for i in available:
if not i[0].localise_by_topic:
msgid = i[1]['_id']
i[0].localise_by_topic = json_str
#print i[0]
print "Updating %s--%s" % (i[0].pointset, i[0].name)
msg_store.update_id(msgid, i[0], i[1], upsert=False)
def add_content_cb(self, req):
#print req
data = json.loads(req.content)
#print data
msg_store = MessageStoreProxy(collection='topological_maps')
query = {"name": req.node, "pointset": self.nodes.name}
query_meta = {}
query_meta["pointset"] = self.nodes.name
query_meta["map"] = self.nodes.map
#print query, query_meta
available = msg_store.query(
strands_navigation_msgs.msg.TopologicalNode._type, query,
query_meta)
#print len(available)
if len(available) != 1:
succeded = False
meta_out = None
print 'there are no nodes or more than 1 with that name'
else:
succeded = True
for i in available:
msgid = i[1]['_id']
if 'contains' in i[1]:
if type(data) is list:
for j in data:
if 'category' in j and 'name' in j:
i[1]['contains'].append(j)
elif type(data) is dict:
if 'category' in data and 'name' in data:
i[1]['contains'].append(data)
else:
a = []
if type(data) is list:
for j in data:
if 'category' in j and 'name' in j:
a.append(j)
elif type(data) is dict:
if 'category' in data and 'name' in data:
a.append(data)
i[1]['contains'] = a
meta_out = str(i[1])
print "Updating %s--%s" % (i[0].pointset, i[0].name)
msg_store.update_id(msgid, i[0], i[1], upsert=False)
return succeded, meta_out
def add_content_cb(self, req):
#print req
data = json.loads(req.content)
#print data
msg_store = MessageStoreProxy(collection='topological_maps')
query = {"name" : req.node, "pointset": self.nodes.name}
query_meta = {}
query_meta["pointset"] = self.nodes.name
query_meta["map"] = self.nodes.map
#print query, query_meta
available = msg_store.query(strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta)
#print len(available)
if len(available) != 1:
succeded = False
print 'there are no nodes or more than 1 with that name'
else:
succeded = True
for i in available:
msgid= i[1]['_id']
if 'contains' in i[1]:
if type(data) is list :
for j in data:
if 'category' in j and 'name' in j :
i[1]['contains'].append(j)
elif type(data) is dict :
if 'category' in data and 'name' in data :
i[1]['contains'].append(data)
else:
a=[]
if type(data) is list :
for j in data:
if 'category' in j and 'name' in j :
a.append(j)
elif type(data) is dict :
if 'category' in data and 'name' in data :
a.append(data)
i[1]['contains']=a
meta_out = str(i[1])
print "Updating %s--%s" %(i[0].pointset, i[0].name)
msg_store.update_id(msgid, i[0], i[1], upsert = False)
return succeded, meta_out
def check_sanity(client):
msg_store = MessageStoreProxy(collection='topological_maps')
db = client.message_store
collection = db["topological_maps"]
available = collection.find().distinct("_meta.pointset")
print available
for point_set in available:
#get one message
#search = {"_meta.pointset": pointset}
query_meta = {}
query_meta["pointset"] = point_set
message_list = msg_store.query(TopologicalNode._type, {}, query_meta)
#points = strands_navigation_msgs.msg.TopologicalMap()
#points.name = point_set
#points.map = point_set
#points.pointset = point_set
#string last_updated
eids = []
for i in message_list:
update = False
print i[0].pointset, i[0].name, i[1]['_id']
if i[0].edges:
for j in i[0].edges:
if j.edge_id == '':
update = True
print 'needs edge id'
j.edge_id = get_edge_id(i[0].name, j.node, eids)
print 'new edge_id %s' % j.edge_id
eids.append(j.edge_id)
if j.top_vel <= 0.1:
update = True
print 'needs top vel'
j.top_vel = 0.55
if j.map_2d == '':
update = True
print 'needs map_2d'
j.map_2d = i[0].map
if update:
msg_store.update_id(i[1]['_id'], i[0], i[1], upsert=False)
def check_sanity(client):
msg_store = MessageStoreProxy(collection='topological_maps')
db=client.message_store
collection=db["topological_maps"]
available = collection.find().distinct("_meta.pointset")
print available
for point_set in available:
#get one message
#search = {"_meta.pointset": pointset}
query_meta = {}
query_meta["pointset"] = point_set
message_list = msg_store.query(TopologicalNode._type, {}, query_meta)
#points = strands_navigation_msgs.msg.TopologicalMap()
#points.name = point_set
#points.map = point_set
#points.pointset = point_set
#string last_updated
eids = []
for i in message_list:
update = False
print i[0].pointset, i[0].name, i[1]['_id']
if i[0].edges:
for j in i[0].edges :
if j.edge_id == '':
update = True
print 'needs edge id'
j.edge_id = get_edge_id(i[0].name, j.node, eids)
print 'new edge_id %s' %j.edge_id
eids.append(j.edge_id)
if j.top_vel <= 0.1 :
update = True
print 'needs top vel'
j.top_vel = 0.55
if j.map_2d == '':
update = True
print 'needs map_2d'
j.map_2d = i[0].map
if update:
msg_store.update_id(i[1]['_id'], i[0], i[1], upsert = False)
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
p_id = msg_store.insert(p)
p_id = msg_store.insert(['test1', 'test2'])
# get it back with a name
print msg_store.query_named("my favourite pose", Pose._type)
p.position.x = 666
# update it with a name
msg_store.update_named("my favourite pose", p)
p.position.y = 2020
# update the other inserted one using the id
msg_store.update_id(p_id, p)
stored_p, meta = msg_store.query_id(p_id, Pose._type)
assert stored_p.position.x == 666
assert stored_p.position.y == 2020
print "stored object ok"
print "stored object inserted at %s (UTC rostime) by %s" % (
meta['inserted_at'], meta['inserted_by'])
print "stored object last updated at %s (UTC rostime) by %s" % (
meta['last_updated_at'], meta['last_updated_by'])
# some other things you can do...
# get it back with a name
print msg_store.query_named("my favourite pose", Pose._type)
p_id = msg_store.insert(p)
p_id = msg_store.insert(['test1', 'test2'])
# get it back with a name
print msg_store.query_named("my favourite pose", Pose._type)
p.position.x = 666
# update it with a name
msg_store.update_named("my favourite pose", p)
p.position.y = 2020
# update the other inserted one using the id
msg_store.update_id(p_id, p)
stored_p, meta = msg_store.query_id(p_id, Pose._type)
assert stored_p.position.x == 666
assert stored_p.position.y == 2020
print "stored object ok"
print "stored object inserted at %s (UTC rostime) by %s" % (meta['inserted_at'], meta['inserted_by'])
print "stored object last updated at %s (UTC rostime) by %s" % (meta['last_updated_at'], meta['last_updated_by'])
# some other things you can do...
# get it back with a name
print msg_store.query_named("my favourite pose", Pose._type)
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 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 SOMAROIDrawer():
def __init__(self):
#self.soma_map = soma_map
#self.soma_map_name = soma_map_name
# default file
rp = RosPack()
# self._interactive = True
self._msg_store=MessageStoreProxy(database="soma2data",collection="soma2_roi")
s = rospy.Service('soma2/draw_roi', DrawROI, self.handle_draw_roi)
# Publisher vis_pub = node_handle.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
self.markerpub = rospy.Publisher("visualization_marker_array", MarkerArray, queue_size=1)
rospy.spin()
def handle_draw_roi(self,req):
self._delete_markers()
if(req.roi_id >=0):
return DrawROIResponse(self.load_objects(req.map_name,req.roi_id))
return True
def _update_poly(self, feedback):
return
def _delete_markers(self):
marker = Marker()
marker.action = 3
marker.header.frame_id = "map"
markerarray = MarkerArray()
markerarray.markers.append(marker)
self.markerpub.publish(markerarray)
def coords_to_lnglat(x, y):
earth_radius = 6371000.0 # in meters
lng = 90 - math.degrees(math.acos(float(x) / earth_radius))
lat = 90 - math.degrees(math.acos(float(y) / earth_radius))
return [lng , lat]
def _retrieve_objects(self, map_name, roi_id):
objs = self._msg_store.query(SOMA2ROIObject._type, message_query={"map_name": map_name,
"roi_id": roi_id})
#print objs
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, map_name, roi_id):
# this is the array for roi ids
self._soma_obj_roi_ids = dict()
markerarray = MarkerArray()
#get objects from db
objs = self._retrieve_objects(map_name,roi_id)
# if collection is empty insert initial object
if not objs:
return False
# otherwise, load all object from collection
for o,om in objs:
count = 1
for pose in o.posearray.poses:
self.load_object(o.id, o.roi_id, o.type, pose,count,markerarray)
count +=1
# print count
self.draw_roi(roi_id,o.posearray.poses,markerarray,count)
# print len(markerarray.markers)
self.markerpub.publish(markerarray)
return True
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,poses,markerarray,ccstart):
roicp = roi
p = poses
cc = ccstart
# print "t is ",t," p is ", p
for pose in p:
# print "This is the pose: ", pose
int_marker = self.create_roi_marker(roi, pose, p,cc)
markerarray.markers.append(int_marker)
cc = cc+1
def undraw_roi(self, roi):
self._server.erase("ROI-" + roi)
self._server.applyChanges()
def load_object(self, soma_id, roi, soma_type, pose,markerno, markerarray):
# print self._soma_obj_markers[str(soma_id)]
# print str(soma_id)
int_marker = self.create_object_marker(soma_id, roi, soma_type, pose, markerno)
markerarray.markers.append(int_marker)
#self.markerpub.publish(int_marker)
#print self._soma_obj_markers[str(soma_id)].keys()
#soma_type = Office, Kitchen, etc, Pose is position
def add_object(self, soma_type, pose, roi_id=None):
# todo: add to mongodb
#create a SOMA2ROI Object
soma_obj = SOMA2ROIObject()
# print roi_id
# a new roi
if roi_id == None:
#soma_id is an id for the soma object like 1,2,3,4. It updates itself from the db if there are existing objects
soma_id = self._next_id()
#soma_roi_id is acutally the roi number. Is it 1,2,3,4? Multiple soma objects can have the same roi id
soma_roi_id = self._next_roi_id()
roi_id = soma_roi_id
# print soma_roi_id
soma_obj.id = str(soma_id)
soma_obj.roi_id = str(soma_roi_id)
soma_obj.map_name = str(self.soma_map_name)
soma_obj.map_unique_id = str(self.map_unique_id)
soma_obj.config = str(self.soma_conf)
soma_obj.type = soma_type
soma_obj.posearray.poses.append(pose)
soma_obj.frame = 'map'
self._soma_obj_roi_ids[str(soma_roi_id)] = list()
self._soma_obj_markers[soma_obj.id] = dict()
#_id = self._msg_store.update_id
_id = self._msg_store.insert(soma_obj)
self._soma_obj_ids[soma_obj.id] = _id
self._soma_obj_roi_ids[soma_obj.roi_id].append(soma_obj.id)
self._soma_obj_type[soma_obj.id] = soma_type
self._soma_obj_roi[soma_obj.id] = roi_id
self._soma_obj_msg[soma_obj.id] = soma_obj
self._soma_obj_pose[soma_obj.id] = soma_obj.posearray.poses
else:
# Get the roi id
soma_roi_id = roi_id
#print roi_id," ",self.soma_map," ",self.soma_conf," ",self._soma_obj_ids['1']
#call the object with that id
res = self._msg_store.query(SOMA2ROIObject._type,message_query={'id':str(roi_id)})
#iterate through the objects. Normally there should be only 1 object returned
for o,om in res:
# print o," hi ",om
soma_obj = o
# print "Soma Object: ", soma_obj
if soma_obj:
soma_obj.posearray.poses.append(pose)
self._soma_obj_pose[soma_obj.id] = soma_obj.posearray.poses
self.insert_geo_json(soma_obj.roi_id,soma_obj)
#print soma_obj
_id = self._soma_obj_ids[soma_obj.id]
_newid = self._msg_store.update_id(_id,soma_obj)
soma_id = soma_obj.id
self._soma_obj_msg[soma_obj.id] = soma_obj
#_id = self._msg_store.update_id
#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].append(soma_obj.roi_id)
#for pose in soma_obj.posearray.poses:
self.load_object(str(soma_id), soma_obj.roi_id, soma_type, pose)
def create_object_marker(self, soma_obj, roi, soma_type, pose,markerno):
# create an interactive marker for our server
marker = Marker()
marker.header.frame_id = "map"
#int_marker.name = soma_obj+'_'+str(markerno)
#int_marker.description = soma_type + ' (' + roi +'_'+str(markerno)+ ')'
marker.pose = pose
marker.id = markerno;
# print marker.pose
marker.pose.position.z = 0.01
#marker = Marker()
marker.type = Marker.SPHERE
marker.action = 0
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
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
return marker
# This part draws the line strips between the points
def create_roi_marker(self, roi, pose, points, count):
#print "POINTS: " + str(points)
#points are all the points belong to that roi, pose is one of the points
marker = Marker()
# print "Marker name: ", int_marker.name
marker.pose = pose
marker.header.frame_id = "map"
marker.type = Marker.LINE_STRIP
marker.scale.x = 0.1
marker.id= count
random.seed(10)
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.points = []
for point in points:
p = Point()
pose = point#self._soma_obj_pose[point]
p.x = pose.position.x - marker.pose.position.x
p.y = pose.position.y - marker.pose.position.y
marker.points.append(p)
p = Point()
pose = points[0]
p.x = pose.position.x - marker.pose.position.x
p.y = pose.position.y - marker.pose.position.y
marker.points.append(p)
return marker
