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