def _on_progress_update(self, *args, **kwargs):
"""
@brief Publish skill progress
"""
log.debug(
"[{}]".format(self.__class__.__name__),
"{}:Task[{task_id}]{type}:{label}[{id}]: Message[{code}]: {msg} ({state})"
.format(self.sm._agent_name[1:], **kwargs))
msg = msgs.SkillProgress()
msg.robot = rospy.get_name()
msg.task_id = kwargs['task_id']
msg.id = kwargs['id']
msg.type = kwargs['type']
msg.label = kwargs['label']
if self.publish_runtime_parameters:
msg.params = utils.serializeParamMap(kwargs['params'])
msg.state = kwargs['state'].value
msg.processor = kwargs['processor']
msg.parent_label = kwargs['parent_label']
msg.parent_id = kwargs['parent_id']
msg.progress_code = kwargs['code']
msg.progress_period = kwargs['period']
msg.progress_time = kwargs['time']
msg.progress_message = kwargs['msg']
self._monitor.publish(msg)
def _addNode(self, element):
element.setUri(self._id_gen.getId(element.getIdNumber()))
if self._verbose:
log.debug('add', str(element._id))
props = {"type": element._type, "label": element._label}
self._graph.add_node(dict(chain(props.items(), element._properties.items())), element._id)
self._addType(element._type, element._id)
def _on_progress_update(self, *args, **kwargs):
"""
@brief Publish all skill progress
"""
task_id = kwargs['task_id']
tree = kwargs['tree']
messages = msgs.TreeProgress()
for (idd, desc) in tree:
log.debug(
"[{}]".format(self.__class__.__name__),
"{}:Task[{task_id}]{type}:{label}[{id}]: Message[{code}]: {msg} ({state})"
.format(self.sm._agent_name[1:],
task_id=task_id,
id=idd,
**desc))
msg = msgs.SkillProgress()
msg.robot = rospy.get_name()
msg.task_id = task_id
msg.id = idd
msg.type = desc['type']
msg.label = desc['label']
if self.publish_runtime_parameters:
msg.params = utils.serializeParamMap(desc['params'])
msg.state = desc['state'].value
msg.processor = desc['processor']
msg.parent_label = desc['parent_label']
msg.parent_id = desc['parent_id']
msg.progress_code = desc['code']
msg.progress_period = desc['period']
msg.progress_time = desc['time']
msg.progress_message = desc['msg']
messages.progress.append(msg)
self._monitor.publish(messages)
def remove_element(self, eid):
if self._verbose:
log.debug('remove', str(eid))
self.pushElement(eid, "remove")
eprops = self._graph.get_node(eid)
self._removeType(eprops["type"], eid)
self._graph.remove_node(eid)
def on_properties_table_item_changed(self, item, row):
item_key = self.wm_properties_widget.item(row, 0)
item_val = self.wm_properties_widget.item(row, 1)
key = item_key.id
value = item_val.text()
elem = self._wmi.get_element(item.text(1))
if elem.hasProperty(key):
prop = elem.getProperty(key)
if value == '':
value = None
if prop.dataTypeIs(bool):
value = item_val.checkState() == Qt.Checked
try:
elem.setProperty(prop.key, value, is_list=prop.isList(), force_convertion=value is not None)
log.debug(self.__class__.__name__, '<{}> property {} to {}'.format(item.text(1), prop.key, value))
except ValueError:
log.error(self.__class__.__name__, 'Changing <{}> property {} to {} failed'.format(item.text(1), prop.key, value))
item_val.setText(str(prop.value))
elif hasattr(elem, key.lower()):
key = key.lower()
attr = getattr(elem, key)
setattr(elem, key, type(attr)(value))
log.debug(self.__class__.__name__, '<{}> attribute {} to {}'.format(item.text(1), key, value))
else:
log.error(self.__class__.__name__, 'Changing <{}> property {} to {} failed'.format(item.text(1), key, value))
self._wmi.update_element_properties(elem)
name = utils.ontology_type2name(elem.id) if not elem.label else utils.ontology_type2name(elem.label)
item.setText(0, name)
def _trigger_children_update(self, e):
"""
@brief Children of e get scheduled for a check of the parent frame
"""
c_rel = e.getRelations(pred=self._spatial_rels, subj="-1")
for r in c_rel:
if r['dst'] in self._tf_list:
log.debug("[{}]".format(self.__class__.__name__), " {} updates child {}".format(e.id, r['dst']))
self._e_to_update.append(self._wmi.get_element(r['dst']))
def on_remove_object_button_clicked(self):
item = self.wm_tree_widget.currentItem()
item_id = item.text(1)
if not item_id:
return
parent = item.parent()
self.wm_tree_widget.setCurrentItem(parent)
elem = self._wmi.get_element(item_id)
self._wmi.remove_element(elem)
# parent = self.wm_tree_widget.currentItem()
# parent_id = parent.text(1)
log.debug(self.__class__.__name__, 'Removed element {}'.format(item_id))
def run(self, *args, **kwargs):
"""
@brief Copied from tfd-src-0.4/plan.py. Redirects stdin and stdout
and invokes a script
@param args The arguments
@param kwargs The keywords arguments
"""
input = kwargs.pop("input", None)
assert not kwargs
redirections = {}
if input:
redirections["stdin"] = open(input)
redirections["stdout"] = subprocess.PIPE
log.debug("[run]", "{} {}".format(args, redirections))
output = subprocess.Popen(sum([arg.split("+") for arg in args], []),
**redirections).communicate()[0]
log.debug("[output]", str(output))
def on_add_object_button_clicked(self):
dialog = SkirosAddObjectDialog(self)
ret = dialog.exec_()
if not ret:
return
log.debug(self.__class__.__name__, 'Create element based on {}'.format(dialog.object))
parent = self.wm_tree_widget.currentItem()
parent_id = parent.text(1)
if not parent_id:
return
elem = self._wmi.get_template_element(dialog.object)
elem.label = utils.ontology_type2name(dialog.object)
elem_id = self._wmi.instanciate(elem, recursive=True, relations=[{'src': parent_id, 'type': 'skiros:contain', 'dst': '-1'}])
# parent = self.wm_tree_widget.currentItem()
# parent_id = parent.text(1)
log.debug(self.__class__.__name__, 'Added element {} to {}'.format(elem_id, parent_id))
def _addEdge(self, esubject, relation, eobject):
if self._verbose:
log.debug('add', str(esubject) + "-" + relation + "-" + str(eobject))
self._graph.add_edge(esubject, eobject, {"type": relation})
def on_save_scene_button_clicked(self):
file = self.scene_file_lineEdit.text()
log.debug(self.__class__.__name__, 'Saving world model to <{}>'.format(file))
self._wmi.save(file)
def on_load_scene_button_clicked(self):
file = self.scene_file_lineEdit.text()
log.debug(self.__class__.__name__, 'Loading world model from <{}>'.format(file))
self._wmi.load(file)