def _wm_get_cb(self, msg): with self._times: to_ret = srvs.WmGetResponse() if msg.action == msg.GET: to_ret.elements.append( utils.element2msg( self._get_context(msg.context).get_element( msg.element.id))) elif msg.action == msg.GET_TEMPLATE: to_ret.elements.append( utils.element2msg( self._get_context(msg.context).get_template_individual( msg.element.label))) elif msg.action == msg.GET_RECURSIVE: for _, e in self._get_context(msg.context).get_recursive( msg.element.id, msg.relation_filter, msg.type_filter).items(): to_ret.elements.append(utils.element2msg(e)) elif msg.action == msg.RESOLVE: for e in self._get_context(msg.context).resolve_elements( utils.msg2element(msg.element)): to_ret.elements.append(utils.element2msg(e)) output = "" einput = utils.msg2element(msg.element) for e in to_ret.elements: output += "{} ".format(e.id) if self._verbose: log.info( "[WmGet]", "Done {} [{}]. Answer: {}. Time: {:0.3f} secs".format( msg.action, einput, output, self._times.get_last())) to_ret.snapshot_id = self._curr_snapshot.hex return to_ret
def get_element(self, eid, context_id='scene'): """ @brief Gets an element instanciated in the world model. @param eid (string)Id of the element instance @param context_id (string)Ontology context identifier @return (Element) """ if eid not in WorldModelInterface._elements_cache: msg = srvs.WmGetRequest() e = msgs.WmElement() e.id = eid msg.context = context_id msg.element = e msg.action = msg.GET res = self._call(self._get, msg) if not res: return None if self._make_cache: WorldModelInterface._elements_cache[eid] = utils.msg2element( res.elements[0]) else: return utils.msg2element(res.elements[0]) return WorldModelInterface._elements_cache[eid]
def _wm_modify_cb(self, msg): to_ret = srvs.WmModifyResponse() with self._times: if msg.action == msg.ADD: for e in msg.elements: updated_e = self._get_context(msg.context).add_element( utils.msg2element(e), msg.author) to_ret.elements.append(utils.element2msg(updated_e)) self._publish_change(msg.author, "add", elements=to_ret.elements, context_id=msg.context) elif msg.action == msg.UPDATE: for e in msg.elements: self._get_context(msg.context).update_element( utils.msg2element(e), msg.author) er = utils.element2msg( self._get_context(msg.context).get_element(e.id)) to_ret.elements.append(er) self._publish_change(msg.author, "update", elements=to_ret.elements, context_id=msg.context) elif msg.action == msg.UPDATE_PROPERTIES: for e in msg.elements: self._get_context(msg.context).update_properties( utils.msg2element(e), msg.author, self._ontology.get_reasoner(msg.type_filter), False) er = utils.element2msg( self._get_context(msg.context).get_element(e.id)) to_ret.elements.append(er) self._publish_change(msg.author, "update", elements=to_ret.elements, context_id=msg.context) elif msg.action == msg.REMOVE: for e in msg.elements: self._get_context(msg.context).remove_element( utils.msg2element(e), msg.author) to_ret.elements = msg.elements self._publish_change(msg.author, "remove", elements=msg.elements, context_id=msg.context) elif msg.action == msg.REMOVE_RECURSIVE: for e in msg.elements: self._get_context(msg.context).remove_recursive( utils.msg2element(e), msg.author, msg.relation_filter, msg.type_filter) to_ret.elements = msg.elements self._publish_change(msg.author, "remove_recursive", elements=msg.elements, context_id=msg.context) if self._verbose: log.info( "[WmModify]", "{} {} {}. Time: {:0.3f} secs".format( msg.author, msg.action, [e.id for e in to_ret.elements], self._times.get_last())) return to_ret
def on_wm_update(self, data): with self._wm_mutex: if self._snapshot_id == data.prev_snapshot_id and data.action != "reset": # Discard msgs not in sync with local wm version self._snapshot_id = data.snapshot_id cur_item = self.wm_tree_widget.currentItem() cur_item_id = cur_item.text(1) if data.action == 'update' or data.action == 'update_properties': for elem in data.elements: elem = rosutils.msg2element(elem) self._update_wm_node(elem, cur_item_id) elif data.action == 'add': for elem in data.elements: elem = rosutils.msg2element(elem) if not self._add_wm_node(elem): self._snapshot_id = "" elif data.action == 'remove' or data.action == 'remove_recursive': for elem in data.elements: elem = rosutils.msg2element(elem) self._remove_wm_node(elem) # reselect current item items = self.wm_tree_widget.findItems(cur_item_id, Qt.MatchRecursive | Qt.MatchFixedString, 1) if items: self.wm_tree_widget.setCurrentItem(items[0]) #self._save_log(data, "wm_edit") elif data.stamp > self._snapshot_stamp or self._snapshot_id == "": # Ignores obsolete msgs log.info("[wm_update]", "Wm not in sync, querying wm scene") self.create_wm_tree()
def _monitor_cb(self, msg): """ @brief Callback updating the cache when a change on wm is detected """ if self._make_cache: if self._last_snapshot_id != msg.prev_snapshot_id or msg.action == 'reset': WorldModelInterface._elements_cache.clear() self._last_snapshot_id = msg.snapshot_id for elem in msg.elements: elem = utils.msg2element(elem) if msg.action == 'update' or msg.action == 'update_properties' or msg.action == 'add': WorldModelInterface._elements_cache[elem.id] = elem elif msg.action == 'remove' or msg.action == 'remove_recursive': if elem.id in WorldModelInterface._elements_cache: del WorldModelInterface._elements_cache[elem.id] else: log.error("[WmMonitor]", "Command {} not recognized.".format(msg.action)) if msg.relation: rel = utils.msg2relation(msg.relation[0]) if rel['src'] in WorldModelInterface._elements_cache: del WorldModelInterface._elements_cache[rel['src']] if rel['dst'] in WorldModelInterface._elements_cache: del WorldModelInterface._elements_cache[rel['dst']] if self._external_monitor_cb: self._external_monitor_cb(msg)
def get_branch(self, eid, relation_filter="skiros:sceneProperty", type_filter="", context_id='scene'): """ @brief Get an element and related children elements. Answer can be filtered @param eid (string)Id of branch root element @param relation_filter (string)Relation type to consider to retrieve the children @param type_filter (string)Filter on Element class type (e.g. skiros:Manipulatable) @param context_id (string)Ontology context identifier @return list(Element) """ msg = srvs.WmGetRequest() e = msgs.WmElement() e.id = eid msg.context = context_id msg.element = e msg.action = msg.GET_RECURSIVE msg.relation_filter = relation_filter msg.type_filter = type_filter res = self._call(self._get, msg) if (res): return [utils.msg2element(x) for x in res.elements]
def get_element(self, eid, context_id='scene'): if eid not in WorldModelInterface._elements_cache: msg = srvs.WmGetRequest() e = msgs.WmElement() e.id = eid msg.context = context_id msg.element = e msg.action = msg.GET res = self._call(self._get, msg) if not res: return None if self._make_cache: WorldModelInterface._elements_cache[eid] = utils.msg2element( res.elements[0]) else: return utils.msg2element(res.elements[0]) return WorldModelInterface._elements_cache[eid]
def resolve_elements(self, e, context_id='scene'): msg = srvs.WmGetRequest() msg.context = context_id msg.element = utils.element2msg(e) msg.action = msg.RESOLVE res = self._call(self._get, msg) if (res): return [utils.msg2element(x) for x in res.elements]
def get_template_element(self, uri, context_id='scene'): msg = srvs.WmGetRequest() e = msgs.WmElement() e.label = uri msg.context = context_id msg.element = e msg.action = msg.GET_TEMPLATE res = self._call(self._get, msg) if (res): return utils.msg2element(res.elements[0])
def add_elements(self, es, context_id='scene'): msg = srvs.WmModifyRequest() msg.context = context_id msg.author = self._author_name + self._debug_info() for e in es: msg.elements.append(utils.element2msg(e)) msg.action = msg.ADD res = self._call(self._modify, msg) to_ret = list() if res: for old, new in zip(es, res.elements): to_ret.append(utils.msg2element(new)) old._id = new.id self._resolve_local_relations(old) return to_ret
def get_scene(self): """ @brief Returns a triple with all elements in the scene and the associated uuid """ msg = srvs.WmGetRequest() e = msgs.WmElement() e.id = "skiros:Scene-0" msg.context = 'scene' msg.element = e msg.action = msg.GET_RECURSIVE msg.relation_filter = "skiros:sceneProperty" msg.type_filter = "" res = self._call(self._get, msg) if (res): return ([utils.msg2element(x) for x in res.elements], res.snapshot_id)
def resolve_elements(self, e, context_id='scene'): """ @brief Find all elements matching the input *type, label and properties) @param e An element to match @param context_id (string)Ontology context identifier @return list(Element) List of matches """ msg = srvs.WmGetRequest() msg.context = context_id msg.element = utils.element2msg(e) msg.action = msg.RESOLVE res = self._call(self._get, msg) if (res): return [utils.msg2element(x) for x in res.elements]
def get_template_element(self, uri, context_id='scene'): """ @brief Gets a template element, an ontology individual that is not instanciated in the world model. @param uri (string)URI of the individual @param context_id (string)Ontology context identifier @return (Element) """ msg = srvs.WmGetRequest() e = msgs.WmElement() e.label = uri msg.context = context_id msg.element = e msg.action = msg.GET_TEMPLATE res = self._call(self._get, msg) if (res): return utils.msg2element(res.elements[0])
def get_branch(self, eid, relation_filter=":sceneProperty", type_filter="", context_id='scene'): """ Get all elements related to the starting one. Answer can be filtered """ msg = srvs.WmGetRequest() e = msgs.WmElement() e.id = eid msg.context = context_id msg.element = e msg.action = msg.GET_RECURSIVE msg.relation_filter = relation_filter msg.type_filter = type_filter res = self._call(self._get, msg) if (res): return [utils.msg2element(x) for x in res.elements]
def add_elements(self, es, context_id='scene'): """ @brief Add elements to the scene. The id is updated with the one assigned from the world model @param es list(Elements) without id @param context_id (string)Ontology context identifier @return list(Elements) with id """ msg = srvs.WmModifyRequest() msg.context = context_id msg.author = self._author_name + self._debug_info() for e in es: msg.elements.append(utils.element2msg(e)) msg.action = msg.ADD res = self._call(self._modify, msg) to_ret = list() if res: for old, new in zip(es, res.elements): to_ret.append(utils.msg2element(new)) old._id = new.id self._resolve_local_relations(old) return to_ret