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