From 809ee747ffb83d03aa870acb3aff0c46ddc9792d Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Tue, 9 Apr 2019 17:59:36 +0200 Subject: [PATCH 1/2] Applied pycodestyle (pep8) with "--aggresive" This should fix most violations of pep8. Excemptions were made for manually structured code through tabs. --- skiros2_common/setup.py | 2 +- .../src/skiros2_common/core/abstract_skill.py | 103 +++--- .../src/skiros2_common/core/conditions.py | 231 +++++++------ .../skiros2_common/core/discrete_reasoner.py | 1 + .../src/skiros2_common/core/params.py | 41 ++- .../src/skiros2_common/core/primitive.py | 2 + .../src/skiros2_common/core/property.py | 17 +- .../src/skiros2_common/core/utils.py | 4 +- .../src/skiros2_common/core/world_element.py | 86 +++-- .../src/skiros2_common/ros/utils.py | 58 +++- .../src/skiros2_common/tools/decorators.py | 2 +- .../src/skiros2_common/tools/dict_diff.py | 8 +- .../src/skiros2_common/tools/id_generator.py | 2 +- .../src/skiros2_common/tools/logger.py | 77 +++-- .../src/skiros2_common/tools/plugin_loader.py | 34 +- .../src/skiros2_common/tools/time_keeper.py | 13 +- skiros2_gui/setup.py | 2 +- skiros2_gui/src/skiros2_gui/core/__init__.py | 1 - .../src/skiros2_gui/core/skiros_widget.py | 249 +++++++------- skiros2_gui/src/skiros2_gui/gui.py | 5 +- skiros2_skill/nodes/utils/cmd | 12 +- skiros2_skill/setup.py | 2 +- .../src/skiros2_skill/core/processors.py | 57 ++-- skiros2_skill/src/skiros2_skill/core/skill.py | 148 +++++---- .../skiros2_skill/core/skill_instanciator.py | 16 +- .../src/skiros2_skill/core/skill_utils.py | 106 +++--- .../src/skiros2_skill/core/visitors.py | 82 ++--- .../skiros2_skill/core/visitors_optimizer.py | 158 ++++----- .../skiros2_skill/ros/discovery_interface.py | 6 +- .../ros/skill_layer_interface.py | 13 +- .../src/skiros2_skill/ros/skill_manager.py | 54 +-- .../ros/skill_manager_interface.py | 16 +- skiros2_skill/src/skiros2_skill/ros/utils.py | 8 +- skiros2_task/nodes/test_domain | 2 +- skiros2_task/setup.py | 2 +- .../src/skiros2_task/core/pddl_interface.py | 75 +++-- .../src/skiros2_task/ros/task_manager.py | 63 ++-- .../ros/task_manager_interface.py | 6 +- skiros2_world_model/nodes/utils/edit_scene | 30 +- skiros2_world_model/nodes/utils/simple_query | 8 +- skiros2_world_model/nodes/utils/view_scene | 11 +- skiros2_world_model/setup.py | 2 +- .../core/local_world_model.py | 313 +++++++++--------- .../core/ontology_rdflib.py | 23 +- .../skiros2_world_model/core/world_model.py | 75 +++-- .../core/world_model_abstract_interface.py | 3 + .../ros/ontology_interface.py | 45 +-- .../ros/ontology_server.py | 10 +- .../ros/world_model_interface.py | 113 ++++--- .../ros/world_model_server.py | 41 +-- 50 files changed, 1307 insertions(+), 1131 deletions(-) diff --git a/skiros2_common/setup.py b/skiros2_common/setup.py index 5701bf5..335fd92 100644 --- a/skiros2_common/setup.py +++ b/skiros2_common/setup.py @@ -1,4 +1,4 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup diff --git a/skiros2_common/src/skiros2_common/core/abstract_skill.py b/skiros2_common/src/skiros2_common/core/abstract_skill.py index 184b6a9..63f3f66 100644 --- a/skiros2_common/src/skiros2_common/core/abstract_skill.py +++ b/skiros2_common/src/skiros2_common/core/abstract_skill.py @@ -22,22 +22,24 @@ """ ParamOptions = Enum('ParamOptions', 'Consume Output Unspecify Lock RespectType') + class SkillDescription(object): """ An abstract skill description """ + def __init__(self): - #Description - self._type= "skiros:" + self.__class__.__name__ - #Params - self._params=params.ParamHandler() - #Conditions - self._pre_conditions=[] - self._hold_conditions=[] - self._post_conditions=[] + # Description + self._type = "skiros:" + self.__class__.__name__ + # Params + self._params = params.ParamHandler() + # Conditions + self._pre_conditions = [] + self._hold_conditions = [] + self._post_conditions = [] self.createDescription() self.generateDefParams() - #self.generateDefConditions() + # self.generateDefConditions() @property def params(self): @@ -78,18 +80,18 @@ def addParam(self, key, value, param_type, options=[], description=""): description: an optional verbose description """ self._params.addParam(key, value, param_type, description) - if type(value) == type(Element()): + if isinstance(value, type(Element())): for o in options: if o == ParamOptions.Consume: - self._post_conditions += [self.getGenerateCond("Consume"+key, key, False)] + self._post_conditions += [self.getGenerateCond("Consume" + key, key, False)] elif o == ParamOptions.Unspecify: - self._post_conditions += [self.getIsSpecifiedCond("Unset"+key, key, False)] + self._post_conditions += [self.getIsSpecifiedCond("Unset" + key, key, False)] elif o == ParamOptions.Lock: - self._pre_conditions += [self.getPropCond(key+'Idle', "skiros:StateProperty", key, "=", "Idle", True)] - self._hold_conditions += [self.getPropCond(key+'Busy', "skiros:StateProperty", key, "=", "Idle", False)] - self._post_conditions += [self.getPropCond(key+'Idle', "skiros:StateProperty", key, "=", "Idle", True)] + self._pre_conditions += [self.getPropCond(key + 'Idle', "skiros:StateProperty", key, "=", "Idle", True)] + self._hold_conditions += [self.getPropCond(key + 'Busy', "skiros:StateProperty", key, "=", "Idle", False)] + self._post_conditions += [self.getPropCond(key + 'Idle', "skiros:StateProperty", key, "=", "Idle", True)] elif o == ParamOptions.RespectType: - self._pre_conditions.append(self.getOnTypeCond(key+'OfType', key, self.params[key].default.type)) + self._pre_conditions.append(self.getOnTypeCond(key + 'OfType', key, self.params[key].default.type)) def generateDefParams(self): """ @@ -97,7 +99,7 @@ def generateDefParams(self): """ if not self._params.hasParam('Robot'): self._params.addParam("Robot", Element("sumo:Agent"), params.ParamTypes.Required) - #if not self._params.hasParam('Skill'): + # if not self._params.hasParam('Skill'): # self._params.addParam("Skill", self.toElement(), params.ParamTypes.Required) def generateDefConditions(self): @@ -105,16 +107,17 @@ def generateDefConditions(self): Some default preconditions are added automatically """ #self.addPreCondition(self.getRelationCond("HasSkill", "hasSkill", "Robot", "Skill", True)) - #for key, param in self._params.getParamMapFiltered(params.ParamTypes.Hardware).iteritems(): + # for key, param in self._params.getParamMapFiltered(params.ParamTypes.Hardware).iteritems(): # self.addPreCondition(self.getPropCond("DeviceIdle", "deviceState", key, "Idle", True)) for key, param in self._params.getParamMapFiltered(params.ParamTypes.Optional).iteritems(): - if param.dataType() == type(Element()): - c1 = self.getGenerateCond("Has"+key, key, True) + if isinstance(Element(), param.dataType()): + c1 = self.getGenerateCond("Has" + key, key, True) dont_add = False for c2 in self._post_conditions: if c1.isEqual(c2) or c1.hasConflict(c2): dont_add = True - if not dont_add: self._post_conditions = [c1] + self._post_conditions + if not dont_add: + self._post_conditions = [c1] + self._post_conditions return True def getOutputParams(self): @@ -156,7 +159,7 @@ def getOnTypeCond(self, clabel, subj, value): def getModifiedParams(self): param_list = set([]) for c in self._post_conditions: - #if isinstance(c, cond.ConditionGenerate): + # if isinstance(c, cond.ConditionGenerate): param_list = param_list.union(set(c.getKeys())) return param_list @@ -196,6 +199,7 @@ def createDescription(self): """ Optional, Not implemented in abstract class. """ return + class SkillCore(SkillDescription): """ @brief An abstract executable skill with a description (type, label, params, conditions), a state and progress code @@ -204,25 +208,26 @@ class SkillCore(SkillDescription): gen_id = IdGen() def __init__(self): - #Description - self._id=SkillCore.gen_id.getId() - self._type="" - self._label="" + # Description + self._id = SkillCore.gen_id.getId() + self._type = "" + self._label = "" self._description = SkillDescription() - #Params - self._params=params.ParamHandler() - #Conditions - self._pre_conditions=[] - self._hold_conditions=[] - self._post_conditions=[] - #Execution + # Params + self._params = params.ParamHandler() + # Conditions + self._pre_conditions = [] + self._hold_conditions = [] + self._post_conditions = [] + # Execution self._state_change = Event() - self._state=State.Uninitialized + self._state = State.Uninitialized self._time_keeper = TimeKeeper() - self._progress_code=0 - self._progress_time=0.0 - self._progress_msg="" + self._progress_code = 0 + self._progress_time = 0.0 + self._progress_msg = "" #--------Class functions-------- + def expand(self, skill): return @@ -234,11 +239,11 @@ def _setState(self, state): self._state_change.set() def _setProgress(self, msg, code=None): - if code==None: - code = self._progress_code+1 - self._progress_code=code - self._progress_time=self._time_keeper.time_from_start() - self._progress_msg=msg + if code is None: + code = self._progress_code + 1 + self._progress_code = code + self._progress_time = self._time_keeper.time_from_start() + self._progress_msg = msg @property def id(self): @@ -311,6 +316,7 @@ def checkPostCond(self, verbose=False): to_ret += c.getKeys() return to_ret #-------- Control functions-------- + def preempt(self): if self.hasState(State.Running): self._setState(self.onPreempt()) @@ -323,13 +329,13 @@ def hasState(self, state): return self._state == state def waitState(self, state, isset=True): - if isset:#Xor? - while self._state!=state: + if isset: # Xor? + while self._state != state: #print 'Waiting set.. {}'.format(self._state) self._state_change.clear() self._state_change.wait() else: - while self._state==state: + while self._state == state: #print 'Waiting not set.. {}'.format(self._state) self._state_change.clear() self._state_change.wait() @@ -355,7 +361,7 @@ def start(self, params=None): return self._state def printInfo(self, verbose=False): - s = "{}-{} ".format(self._type,self._label) + s = "{}-{} ".format(self._type, self._label) if verbose: s += "[" s += self._params.printState() + ']\n' @@ -365,7 +371,7 @@ def printInfo(self, verbose=False): return s def printState(self, verbose=False): - s = "{}-{}({})".format(self.type[self.type.find(":")+1:], self.label, self.state) + s = "{}-{}({})".format(self.type[self.type.find(":") + 1:], self.label, self.state) if verbose: s += "[{}]".format(self.params.printState()) return s @@ -412,11 +418,10 @@ def setDescription(self, description, label=""): """ self._description = description self._type = description._type - if label!="": + if label != "": self._label = label self._resetDescription() - #-------- Virtual functions-------- def modifyDescription(self, skill): """ diff --git a/skiros2_common/src/skiros2_common/core/conditions.py b/skiros2_common/src/skiros2_common/core/conditions.py index 43331b2..0fb2ff0 100644 --- a/skiros2_common/src/skiros2_common/core/conditions.py +++ b/skiros2_common/src/skiros2_common/core/conditions.py @@ -4,19 +4,21 @@ import operator operators = {'>': operator.gt, - '<': operator.lt, - '>=': operator.ge, - '<=': operator.le, - '=': operator.eq} + '<': operator.lt, + '>=': operator.ge, + '<=': operator.le, + '=': operator.eq} + class ConditionBase(object): """ """ + def __init__(self, clabel, subj, desired_state): - self._desired_state=desired_state - self._subject_key=subj - self._label=clabel - self._description="" + self._desired_state = desired_state + self._subject_key = subj + self._label = clabel + self._description = "" def __eq__(self, other): if self.isEqual(other): @@ -31,8 +33,8 @@ def __ne__(self, other): return True def remap(self, initial_key, target_key): - if self._subject_key==initial_key: - self._subject_key=target_key + if self._subject_key == initial_key: + self._subject_key = target_key self._setDescription() #print "New description: " + self.getDescription() @@ -48,22 +50,27 @@ def getKeys(self): def getDescription(self): return self._description - #Virtual functions + # Virtual functions def _setDescription(self): """ Not implemented in abstract class. """ raise NotImplementedError("Not implemented in abstract class") + def evaluate(self, ph, wmi): """ Not implemented in abstract class. """ raise NotImplementedError("Not implemented in abstract class") + def setTrue(self, ph, wmi): """ Not implemented in abstract class. """ raise NotImplementedError("Not implemented in abstract class") + def revert(self): """ Not implemented in abstract class. """ raise NotImplementedError("Not implemented in abstract class") + def setDesiredState(self, ph): """ Used to resolve the element in the world model. """ raise NotImplementedError("Not implemented in abstract class") + def isEqual(self, other): """ Equality function. """ raise NotImplementedError("Not implemented in abstract class") @@ -72,10 +79,11 @@ def toElement(self): """ World model representation . """ raise NotImplementedError("Not implemented in abstract class") + class ConditionOr(ConditionBase): def __init__(self, desired_state): - self._desired_state=desired_state - self._label="or" + self._desired_state = desired_state + self._label = "or" self._params = None self._children = list() self._setDescription() @@ -104,7 +112,7 @@ def isEqual(self, other): def hasConflict(self, other): if isinstance(other, ConditionOr): - if self._desired_state!=other._desired_state: + if self._desired_state != other._desired_state: raise Exception("TODO") return True return False @@ -148,6 +156,7 @@ def toElement(self): to_ret.setProperty("skiros:desiredState", self._desired_state) return to_ret + class ConditionProperty(ConditionBase): """ @brief Condition over an element property @@ -166,45 +175,46 @@ class ConditionProperty(ConditionBase): >>> majorMinusOne.evaluate(ph, None) True """ + def __init__(self, clabel, olabel, subj, operator, value, desired_state): - self._desired_state=desired_state - self._subject_key=subj - self._value=value - self._label=clabel - self._operator=operator - self._owl_label=olabel + self._desired_state = desired_state + self._subject_key = subj + self._value = value + self._label = clabel + self._operator = operator + self._owl_label = olabel self._params = None self._setDescription() def isEqual(self, other): if isinstance(other, ConditionProperty): - return self._subject_key==other._subject_key and self._operator==other._operator and self._owl_label==other._owl_label and self._value==other._value and self._desired_state==other._desired_state + return self._subject_key == other._subject_key and self._operator == other._operator and self._owl_label == other._owl_label and self._value == other._value and self._desired_state == other._desired_state else: return False def hasConflict(self, other): if isinstance(other, ConditionProperty): - if self._owl_label==other._owl_label and self._value==other._value and self._desired_state!=other._desired_state: - return self.getParamId(self._subject_key)==other.getParamId(other._subject_key) or self._subject_key==other._subject_key + if self._owl_label == other._owl_label and self._value == other._value and self._desired_state != other._desired_state: + return self.getParamId(self._subject_key) == other.getParamId(other._subject_key) or self._subject_key == other._subject_key return False def _setDescription(self): self._description = "[{}] {}-{}-{} ({})".format(self._label, - self._subject_key, - self._owl_label, - self._value, - self._desired_state) + self._subject_key, + self._owl_label, + self._value, + self._desired_state) def evaluate(self, ph, wmi): self._params = ph self._wm = wmi subj = self._params.getParamValue(self._subject_key) - if self._operator=="=": + if self._operator == "=": return subj.hasProperty(self._owl_label, self._value) == self._desired_state else: if not subj.hasProperty(self._owl_label): return False - return operators[self._operator](subj.getProperty(self._owl_label).value, self._value)==self._desired_state + return operators[self._operator](subj.getProperty(self._owl_label).value, self._value) == self._desired_state def setTrue(self, ph, wmi): self._params = ph @@ -237,7 +247,8 @@ def revert(self, ph, wmi): def setDesiredState(self, ph): e = ph.getParamValue(self._subject_key) - if e.getIdNumber()>=0: return + if e.getIdNumber() >= 0: + return if self._desired_state: if not e.hasProperty(self._owl_label, self._value): e.appendProperty(self._owl_label, self._value) @@ -257,19 +268,19 @@ def toElement(self): class ConditionRelation(ConditionBase): def __init__(self, clabel, olabel, subj, obj, desired_state): - self._desired_state=desired_state - self._subject_key=subj - self._object_key=obj - self._label=clabel - self._owl_label=olabel + self._desired_state = desired_state + self._subject_key = subj + self._object_key = obj + self._label = clabel + self._owl_label = olabel self._params = None self._setDescription() def remap(self, initial_key, target_key): - if self._subject_key==initial_key: - self._subject_key=target_key - elif self._object_key==initial_key: - self._object_key=target_key + if self._subject_key == initial_key: + self._subject_key = target_key + elif self._object_key == initial_key: + self._object_key = target_key self._setDescription() def getKeys(self): @@ -277,23 +288,24 @@ def getKeys(self): def isEqual(self, other): if isinstance(other, ConditionRelation): - return self._subject_key==other._subject_key and self._owl_label==other._owl_label and self._object_key==other._object_key and self._desired_state==other._desired_state + return self._subject_key == other._subject_key and self._owl_label == other._owl_label and self._object_key == other._object_key and self._desired_state == other._desired_state else: return False def hasConflict(self, other): if isinstance(other, ConditionRelation): - if self._owl_label==other._owl_label and self._desired_state!=other._desired_state: + if self._owl_label == other._owl_label and self._desired_state != other._desired_state: #print "{}=={} and {}=={}".format(self.getParamId(self._subject_key), other.getParamId(other._subject_key), self.getParamId(self._object_key), other.getParamId(other._object_key)) - return (self.getParamId(self._subject_key)==other.getParamId(other._subject_key) and self.getParamId(self._object_key)==other.getParamId(other._object_key)) or (self._subject_key==other._subject_key and self._object_key==other._object_key) + return (self.getParamId(self._subject_key) == other.getParamId(other._subject_key) and self.getParamId(self._object_key) == + other.getParamId(other._object_key)) or (self._subject_key == other._subject_key and self._object_key == other._object_key) return False def _setDescription(self): self._description = "[{}] {}-{}-{} ({})".format(self._label, - self._subject_key, - self._owl_label, - self._object_key, - self._desired_state) + self._subject_key, + self._owl_label, + self._object_key, + self._desired_state) def evaluate(self, ph, wmi): self._params = ph @@ -301,17 +313,17 @@ def evaluate(self, ph, wmi): subj = self._params.getParamValue(self._subject_key) obj = self._params.getParamValue(self._object_key) additional = "" - if subj.getIdNumber()<0: + if subj.getIdNumber() < 0: additional += ". ?x rdf:type {}".format(subj.type) subj = "?x" else: subj = subj.id - if obj.getIdNumber()<0: + if obj.getIdNumber() < 0: additional += ". ?y rdf:type {}".format(obj.type) obj = "?y" else: obj = obj.id - if additional=="": + if additional == "": v = self._wm.get_relations(subj, self._owl_label, obj) else: v = self._wm.query_ontology("SELECT * WHERE {" + "{} {} {}".format(subj, self._owl_label, obj) + additional + ".}") @@ -353,10 +365,10 @@ def revert(self, ph, wmi): def setDesiredState(self, ph): subj = ph.getParamValue(self._subject_key) obj = ph.getParamValue(self._object_key) - if subj.getIdNumber()<0: + if subj.getIdNumber() < 0: if not subj.hasRelation("-1", self._owl_label, self._object_key, self._desired_state): subj.addRelation("-1", self._owl_label, self._object_key, self._desired_state) - elif obj.getIdNumber()<0: + elif obj.getIdNumber() < 0: if not obj.hasRelation("-1", self._owl_label, self._object_key, self._desired_state): obj.addRelation(self._subject_key, self._owl_label, "-1", self._desired_state) @@ -371,19 +383,19 @@ def toElement(self): class AbstractConditionRelation(ConditionBase): def __init__(self, clabel, olabel, subj, obj, desired_state): - self._desired_state=desired_state - self._subject_key=subj - self._object_key=obj - self._label=clabel - self._owl_label=olabel + self._desired_state = desired_state + self._subject_key = subj + self._object_key = obj + self._label = clabel + self._owl_label = olabel self._params = None self._setDescription() def remap(self, initial_key, target_key): - if self._subject_key==initial_key: - self._subject_key=target_key - elif self._object_key==initial_key: - self._object_key=target_key + if self._subject_key == initial_key: + self._subject_key = target_key + elif self._object_key == initial_key: + self._object_key = target_key self._setDescription() def getKeys(self): @@ -391,23 +403,24 @@ def getKeys(self): def isEqual(self, other): if isinstance(other, ConditionRelation): - return self._subject_key==other._subject_key and self._owl_label==other._owl_label and self._object_key==other._object_key and self._desired_state==other._desired_state + return self._subject_key == other._subject_key and self._owl_label == other._owl_label and self._object_key == other._object_key and self._desired_state == other._desired_state else: return False def hasConflict(self, other): if isinstance(other, ConditionRelation): - if self._owl_label==other._owl_label and self._desired_state!=other._desired_state: + if self._owl_label == other._owl_label and self._desired_state != other._desired_state: #print "{}=={} and {}=={}".format(self.getParamId(self._subject_key), other.getParamId(other._subject_key), self.getParamId(self._object_key), other.getParamId(other._object_key)) - return (self.getParamId(self._subject_key)==other.getParamId(other._subject_key) and self.getParamId(self._object_key)==other.getParamId(other._object_key)) or (self._subject_key==other._subject_key and self._object_key==other._object_key) + return (self.getParamId(self._subject_key) == other.getParamId(other._subject_key) and self.getParamId(self._object_key) == + other.getParamId(other._object_key)) or (self._subject_key == other._subject_key and self._object_key == other._object_key) return False def _setDescription(self): self._description = "[{}] {}-{}-{} ({})".format(self._label, - self._subject_key, - self._owl_label, - self._object_key, - self._desired_state) + self._subject_key, + self._owl_label, + self._object_key, + self._desired_state) def evaluate(self, ph, wmi): self._params = ph @@ -438,10 +451,10 @@ def revert(self, ph, wmi): def setDesiredState(self, ph): subj = ph.getParamValue(self._subject_key) obj = ph.getParamValue(self._object_key) - if subj.getIdNumber()<0: + if subj.getIdNumber() < 0: if not subj.hasRelation("-1", self._owl_label, self._object_key, self._desired_state, abstract=True): subj.addRelation("-1", self._owl_label, self._object_key, self._desired_state, abstract=True) - if obj.getIdNumber()<0: + if obj.getIdNumber() < 0: if not obj.hasRelation("-1", self._owl_label, self._object_key, self._desired_state, abstract=True): obj.addRelation(self._subject_key, self._owl_label, "-1", self._desired_state, abstract=True) @@ -453,32 +466,33 @@ def toElement(self): to_ret.setProperty("skiros:desiredState", self._desired_state) return to_ret + class ConditionHasProperty(ConditionBase): def __init__(self, clabel, olabel, subj, desired_state): - self._desired_state=desired_state - self._subject_key=subj - self._label=clabel - self._owl_label=olabel + self._desired_state = desired_state + self._subject_key = subj + self._label = clabel + self._owl_label = olabel self._params = None self._setDescription() def isEqual(self, other): if isinstance(other, ConditionHasProperty): - return self._subject_key==other._subject_key and self._owl_label==other._owl_label and self._desired_state==other._desired_state + return self._subject_key == other._subject_key and self._owl_label == other._owl_label and self._desired_state == other._desired_state else: return False def hasConflict(self, other): if isinstance(other, ConditionHasProperty): - if self._owl_label==other._owl_label and self._desired_state!=other._desired_state: - return self.getParamId(self._subject_key)==other.getParamId(other._subject_key) or self._subject_key==other._subject_key + if self._owl_label == other._owl_label and self._desired_state != other._desired_state: + return self.getParamId(self._subject_key) == other.getParamId(other._subject_key) or self._subject_key == other._subject_key return False def _setDescription(self): self._description = "[{}] {}-{} ({})".format(self._label, - self._subject_key, - self._owl_label, - self._desired_state) + self._subject_key, + self._owl_label, + self._desired_state) def evaluate(self, ph, wmi): self._params = ph @@ -486,7 +500,7 @@ def evaluate(self, ph, wmi): subj = self._params.getParamValue(self._subject_key) #print self._description + "\n{}".format(subj.printState(True)) if subj.getIdNumber() < 0: - if self._params.getParam(self._subject_key).paramType==params.ParamTypes.Optional:#If optional return true, else return false + if self._params.getParam(self._subject_key).paramType == params.ParamTypes.Optional: # If optional return true, else return false return True else: return False @@ -528,7 +542,8 @@ def revert(self, ph, wmi): def setDesiredState(self, ph): e = ph.getParamValue(self._subject_key) - if e.getIdNumber()>=0: return + if e.getIdNumber() >= 0: + return if self._desired_state: if not e.hasProperty(self._owl_label): e.setProperty(self._owl_label, "") @@ -543,30 +558,31 @@ def toElement(self): to_ret.setProperty("skiros:desiredState", self._desired_state) return to_ret + class ConditionIsSpecified(ConditionBase): def __init__(self, clabel, subj, desired_state): - self._subject_key=subj - self._label=clabel - self._desired_state=desired_state + self._subject_key = subj + self._label = clabel + self._desired_state = desired_state self._params = None self._setDescription() def isEqual(self, other): if isinstance(other, ConditionIsSpecified): - return self._subject_key==other._subject_key and self._desired_state==other._desired_state + return self._subject_key == other._subject_key and self._desired_state == other._desired_state else: return False def hasConflict(self, other): if isinstance(other, ConditionIsSpecified): - return self._subject_key==other._subject_key and self._desired_state!=other._desired_state + return self._subject_key == other._subject_key and self._desired_state != other._desired_state else: return False def _setDescription(self): self._description = "[{}] {} ({})".format(self._label, - self._subject_key, - self._desired_state) + self._subject_key, + self._desired_state) def evaluate(self, ph, wmi): self._params = ph @@ -616,31 +632,32 @@ def toElement(self): to_ret.setProperty("skiros:desiredState", self._desired_state) return to_ret + class ConditionGenerate(ConditionBase): - #TODO: understand: any difference with isSpecified? + # TODO: understand: any difference with isSpecified? def __init__(self, clabel, subj, desired_state): - self._subject_key=subj - self._label=clabel - self._desired_state=desired_state + self._subject_key = subj + self._label = clabel + self._desired_state = desired_state self._params = None self._setDescription() def isEqual(self, other): if isinstance(other, ConditionGenerate): - return self._subject_key==other._subject_key and self._desired_state==other._desired_state + return self._subject_key == other._subject_key and self._desired_state == other._desired_state else: return False def hasConflict(self, other): if isinstance(other, ConditionGenerate): - return self._subject_key==other._subject_key and self._desired_state!=other._desired_state + return self._subject_key == other._subject_key and self._desired_state != other._desired_state else: return False def _setDescription(self): self._description = "[{}] {} ({})".format(self._label, - self._subject_key, - self._desired_state) + self._subject_key, + self._desired_state) def evaluate(self, ph, wmi): self._params = ph @@ -680,9 +697,9 @@ def revert(self, ph, wmi): self._params = ph self._wm = wmi #print self._description + " {}".format(self._cache_new.printState()) - if self._cache_new.getIdNumber()>=0 and self._cache.getIdNumber()<0: + if self._cache_new.getIdNumber() >= 0 and self._cache.getIdNumber() < 0: self._wm.remove_element(self._cache_new._id) - elif self._cache_new.getIdNumber()<0 and self._cache.getIdNumber()>=0: + elif self._cache_new.getIdNumber() < 0 and self._cache.getIdNumber() >= 0: self._wm.add_element(self._cache, ":Scene-0", "contain") self._params.specify(self._subject_key, self._cache) self._has_cache = False @@ -701,28 +718,28 @@ def toElement(self): class ConditionOnType(ConditionBase): def __init__(self, clabel, subj, value): - self._subject_key=subj - self._label=clabel - self._value=value + self._subject_key = subj + self._label = clabel + self._value = value self._params = None self._setDescription() def isEqual(self, other): if isinstance(other, ConditionOnType): - return self._subject_key==other._subject_key and self._value==other._value + return self._subject_key == other._subject_key and self._value == other._value else: return False def hasConflict(self, other): if isinstance(other, ConditionOnType): - return self._subject_key==other._subject_key + return self._subject_key == other._subject_key else: return False def _setDescription(self): self._description = "[{}] {} is of type {}".format(self._label, - self._subject_key, - self._value) + self._subject_key, + self._value) def evaluate(self, ph, wmi): self._params = ph @@ -739,7 +756,7 @@ def revert(self, ph, wmi): def setDesiredState(self, ph): e = ph.getParamValue(self._subject_key) - if e.id>=0: + if e.id >= 0: return else: e._type = self._value @@ -748,4 +765,4 @@ def toElement(self): to_ret = Element("skiros:" + self.__class__.__name__, self._label) to_ret.setProperty("skiros:hasSubject", self._subject_key) to_ret.setProperty("skiros:desiredValue", self._value) - return to_ret \ No newline at end of file + return to_ret diff --git a/skiros2_common/src/skiros2_common/core/discrete_reasoner.py b/skiros2_common/src/skiros2_common/core/discrete_reasoner.py index fe8acea..31b580f 100644 --- a/skiros2_common/src/skiros2_common/core/discrete_reasoner.py +++ b/skiros2_common/src/skiros2_common/core/discrete_reasoner.py @@ -30,6 +30,7 @@ from multiprocessing.dummy import Process + class DiscreteReasoner(object): def init(self, wmi): """ diff --git a/skiros2_common/src/skiros2_common/core/params.py b/skiros2_common/src/skiros2_common/core/params.py index 6a1f12d..c2a1b54 100644 --- a/skiros2_common/src/skiros2_common/core/params.py +++ b/skiros2_common/src/skiros2_common/core/params.py @@ -11,6 +11,7 @@ """ ParamTypes = Enum('ParamTypes', 'Required Optional') + class Param(Property): """ @brief A param is a property with addionally: @@ -29,23 +30,24 @@ class Param(Property): """ __slots__ = ['_key', '_description', '_param_type', '_data_type', '_is_list', '_values', '_default', '_last_update'] + def __init__(self, key, description, value, param_type, is_list=False): self._is_list = is_list - self._key=key - self._description=description + self._key = key + self._description = description self._setLastUpdate() if isinstance(param_type, int): - self._param_type=ParamTypes(param_type+1) + self._param_type = ParamTypes(param_type + 1) else: - self._param_type=param_type + self._param_type = param_type if isinstance(value, list): self._values = value self._data_type = type(value[0]) elif isinstance(value, type): - self._data_type=value + self._data_type = value self._values = list() else: - self._data_type=type(value) + self._data_type = type(value) self._values = [value] self._default = deepcopy(self._values) @@ -84,7 +86,7 @@ def hasChanges(self, time, tolerance=1): @time a datetime @tolerance the tolerance in microseconds """ - if (self._last_update-time)>timedelta(microseconds=tolerance): + if (self._last_update - time) > timedelta(microseconds=tolerance): return True return False @@ -120,7 +122,7 @@ def hasSpecifiedDefault(self): """ @brief Check if the current parameter has default values specified """ - return len(self._default)>0 + return len(self._default) > 0 def getDefaultValue(self, index=0): return self._default[index] @@ -133,7 +135,8 @@ def hasDefaultValues(self): @brief Check if the current parameter value is the default """ for v1, v2 in zip(self._default, self._values): - if v1!=v2: return False + if v1 != v2: + return False return True def makeDefault(self, values): @@ -163,7 +166,7 @@ def paramTypeIs(self, ptype): def toElement(self): to_ret = Element("skiros:Parameter", self._key) to_ret.setProperty("rdfs:comment", self._description) - to_ret.setProperty("skiros:ParameterType", int(self._param_type)-1) + to_ret.setProperty("skiros:ParameterType", int(self._param_type) - 1) if(not self.dataTypeIs(Element)): to_ret.setProperty("skiros:DataType", self._data_type) if self.hasSpecifiedDefault(): @@ -174,10 +177,11 @@ def toElement(self): to_ret.setProperty("skiros:DataType", self._default[0]._type) if self.isSpecified(): for v in self._values: - if v._id!="": + if v._id != "": to_ret.addRelation("-1", "skiros:hasValue", v._id) return to_ret + class ParamHandler(object): """ >>> ph = ParamHandler() @@ -193,10 +197,11 @@ class ParamHandler(object): """ __slots__ = ['_params'] + def __init__(self, params=None): - self._params={} + self._params = {} if params: - self._params=params + self._params = params def __getitem__(self, key): if self.hasParam(key): @@ -214,7 +219,7 @@ def iteritems(self): return self._params.iteritems() def reset(self, copy): - self._params=copy + self._params = copy def getCopy(self): return deepcopy(self._params) @@ -229,12 +234,12 @@ def merge(self, other): to_ret = self.getCopy() for key, param in other._params.iteritems(): if self.hasParam(key): - if not param.isSpecified():#If not specified (None) parameter is removed + if not param.isSpecified(): # If not specified (None) parameter is removed del to_ret[key] else: to_ret[key].setValues(param.getValues()) else: - if param.isSpecified():#Only if specified (not None) parameter is added + if param.isSpecified(): # Only if specified (not None) parameter is added to_ret[key] = param return to_ret @@ -259,7 +264,7 @@ def specifyParams(self, other, keep_default=False): t = self._params[key] if keep_default and t.hasSpecifiedDefault(): if t.dataTypeIs(Element): - if t.getDefaultValue().getIdNumber()<0 or t.getDefaultValue().getIdNumber()==param.value.getIdNumber(): + if t.getDefaultValue().getIdNumber() < 0 or t.getDefaultValue().getIdNumber() == param.value.getIdNumber(): t.values = param.values else: t.values = param.values @@ -276,7 +281,7 @@ def hasParam(self, key): """ @brief Check that a key exists and return false otherwise """ - return self._params.has_key(key) + return key in self._params def setDefault(self, key=None): """ diff --git a/skiros2_common/src/skiros2_common/core/primitive.py b/skiros2_common/src/skiros2_common/core/primitive.py index 6631d2d..b8a7e55 100644 --- a/skiros2_common/src/skiros2_common/core/primitive.py +++ b/skiros2_common/src/skiros2_common/core/primitive.py @@ -3,11 +3,13 @@ from skiros2_common.core.world_element import Element from datetime import datetime + class PrimitiveBase(SkillCore): """ @brief Base class for primitive skills """ #--------Control functions-------- + def tick(self): if self.hasState(State.Success) or self.hasState(State.Failure): log.error("tick", "Reset required before ticking.") diff --git a/skiros2_common/src/skiros2_common/core/property.py b/skiros2_common/src/skiros2_common/core/property.py index 8f9efe5..9ff33f6 100644 --- a/skiros2_common/src/skiros2_common/core/property.py +++ b/skiros2_common/src/skiros2_common/core/property.py @@ -1,6 +1,7 @@ import skiros2_common.tools.logger as log import ast + class Property(object): """ @brief Touple key-value with datatype check @@ -18,7 +19,7 @@ def __init__(self, key, value, is_list=False): Value can also be a type, in such a case the _data_type is set and the value list is left empty """ self._is_list = is_list - self._key=key + self._key = key if isinstance(value, list): self._values = value self._data_type = type(value[0]) @@ -27,13 +28,13 @@ def __init__(self, key, value, is_list=False): self._data_type = value else: self._values = [value] - self._data_type= type(value) + self._data_type = type(value) def isSpecified(self): """ @brief Return true if the property has at least one value specified """ - return len(self._values)>0 + return len(self._values) > 0 def isList(self): """ @@ -86,7 +87,7 @@ def dataTypeIs(self, vtype): if isinstance(vtype, type): return self._data_type == vtype else: - return self._data_type == type(vtype) + return isinstance(vtype, self._data_type) def setValue(self, value, index=0): """ @@ -115,7 +116,7 @@ def setValues(self, value): @brief Set all the values """ if isinstance(value, list): - if len(value)==0: + if len(value) == 0: self._values = list() return if isinstance(value[0], self._data_type): @@ -134,7 +135,7 @@ def removeValue(self, value): @brief Removes the first value matching. Does nothing if value is not present """ i = self.find(value) - if i>=0: + if i >= 0: del self._values[i] def find(self, value): @@ -153,7 +154,7 @@ def addValue(self, value): if isinstance(value, self._data_type): self._values.append(value) else: - log.error("append", self._key+": "+str(type(value))+"!="+str(self._data_type)) + log.error("append", self._key + ": " + str(type(value)) + "!=" + str(self._data_type)) return def getValue(self, index=0): @@ -182,4 +183,4 @@ def printState(self): """ v = str(self._values) max_lenght = 500 - return "{}:{}".format(self._key, v) if len(v)>> e.getProperty("Hello").values [2.0] """ - __slots__ = ['_last_update', '_type', '_label', '_id', '_properties', '_local_relations','_relations'] + __slots__ = ['_last_update', '_type', '_label', '_id', '_properties', '_local_relations', '_relations'] _plug_loader = None _property_reasoner_map = None def printState(self, verbose=False, filter=""): - if self._id=="": + if self._id == "": to_ret = self._type else: to_ret = self._id - to_ret = to_ret + "-" + self._label #+ self._properties + to_ret = to_ret + "-" + self._label # + self._properties if verbose: for k, p in self._properties.iteritems(): - if k==filter or filter=="": + if k == filter or filter == "": to_ret += "\n" + p.printState() for r in self._relations: to_ret += "\n" + str(r) @@ -46,18 +47,17 @@ def printState(self, verbose=False, filter=""): def __init__(self, etype="Unknown", elabel="", eid=""): # Description - self._type=etype - self._label=elabel - self._id=eid - self._properties=dict() - self._local_relations=list() #Reference to Elements - self._relations=list() #Reference to IDs + self._type = etype + self._label = elabel + self._id = eid + self._properties = dict() + self._local_relations = list() # Reference to Elements + self._relations = list() # Reference to IDs self._setLastUpdate() def __repr__(self): return self.printState() - @property def id(self): return self._id @@ -89,7 +89,7 @@ def isAbstract(self): """ @brief Return True if the element is abstract, or False if it is instanciated in the world model """ - return self.id=="" + return self.id == "" def _setLastUpdate(self): self._last_update = datetime.now() @@ -100,7 +100,7 @@ def getLastUpdate(self): def _initPluginLoader(self): Element._plug_loader = PluginLoader() Element._property_reasoner_map = {} - #TODO: remove dependency from ROSpy + # TODO: remove dependency from ROSpy for package in rospy.get_param('wm/reasoners_pkgs', []): Element._plug_loader.load(package, DiscreteReasoner) for plugin in Element._plug_loader: @@ -117,7 +117,7 @@ def _getReasoner(self, get_code): """ if not isinstance(Element._plug_loader, PluginLoader): self._initPluginLoader() - if not Element._property_reasoner_map.has_key(get_code): + if get_code not in Element._property_reasoner_map: raise KeyError("No reasoner associated to data {}. Debug: {}".format(get_code, Element._property_reasoner_map)) return Element._property_reasoner_map[get_code] @@ -125,7 +125,7 @@ def getIdNumber(self): """ @brief Return the element id number as integer """ - if self._id.find('-')<0: + if self._id.find('-') < 0: return -1 return int(self._id.split('-')[1]) @@ -159,7 +159,6 @@ def getData(self, get_code): """ return self._getReasoner(get_code).getData(self, get_code) - def setData(self, set_code, data): """ @brief Set data using a reasoner @@ -197,7 +196,7 @@ def removeRelation2(self, subj, predicate, obj, value=True, abstract=False): """ try: self._relations.remove({'src': subj, 'type': predicate, 'dst': obj, 'state': value, 'abstract': abstract}) - except: + except BaseException: log.error("[removeRelation2]", "Can t remove {} from {}".format({'src': subj, 'type': predicate, 'dst': obj, 'state': value, 'abstract': abstract}, self._relations)) def getRelations(self, subj="", pred=[], obj=""): @@ -208,10 +207,10 @@ def getRelations(self, subj="", pred=[], obj=""): if isinstance(pred, str): pred = [pred] if pred: - if pred[0]=="": + if pred[0] == "": pred = [] for r in self._relations: - if (r['src']==subj or subj=="") and (r['type'] in pred or not pred) and (r['dst']==obj or obj==""): + if (r['src'] == subj or subj == "") and (r['type'] in pred or not pred) and (r['dst'] == obj or obj == ""): to_ret.append(r) return to_ret @@ -219,7 +218,7 @@ def setRelation(self, subj, predicate, obj): """ @brief Set a relation, removing previous definitions """ - self.removeRelations(self.getRelations(subj if subj=="-1" else "", predicate, obj if obj=="-1" else "")) + self.removeRelations(self.getRelations(subj if subj == "-1" else "", predicate, obj if obj == "-1" else "")) self.addRelation(subj, predicate, obj) def addRelation(self, subj, predicate, obj, value=True, abstract=False): @@ -248,9 +247,9 @@ def hasRelation(self, subj, predicate, obj, value=True, abstract=False): @value The state of the relation should be True or False @abstract Whether the relation is between abstract objects or instances """ - if subj=="-1": + if subj == "-1": subj = self.id - elif obj=="-1": + elif obj == "-1": obj = self.id return {'src': subj, 'type': predicate, 'dst': obj, 'state': value, 'abstract': abstract} in self._relations @@ -261,10 +260,10 @@ def hasProperty(self, key, value=None, not_none=False): @value if specified, return true if the property has that value @not_none when set to true, checks that the property doesn't have none as value """ - if not self._properties.has_key(key): + if key not in self._properties: return False if value is not None: - return self.getProperty(key).find(value)!=-1 + return self.getProperty(key).find(value) != -1 return self.getProperty(key).values or not not_none def setProperty(self, key, value, datatype=None, is_list=False, force_convertion=False): @@ -279,21 +278,21 @@ def setProperty(self, key, value, datatype=None, is_list=False, force_convertion old_reasoners = self._properties[key].values if datatype: - if datatype=="xsd:double" or datatype=="xsd:float": + if datatype == "xsd:double" or datatype == "xsd:float": self._properties[key] = Property(key, float, is_list) - if value!=None: + if value is not None: self._properties[key].setValues(value) - elif datatype=="xsd:int" or datatype=="xsd:integer": + elif datatype == "xsd:int" or datatype == "xsd:integer": self._properties[key] = Property(key, int, is_list) - if value!=None: + if value is not None: self._properties[key].setValues(int(value)) - elif datatype=="xsd:boolean": + elif datatype == "xsd:boolean": self._properties[key] = Property(key, bool, is_list) - if value!=None: + if value is not None: self._properties[key].setValues(value) - elif datatype=="xsd:string": + elif datatype == "xsd:string": self._properties[key] = Property(key, str, is_list) - if value!=None: + if value is not None: self._properties[key].setValues(str(value)) else: log.warn("[Element]", "Datatype {} not recognized. Set default".format(datatype)) @@ -304,7 +303,7 @@ def setProperty(self, key, value, datatype=None, is_list=False, force_convertion value = self._properties[key].dataType()(value) self._properties[key].setValues(value) else: - if type(value)==unicode: + if isinstance(value, unicode): value = str(value) self._properties[key] = Property(key, value, is_list) @@ -312,11 +311,10 @@ def setProperty(self, key, value, datatype=None, is_list=False, force_convertion new_reasoners = self._properties[key].values try: [self._getReasoner(r).removeProperties(self) for r in old_reasoners if r not in new_reasoners] - [self._getReasoner(r).addProperties(self) for r in new_reasoners if r not in old_reasoners] - except KeyError, e: + [self._getReasoner(r).addProperties(self) for r in new_reasoners if r not in old_reasoners] + except KeyError as e: log.error("WorldElement", e.message) - def removeProperty(self, key): """ @brief Remove the property @@ -328,7 +326,6 @@ def removeProperty(self, key): del self._properties[key] - def appendProperty(self, key, value): """ @brief Append a value to the property. If property doesn't exist it is created. @@ -357,22 +354,22 @@ def isInstance(self, abstract, wmi): Compare the element to an abstract description Return true if this is a valid instance, false otherwise """ - #Filter by type - if not self._type==abstract._type and not (wmi.add_prefix(self._type) in wmi.get_sub_classes(abstract._type, True)): + # Filter by type + if not self._type == abstract._type and not (wmi.add_prefix(self._type) in wmi.get_sub_classes(abstract._type, True)): return False - #Filter by label - if not (abstract._label=="" or abstract._label=="Unknown" or self._label==abstract._label): + # Filter by label + if not (abstract._label == "" or abstract._label == "Unknown" or self._label == abstract._label): return False - #Filter by properties + # Filter by properties for k, p in abstract._properties.iteritems(): if not self.hasProperty(k): return False for v in p.getValues(): if not v in self.getProperty(k).values: return False - #Filter by relations + # Filter by relations for r in abstract._relations: - if r["src"]=="-1" or r["src"]==abstract._id:#-1 is the special autoreferencial value + if r["src"] == "-1" or r["src"] == abstract._id: # -1 is the special autoreferencial value #print wmi.getRelations(self._id, r["type"], -1) if not wmi.get_relations(self._id, r["type"], "-1"): return False @@ -381,4 +378,3 @@ def isInstance(self, abstract, wmi): if not wmi.get_relations("-1", r["type"], self._id): return False return True - diff --git a/skiros2_common/src/skiros2_common/ros/utils.py b/skiros2_common/src/skiros2_common/ros/utils.py index 4835c14..d9b55bf 100644 --- a/skiros2_common/src/skiros2_common/ros/utils.py +++ b/skiros2_common/src/skiros2_common/ros/utils.py @@ -38,31 +38,34 @@ import skiros2_common.core.params as param from pydoc import locate + def json_load_byteified(file_handle): try: return _byteify( json.load(file_handle, object_hook=_byteify), ignore_dicts=True ) - except: + except BaseException: return None + def json_loads_byteified(json_text): try: return _byteify( json.loads(json_text, object_hook=_byteify), ignore_dicts=True ) - except: + except BaseException: return None -def _byteify(data, ignore_dicts = False): + +def _byteify(data, ignore_dicts=False): # if this is a unicode string, return its string representation if isinstance(data, unicode): return data.encode('utf-8') # if this is a list of values, return list of byteified values if isinstance(data, list): - return [ _byteify(item, ignore_dicts=True) for item in data ] + return [_byteify(item, ignore_dicts=True) for item in data] # if this is a dictionary, return dictionary of byteified keys and values # but only if we haven't already byteified it if isinstance(data, dict) and not ignore_dicts: @@ -83,6 +86,7 @@ def _byteify(data, ignore_dicts = False): ctype_map = {} + class StrEncoder(json.JSONEncoder): def default(self, obj): if name in complex_types_names: @@ -90,6 +94,7 @@ def default(self, obj): # Let the base class default method raise the TypeError return json.JSONEncoder.default(self, obj) + class ParamsEncoder(json.JSONEncoder): def default(self, obj): name = obj.__class__.__name__ @@ -98,6 +103,7 @@ def default(self, obj): # Let the base class default method raise the TypeError return json.JSONEncoder.default(self, obj) + class UnicodeDecoder(json.JSONDecoder): def default(self, obj): name = obj.__class__.__name__ @@ -110,6 +116,7 @@ def default(self, obj): def defaultDecoder(obj): return obj + def registerDecoder(name, func): if name in complex_types_string: print "{} already registered as decoder".format(name) @@ -117,6 +124,7 @@ def registerDecoder(name, func): complex_types_string.append(name) complex_types_decoders.append(func) + def registerEncoder(ctype, func): class_name = ctype.__name__ if class_name in complex_types_names: @@ -126,6 +134,7 @@ def registerEncoder(ctype, func): complex_types_names.append(class_name) complex_types_encoders.append(func) + def registerClass(name, class_type, encoder=json.JSONEncoder.default, decoder=defaultDecoder): """ Class registration is used to translate a complex type to C element and vice-versa @@ -133,6 +142,7 @@ def registerClass(name, class_type, encoder=json.JSONEncoder.default, decoder=de registerDecoder(name, decoder) registerEncoder(class_type, encoder) + def registerCtype(name, class_type): """ C types are used to translate a type to a C type string. To use when the C string is already used by a registered class @@ -151,7 +161,7 @@ def getStrFromType(obj): name = obj.__name__ if name in complex_types_names: return complex_types_string[complex_types_names.index(name)] - elif ctype_map.has_key(name): + elif name in ctype_map: return ctype_map[name] else: return name @@ -166,45 +176,52 @@ def getTypeFromStr(name): else: return locate(name)() + def encodeParam(encoder, obj): - return {"key": obj._key, "description": obj._description, "specType": int(obj._param_type)-1, "type": getStrFromType(obj._data_type), + return {"key": obj._key, "description": obj._description, "specType": int(obj._param_type) - 1, "type": getStrFromType(obj._data_type), "values": obj._values} + def decodeParam(p): if isinstance(p, str): v = json_loads_byteified(p) else: v = p - if len(v['values'])>0: + if len(v['values']) > 0: return param.Param(v['key'], v['description'], decode(v['values'], v['type']), v['specType']) else: return param.Param(v['key'], v['description'], getTypeFromStr(v['type']).__class__, v['specType']) + def encodeProperty(encoder, obj): return {"key": obj._key, "type": getStrFromType(obj._data_type), "values": obj._values} + def decodeProperty(p): if isinstance(p, str): v = json_loads_byteified(p) else: v = p - if len(v['values'])>0: + if len(v['values']) > 0: return param.Property(v['key'], decode(v['values'], v['type'])) else: return param.Property(v['key'], getTypeFromStr(v['type']).__class__) + def encodeElement(encoder, obj): return {"id": obj._id, "label": obj._label, "type": obj._type, "last_update": 0.0, - "properties": {k: encoder.default(v) for k, v in obj._properties.iteritems() }, "relations": obj._relations} + "properties": {k: encoder.default(v) for k, v in obj._properties.iteritems()}, "relations": obj._relations} + def decodeElement(json_string): - #TODO json_string['last_update'] + # TODO json_string['last_update'] e = we.Element(json_string['type'], json_string['label'], json_string['id']) for k, p in json_string['properties'].iteritems(): e._properties[k] = decodeProperty(p) e._relations = json_string['relations'] return e + registerClass("skiros_wm::Element", we.Element, encodeElement, decodeElement) registerClass("skiros_wm::Param", param.Param, encodeParam) registerClass("skiros_wm::Property", param.Property, encodeProperty) @@ -219,6 +236,7 @@ def decode(values, data_type): else: return values + def serializeParamMap(param_map): """ >>> ph = param.ParamHandler() @@ -249,6 +267,7 @@ def serializeParamMap(param_map): s_param_map.append(msg) return s_param_map + def deserializeParamMap(params): """ >>> ph = param.ParamHandler() @@ -262,10 +281,11 @@ def deserializeParamMap(params): param_map = {} for p in params: dp = decodeParam(p.param) - if dp!=None: + if dp is not None: param_map[dp._key] = dp return param_map + def deserializePropertyMap(msg): """ >>> params = {} @@ -282,13 +302,14 @@ def deserializePropertyMap(msg): """ p_map = {} for p in msg: - dataValue = json_loads_byteified(p.dataValue)#TODO: if doesn-t work add old check - if len(dataValue)>0: - p_map[p.key] = param.Property(p.key, decode(dataValue, p.dataType)) + dataValue = json_loads_byteified(p.dataValue) # TODO: if doesn-t work add old check + if len(dataValue) > 0: + p_map[p.key] = param.Property(p.key, decode(dataValue, p.dataType)) else: - p_map[p.key] = param.Property(p.key, getTypeFromStr(p.dataType).__class__) + p_map[p.key] = param.Property(p.key, getTypeFromStr(p.dataType).__class__) return p_map + def serializePropertyMap(p_map): """ >>> params = {} @@ -313,6 +334,7 @@ def serializePropertyMap(p_map): s_p_map.append(msg) return s_p_map + def msg2element(msg): e = we.Element() e._id = msg.id @@ -323,6 +345,7 @@ def msg2element(msg): e.addRelation(r.subjectId, r.predicate, r.objectId) return e + def element2msg(element): msg = msgs.WmElement() msg.id = element.id @@ -333,6 +356,7 @@ def element2msg(element): msg.relations.append(relation2msg(r)) return msg + def relation2msg(r): rmsg = msgs.Relation() rmsg.subjectId = r['src'] @@ -340,15 +364,19 @@ def relation2msg(r): rmsg.objectId = r['dst'] return rmsg + def msg2relation(msg): return makeRelation(msg.subjectId, msg.predicate, msg.objectId) + def makeRelation(subj, pred, obj): return {'src': subj, 'type': pred, 'dst': obj} + def makeRelationMsg(subj, pred, obj): return msgs.Relation(subj, pred, obj) + def makeStatementMsg(subj, pred, obj, value): rmsg = msgs.Statement() rmsg.relation = makeRelationMsg(subj, pred, obj) diff --git a/skiros2_common/src/skiros2_common/tools/decorators.py b/skiros2_common/src/skiros2_common/tools/decorators.py index be04e1d..52bf842 100644 --- a/skiros2_common/src/skiros2_common/tools/decorators.py +++ b/skiros2_common/src/skiros2_common/tools/decorators.py @@ -31,6 +31,7 @@ from functools import wraps + class PrettyMetaClass(type): """ Meta class for adding a simple string representation to a class @@ -62,7 +63,6 @@ def class_name(self): return type(self).__name__ - def abstractmethod(func): """ Simple decorator for abstract methods. diff --git a/skiros2_common/src/skiros2_common/tools/dict_diff.py b/skiros2_common/src/skiros2_common/tools/dict_diff.py index a09e10d..6f73940 100644 --- a/skiros2_common/src/skiros2_common/tools/dict_diff.py +++ b/skiros2_common/src/skiros2_common/tools/dict_diff.py @@ -6,22 +6,28 @@ class DictDiff(object): (3) keys same in both but changed values (4) keys same in both and unchanged values """ + def __init__(self, current_dict, past_dict): self.current_dict, self.past_dict = current_dict, past_dict self.set_current, self.set_past = set(current_dict.keys()), set(past_dict.keys()) self.intersect = self.set_current.intersection(self.set_past) + @property def added(self): return self.set_current - self.intersect + @property def removed(self): return self.set_past - self.intersect + @property def changed(self): return [o for o in self.intersect if self.past_dict[o] != self.current_dict[o]] + @property def changed_values(self): return {o: self.current_dict[o] for o in self.intersect if self.past_dict[o] != self.current_dict[o]} + @property def unchanged(self): - return [o for o in self.intersect if self.past_dict[o] == self.current_dict[o]] \ No newline at end of file + return [o for o in self.intersect if self.past_dict[o] == self.current_dict[o]] diff --git a/skiros2_common/src/skiros2_common/tools/id_generator.py b/skiros2_common/src/skiros2_common/tools/id_generator.py index feedf34..d4108c4 100644 --- a/skiros2_common/src/skiros2_common/tools/id_generator.py +++ b/skiros2_common/src/skiros2_common/tools/id_generator.py @@ -4,7 +4,7 @@ def __init__(self): self._ids = [] def getId(self, desired=-1): - if desired>=0: + if desired >= 0: self._id = desired while self._id in self._ids: self._id += 1 diff --git a/skiros2_common/src/skiros2_common/tools/logger.py b/skiros2_common/src/skiros2_common/tools/logger.py index fe8e9f7..2b7a171 100644 --- a/skiros2_common/src/skiros2_common/tools/logger.py +++ b/skiros2_common/src/skiros2_common/tools/logger.py @@ -29,7 +29,7 @@ class logColor: class Log: __LOG = [] - + __level = logMode.TRACE __indent = 0 __buffered = False @@ -37,15 +37,23 @@ class Log: # colored messages def msgMode(self, mode, msg): - if (mode == logMode.ALWAYS): return msg - elif (mode == logMode.ERROR): return msgError(msg) - elif (mode == logMode.WARN): return msgWarn(msg) - elif (mode == logMode.INFO): return msgInfo(msg) - elif (mode == logMode.OK): return msgOk(msg) - elif (mode == logMode.DEBUG): return msgDebug(msg) - elif (mode == logMode.TRACE): return msgTrace(msg) - else: return msg - + if (mode == logMode.ALWAYS): + return msg + elif (mode == logMode.ERROR): + return msgError(msg) + elif (mode == logMode.WARN): + return msgWarn(msg) + elif (mode == logMode.INFO): + return msgInfo(msg) + elif (mode == logMode.OK): + return msgOk(msg) + elif (mode == logMode.DEBUG): + return msgDebug(msg) + elif (mode == logMode.TRACE): + return msgTrace(msg) + else: + return msg + @staticmethod def msgTrace(msg): return msg @staticmethod @@ -79,6 +87,7 @@ def countWarnMsg(self): return countMsg(logMode.WARN) def countOkMsg(self): return countMsg(logMode.OK) def countInfoMsg(self): return countMsg(logMode.INFO) def countDebugMsg(self): return countMsg(logMode.DEBUG) + def countTraceMsg(self): return countMsg(logMode.TRACE) def hasError(self): return self.hasMode(logMode.ERROR) @@ -86,6 +95,7 @@ def hasWarn(self): return self.hasMode(logMode.WARN) def hasOk(self): return self.hasMode(logMode.OK) def hasInfo(self): return self.hasMode(logMode.INFO) def hasDebug(self): return self.hasMode(logMode.DEBUG) + def hasTrace(self): return self.hasMode(logMode.TRACE) def assertError(self, cond, msg, desc = None): return self.test(cond, msg, logMode.ERROR, desc) @@ -101,12 +111,13 @@ def lastWarn(self): return self.lastModeMsg(logMode.WARN) # set logging level def setLevel(self, mode): self.__level = mode + def getLevel(self): return self.__level # clear log def clear(self): self.__LOG = [] - + # reset log def reset(self): self.__LOG = [] @@ -123,58 +134,63 @@ def disableOutput(self): # set indentation def indent(self): self.__indent += 2 + def unindent(self): self.__indent = max(0, self.__indent - 2) - + # return last message of mode def lastModeMsg(self, mode): msg = None for item in self.__LOG: k = item[0] v = item[1] - if k == mode: msg = v + if k == mode: + msg = v return msg - + # get number of messages def countMsg(self, mode): count = 0 for msg in self.__LOG: - if (msg[0] <= mode) and (msg[0] != logMode.ALWAYS): count += 1 + if (msg[0] <= mode) and (msg[0] != logMode.ALWAYS): + count += 1 return count - + # add log message - def log(self, mode, msg, desc = None): + def log(self, mode, msg, desc=None): if (mode <= self.__level): msg = msg.rjust(len(msg) + self.__indent) - self.__LOG.append( (mode, msg, desc) ) + self.__LOG.append((mode, msg, desc)) if not self.__buffered: print(self.msgToString(self.__LOG[-1])) - # has log message with mode def hasMode(self, mode): for msg in self.__LOG: - if (msg[0] <= mode) and (msg[0] != logMode.ALWAYS): return True + if (msg[0] <= mode) and (msg[0] != logMode.ALWAYS): + return True return False - + # test condition and add log message - def test(self, cond, msg, modeFailed, descFailed = None, modeSuccess = None, descSuccess = None): - if (not cond): self.log(modeFailed, msg, descFailed) + def test(self, cond, msg, modeFailed, descFailed=None, modeSuccess=None, descSuccess=None): + if (not cond): + self.log(modeFailed, msg, descFailed) elif (modeSuccess is not None): self.log(modeSuccess, msg, descSuccess) - + return cond # convert log to string def toString(self): resString = "" - + maxLength = 0 for msg in self.__LOG: - if (msg[0] == logMode.ALWAYS): continue + if (msg[0] == logMode.ALWAYS): + continue m = re.sub("\\x1b\[[0-9]+m", "", msg[1]) if (len(m) > maxLength): maxLength = len(m) - + maxLength += 2 for msg in self.__LOG: k = msg[0] @@ -193,9 +209,10 @@ def toString(self): if (d is not None): resString += msgMode(k, d) resString += "\n" - - if not self.__useColor: resString = re.sub("\\x1b\[[0-9]+m", "",resString) - + + if not self.__useColor: + resString = re.sub("\\x1b\[[0-9]+m", "", resString) + return resString.strip("\n") def msgToString(self, msg): diff --git a/skiros2_common/src/skiros2_common/tools/plugin_loader.py b/skiros2_common/src/skiros2_common/tools/plugin_loader.py index 99fc9a2..ce58b04 100644 --- a/skiros2_common/src/skiros2_common/tools/plugin_loader.py +++ b/skiros2_common/src/skiros2_common/tools/plugin_loader.py @@ -36,6 +36,7 @@ import re + class PluginLoader(object): @classmethod @@ -46,19 +47,20 @@ def __import_plugins(self, package, base): package = importlib.import_module(package) for loader, modname, is_pkg in pkgutil.walk_packages(path=package.__path__, - prefix=package.__name__+'.', + prefix=package.__name__ + '.', onerror=lambda x: None): - if is_pkg: continue + if is_pkg: + continue if modname not in sys.modules: importlib.import_module(modname) module = sys.modules[modname] - pred = lambda member: inspect.isclass(member) and \ - member.__module__ == modname and \ - issubclass(member, base) and \ - not member.__subclasses__() + def pred(member): return inspect.isclass(member) and \ + member.__module__ == modname and \ + issubclass(member, base) and \ + not member.__subclasses__() classes = dict(inspect.getmembers(module, pred)) results.extend(classes.values()) @@ -72,8 +74,8 @@ def signature(self, plugin): args = inspect.getargspec(plugin.__init__) names = args[0] defaults = args[-1] if args[-1] is not None else [] - req = names[1:len(names)-len(defaults)] - opt = dict(zip(names[len(names)-len(defaults):], defaults)) + req = names[1:len(names) - len(defaults)] + opt = dict(zip(names[len(names) - len(defaults):], defaults)) return (names[1:], req, opt) @classmethod @@ -88,7 +90,8 @@ def split(self, plugin): def match(self, plugin, desc=None): clazz = self.split(plugin)[::-1] for a, b in zip(clazz, desc): - if b and not re.match("^"+b+"$", a): return False + if b and not re.match("^" + b + "$", a): + return False return True @classmethod @@ -102,7 +105,6 @@ def instance(self, plugin, args_dict): print(" ERROR while instantiating: " + str(e)) return p - def __init__(self): self._plugins = list() @@ -118,7 +120,6 @@ def _filter(self, desc): def _exclude(self, desc): return [p for p in self._plugins if not self.match(p, desc)] - def load(self, folder, base_class): self._plugins += self.__import_plugins(folder, base_class) if self.size() == 0: @@ -131,12 +132,12 @@ def load(self, folder, base_class): def size(self): return len(self._plugins) - def create(self, args_dict = {}): + def create(self, args_dict={}): instances = [] for p in self._plugins: names, req, opt = self.signature(p) args = opt.copy() - args.update( { k:args_dict[k] for k in req if k in args_dict.keys()} ) + args.update({k: args_dict[k] for k in req if k in args_dict.keys()}) instance = self.instance(p, args) if instance is not None: @@ -146,12 +147,13 @@ def create(self, args_dict = {}): def getPluginByName(self, name): p = self._filter([name]) - if len(p) == 0: raise Exception("No plugin with name " + str(name) + " found!") - elif len(p) > 1: print("WARNING: Multiple plugins with name " + str(name) + " found!\n" + str(p)) + if len(p) == 0: + raise Exception("No plugin with name " + str(name) + " found!") + elif len(p) > 1: + print("WARNING: Multiple plugins with name " + str(name) + " found!\n" + str(p)) return p[0] def list(self): for p in self._plugins: print(self.split(p)) return [p.__name__ for p in self._plugins] - diff --git a/skiros2_common/src/skiros2_common/tools/time_keeper.py b/skiros2_common/src/skiros2_common/tools/time_keeper.py index c1f4dcc..82de1b1 100644 --- a/skiros2_common/src/skiros2_common/tools/time_keeper.py +++ b/skiros2_common/src/skiros2_common/tools/time_keeper.py @@ -29,7 +29,8 @@ def __enter__(self): return self def __exit__(self, type, value, traceback): - if type is not None: return False + if type is not None: + return False dt = now() - self._start_time log.info(self._name, "Total: {:0.4f} secs.".format(dt)) return True @@ -42,6 +43,8 @@ def __exit__(self, type, value, traceback): time_to_do_something = Timekeeper.getLast() avg_time_to_do_something = Timekeeper.getAvgTime() """ + + class TimeKeeper(): def __init__(self): self._list = [] @@ -67,9 +70,9 @@ def __enter__(self): return self def __exit__(self, type, value, traceback): - if len(self._list)>10: + if len(self._list) > 10: self._list.pop(0) - self._list.append(time.time()-self._start_time) + self._list.append(time.time() - self._start_time) def getAvgTime(self): return sum(self._list) / float(len(self._list)) @@ -84,12 +87,14 @@ def getLast(self): --Do something-- time_to_do_something = Timekeeper.list[-1] """ + + class TimeKeepers(): def __init__(self): self._map = {} def __getitem__(self, key): - if not self._map.has_key(key): + if key not in self._map: self._map[key] = TimeKeeper() return self._map[key] diff --git a/skiros2_gui/setup.py b/skiros2_gui/setup.py index 344e0d0..0c1e78d 100644 --- a/skiros2_gui/setup.py +++ b/skiros2_gui/setup.py @@ -1,4 +1,4 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup diff --git a/skiros2_gui/src/skiros2_gui/core/__init__.py b/skiros2_gui/src/skiros2_gui/core/__init__.py index 633f866..40a96af 100644 --- a/skiros2_gui/src/skiros2_gui/core/__init__.py +++ b/skiros2_gui/src/skiros2_gui/core/__init__.py @@ -1,2 +1 @@ # -*- coding: utf-8 -*- - diff --git a/skiros2_gui/src/skiros2_gui/core/skiros_widget.py b/skiros2_gui/src/skiros2_gui/core/skiros_widget.py index 1d89a97..c3de464 100644 --- a/skiros2_gui/src/skiros2_gui/core/skiros_widget.py +++ b/skiros2_gui/src/skiros2_gui/core/skiros_widget.py @@ -28,10 +28,11 @@ from threading import Lock from datetime import datetime + class SkirosAddObjectDialog(QDialog): -#============================================================================== -# Modal dialog -#============================================================================== + #============================================================================== + # Modal dialog + #============================================================================== default_type = 'sumo:Object' @@ -54,8 +55,7 @@ def __init__(self, *args, **kwargs): self._comboBoxes = [] self.create_comboBox(label='Type') self.comboBox_individual.clear() - [self.comboBox_individual.addItem(l,d) for l,d in self.get_individuals(self.default_type).iteritems()] - + [self.comboBox_individual.addItem(l, d) for l, d in self.get_individuals(self.default_type).iteritems()] @property def object(self): @@ -66,7 +66,6 @@ def object(self): """ return self.comboBox_individual.itemData(self.comboBox_individual.currentIndex()) - def on_select_type(self, id, index): """Callback for change selection in dropdown lists. @@ -79,18 +78,18 @@ def on_select_type(self, id, index): # log.debug(self.__class__.__name__, 'Selected {}: {}'.format(id, index)) # clear filters after selected - while id < len(self._comboBoxes)-1: + while id < len(self._comboBoxes) - 1: # log.debug(self.__class__.__name__, 'Delete {}'.format(id+1)) - label = self.formLayout.labelForField(self._comboBoxes[id+1]) + label = self.formLayout.labelForField(self._comboBoxes[id + 1]) label.deleteLater() - self._comboBoxes[id+1].deleteLater() - del self._comboBoxes[id+1] + self._comboBoxes[id + 1].deleteLater() + del self._comboBoxes[id + 1] # get current selection - if index > 0: # if not 'All' is selected + if index > 0: # if not 'All' is selected selected = self._comboBoxes[id].itemData(self._comboBoxes[id].currentIndex()) - elif id > 0: # if 'All' is selected and it is not the first combo box - selected = self._comboBoxes[id-1].itemData(self._comboBoxes[id-1].currentIndex()) + elif id > 0: # if 'All' is selected and it is not the first combo box + selected = self._comboBoxes[id - 1].itemData(self._comboBoxes[id - 1].currentIndex()) else: # if 'All' is selected and it is the first combo box selected = self.default_type # log.debug(self.__class__.__name__, 'Selected type {}'.format(selected)) @@ -104,10 +103,9 @@ def on_select_type(self, id, index): self.comboBox_individual.clear() if index > 0 or (id > 0 and index == 0): self.comboBox_individual.addItem('new ' + utils.ontology_type2name(selected), selected) - [self.comboBox_individual.addItem(l,d) for l,d in self.get_individuals(selected).iteritems()] + [self.comboBox_individual.addItem(l, d) for l, d in self.get_individuals(selected).iteritems()] QTimer.singleShot(0, self.adjustSize) - def create_comboBox(self, subtype='sumo:Object', label='Subtype'): """Inserts a new combobox in the dialog based on the subtype. @@ -121,12 +119,11 @@ def create_comboBox(self, subtype='sumo:Object', label='Subtype'): sizePolicy = QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed) comboBox.setSizePolicy(sizePolicy) comboBox.addItem('All') - [comboBox.addItem(l,d) for l,d in self.get_types(subtype).iteritems()] + [comboBox.addItem(l, d) for l, d in self.get_types(subtype).iteritems()] comboBox.currentIndexChanged.connect(partial(self.on_select_type, len(self._comboBoxes))) self.formLayout.insertRow(len(self._comboBoxes), label, comboBox) self._comboBoxes.append(comboBox) - def get_types(self, subtype): """Retrieves available subtype from the ontology. @@ -138,7 +135,6 @@ def get_types(self, subtype): """ return utils.ontology_type2name_dict(self.parent()._wmi.get_sub_classes(subtype, False)) - def get_individuals(self, subtype): """Retrieves available individuals from the world model. @@ -182,10 +178,10 @@ def _make_box(self, msg, size): return marker def _make_box_control(self, msg, size): - control = InteractiveMarkerControl() + control = InteractiveMarkerControl() control.always_visible = True - control.markers.append( self._make_box(msg, size) ) - msg.controls.append( control ) + control.markers.append(self._make_box(msg, size)) + msg.controls.append(control) return control def initInteractiveServer(self, name): @@ -216,10 +212,10 @@ def make_6dof_marker(self, pose, size, frame_id, base_frame_id, interaction_mode self._make_box_control(int_marker, size) int_marker.controls[0].interaction_mode = interaction_mode - n = norm([1,1]) + n = norm([1, 1]) control = InteractiveMarkerControl() - control.orientation.w = 1/n - control.orientation.x = 1/n + control.orientation.w = 1 / n + control.orientation.x = 1 / n control.orientation.y = 0 control.orientation.z = 0 control.name = "rotate_x" @@ -227,8 +223,8 @@ def make_6dof_marker(self, pose, size, frame_id, base_frame_id, interaction_mode int_marker.controls.append(control) control = InteractiveMarkerControl() - control.orientation.w = 1/n - control.orientation.x = 1/n + control.orientation.w = 1 / n + control.orientation.x = 1 / n control.orientation.y = 0 control.orientation.z = 0 control.name = "move_x" @@ -236,37 +232,37 @@ def make_6dof_marker(self, pose, size, frame_id, base_frame_id, interaction_mode int_marker.controls.append(control) control = InteractiveMarkerControl() - control.orientation.w = 1/n + control.orientation.w = 1 / n control.orientation.x = 0 - control.orientation.y = 1/n + control.orientation.y = 1 / n control.orientation.z = 0 control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() - control.orientation.w = 1/n + control.orientation.w = 1 / n control.orientation.x = 0 - control.orientation.y = 1/n + control.orientation.y = 1 / n control.orientation.z = 0 control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() - control.orientation.w = 1/n + control.orientation.w = 1 / n control.orientation.x = 0 control.orientation.y = 0 - control.orientation.z = 1/n + control.orientation.z = 1 / n control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() - control.orientation.w = 1/n + control.orientation.w = 1 / n control.orientation.x = 0 control.orientation.y = 0 - control.orientation.z = 1/n + control.orientation.z = 1 / n control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) @@ -274,10 +270,11 @@ def make_6dof_marker(self, pose, size, frame_id, base_frame_id, interaction_mode self._server.insert(int_marker, self.on_marker_feedback) self._server.applyChanges() + class SkirosWidget(QWidget, SkirosInteractiveMarkers): -#============================================================================== -# General -#============================================================================== + #============================================================================== + # General + #============================================================================== widget_id = 'skiros_gui' @@ -290,9 +287,9 @@ def __init__(self): ui_file = os.path.join(rospkg.RosPack().get_path('skiros2_gui'), 'src/skiros2_gui/core', 'skiros_gui.ui') loadUi(ui_file, self) - self.skill_tree_widget.currentItemChanged.connect( lambda : self.on_skill_tree_widget_item_selection_changed( self.skill_tree_widget.currentItem() ) ) - self.wm_tree_widget.itemSelectionChanged.connect( lambda : self.on_wm_tree_widget_item_selection_changed( self.wm_tree_widget.currentItem() ) ) - self.wm_properties_widget.itemChanged.connect( lambda p: self.on_properties_table_item_changed( self.wm_tree_widget.currentItem(), p.row() ) ) + self.skill_tree_widget.currentItemChanged.connect(lambda: self.on_skill_tree_widget_item_selection_changed(self.skill_tree_widget.currentItem())) + self.wm_tree_widget.itemSelectionChanged.connect(lambda: self.on_wm_tree_widget_item_selection_changed(self.wm_tree_widget.currentItem())) + self.wm_properties_widget.itemChanged.connect(lambda p: self.on_properties_table_item_changed(self.wm_tree_widget.currentItem(), p.row())) self.wm_relations_widget.resizeEvent = self.on_wm_relations_widget_resized self.wm_update_signal.connect(lambda d: self.on_wm_update(d)) self.task_progress_signal.connect(lambda d: self.on_progress_update(d)) @@ -304,21 +301,20 @@ def __init__(self): self.tableWidget_output.setColumnWidth(4, 40) self.reset() - def reset(self): - #The plugin should not call init_node as this is performed by rqt_gui_py. - #Due to restrictions in Qt, you cannot manipulate Qt widgets directly within ROS callbacks, - #because they are running in a different thread. + # The plugin should not call init_node as this is performed by rqt_gui_py. + # Due to restrictions in Qt, you cannot manipulate Qt widgets directly within ROS callbacks, + # because they are running in a different thread. self.initInteractiveServer(SkirosWidget.widget_id) self._wmi = wmi.WorldModelInterface(SkirosWidget.widget_id) self._sli = sli.SkillLayerInterface(SkirosWidget.widget_id) - #Setup a timer to keep interface updated + # Setup a timer to keep interface updated self.refresh_timer = QTimer() self.refresh_timer.timeout.connect(self.refresh_timer_cb) self.refresh_timer.start(500) - #World model tab + # World model tab self._wmi.set_monitor_cb(lambda d: self.wm_update_signal.emit(d)) self._sli.set_monitor_cb(lambda d: self.task_progress_signal.emit(d)) self._snapshot_id = "" @@ -326,16 +322,15 @@ def reset(self): self._wm_mutex = Lock() self._task_mutex = Lock() - #Skill tab + # Skill tab self.last_executed_skill = "" self.skill_stop_button.setEnabled(False) - self.space_shortcut=QShortcut(QtGui.QKeySequence(Qt.Key_Space), self) + self.space_shortcut = QShortcut(QtGui.QKeySequence(Qt.Key_Space), self) self.space_shortcut.activated.connect(self.skill_start_stop) - #self.space_shortcut.setContext(QtCore.Qt.WidgetWithChildrenShortcut) - #Log tab + # self.space_shortcut.setContext(QtCore.Qt.WidgetWithChildrenShortcut) + # Log tab self.log_file = None - def shutdown_plugin(self): # TODO unregister all publishers here pass @@ -347,17 +342,16 @@ def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value("logs_file_name", self.logs_file_lineEdit.text()) instance_settings.set_value("last_executed_skill", self.last_executed_skill) - def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: - if self._wmi.get_scene_name()!="": + if self._wmi.get_scene_name() != "": self.scene_file_lineEdit.setText(self._wmi.get_scene_name()) - elif instance_settings.value("scene_name") is not None and instance_settings.value("scene_name")!="": + elif instance_settings.value("scene_name") is not None and instance_settings.value("scene_name") != "": self.scene_file_lineEdit.setText(instance_settings.value("scene_name")) if instance_settings.value("logs_file_name") is not None: self.logs_file_lineEdit.setText(instance_settings.value("logs_file_name")) if instance_settings.value("save_logs") is not None: - self.save_logs_checkBox.setChecked(instance_settings.value("save_logs")=='true') + self.save_logs_checkBox.setChecked(instance_settings.value("save_logs") == 'true') self.on_save_logs_checkBox_clicked() if instance_settings.value("last_executed_skill") is not None: self.last_executed_skill = instance_settings.value("last_executed_skill") @@ -370,7 +364,6 @@ def restore_settings(self, plugin_settings, instance_settings): # pass - #============================================================================== # General #============================================================================== @@ -379,13 +372,13 @@ def refresh_timer_cb(self): """ Keeps ui updated """ - #Update robot BT rate + # Update robot BT rate if self._sli.agents: robot_txt = "" for name, manager in self._sli.agents.iteritems(): robot_txt += "{}: {:0.1f}hz ".format(name.replace("/", ""), manager.get_tick_rate()) self.robot_rate_info.setText(robot_txt) - #Update skill list + # Update skill list if self._sli.has_changes: self.skill_tree_widget.clear() self.skill_tree_widget.setColumnCount(3) @@ -420,19 +413,19 @@ def on_save_scene_button_clicked(self): log.debug(self.__class__.__name__, 'Saving world model to <{}>'.format(file)) self._wmi.save(file) - @Slot() def on_add_object_button_clicked(self): dialog = SkirosAddObjectDialog(self) ret = dialog.exec_() - if not ret: return + 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 - + if not parent_id: + return elem = self._wmi.get_template_element(dialog.object) elem.label = utils.ontology_type2name(dialog.object) @@ -445,12 +438,12 @@ def on_add_object_button_clicked(self): # item = QTreeWidgetItem(parent, [utils.ontology_type2name(elem_id), elem_id]) # self.wm_tree_widget.setCurrentItem(item) - @Slot() def on_remove_object_button_clicked(self): item = self.wm_tree_widget.currentItem() item_id = item.text(1) - if not item_id: return + if not item_id: + return parent = item.parent() self.wm_tree_widget.setCurrentItem(parent) @@ -461,7 +454,6 @@ def on_remove_object_button_clicked(self): # parent_id = parent.text(1) log.debug(self.__class__.__name__, 'Removed element {}'.format(item_id)) - #item = self.wm_tree_widget.currentItem() # self.remove_wm_tree_widget_item(item) # parent.removeChild(item) @@ -474,11 +466,10 @@ def on_remove_object_button_clicked(self): # else: # [self.remove_wm_tree_widget_item(child) for child in item.takeChildren()] - @Slot() 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 + 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) @@ -488,7 +479,8 @@ def on_wm_update(self, data): # check if element is already in tree items = self.wm_tree_widget.findItems(elem.id, Qt.MatchRecursive | Qt.MatchFixedString, 1) - if not items: continue + if not items: + continue # check if updated item is selected if elem.id == cur_item_id: @@ -499,7 +491,8 @@ def on_wm_update(self, data): # get parent node in tree parent = items[0].parent() - if not parent: continue + if not parent: + continue # check if the old parent is still parent of the updated element has_child = elem.getRelations(subj=parent.text(1), pred=self._wmi.get_sub_properties('skiros:spatiallyRelated'), obj='-1') @@ -520,11 +513,10 @@ def on_wm_update(self, data): 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 + 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 on_marker_feedback(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: elem = self._wmi.get_element(feedback.marker_name) @@ -535,7 +527,7 @@ def on_marker_feedback(self, feedback): def on_wm_tree_widget_item_selection_changed(self, item): self.wm_properties_widget.blockSignals(True) self.clear_markers() - if item.text(1).find("_skills")<0: + if item.text(1).find("_skills") < 0: elem = self._wmi.get_element(item.text(1)) self.fill_properties_table(elem) self.fill_relations_table(elem) @@ -543,13 +535,12 @@ def on_wm_tree_widget_item_selection_changed(self, item): p = elem.getData(":Pose") size = elem.getData(":Size") if not None in p[0] and not None in p[1]: - self.make_6dof_marker(p, size, elem.id, elem.getProperty("skiros:BaseFrameId").value, InteractiveMarkerControl.NONE) # NONE,MOVE_3D, MOVe_ROTATE_3D + self.make_6dof_marker(p, size, elem.id, elem.getProperty("skiros:BaseFrameId").value, InteractiveMarkerControl.NONE) # NONE,MOVE_3D, MOVe_ROTATE_3D else: self.wm_properties_widget.setRowCount(0) self.wm_relations_widget.setRowCount(0) self.wm_properties_widget.blockSignals(False) - @Slot() def on_properties_table_item_changed(self, item, row): item_key = self.wm_properties_widget.item(row, 0) @@ -560,8 +551,10 @@ def on_properties_table_item_changed(self, item, row): if elem.hasProperty(key): prop = elem.getProperty(key) - if value == '': value = None - if prop.dataTypeIs(bool): value = item_val.checkState() == Qt.Checked + 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)) @@ -580,15 +573,12 @@ def on_properties_table_item_changed(self, item, row): name = utils.ontology_type2name(elem.id) if not elem.label else utils.ontology_type2name(elem.label) item.setText(0, name) - - @Slot() def on_wm_relations_widget_resized(self, event): - width = self.wm_relations_widget.size().width()-2 + width = self.wm_relations_widget.size().width() - 2 cols = self.wm_relations_widget.columnCount() for i in range(cols): - self.wm_relations_widget.setColumnWidth(i, float(width)/cols) - + self.wm_relations_widget.setColumnWidth(i, float(width) / cols) def create_wm_tree(self): scene_tuple = self._wmi.get_scene() @@ -625,7 +615,7 @@ def _add_wm_node(self, elem): return parent_id = '{}_skills'.format(parent_rel['src']) item = self.wm_tree_widget.findItems(parent_id, Qt.MatchRecursive | Qt.MatchFixedString, 1) - if not item:#In case it is still not existing i create the "support" skill node + if not item: # In case it is still not existing i create the "support" skill node item = self.wm_tree_widget.findItems(parent_rel['src'], Qt.MatchRecursive | Qt.MatchFixedString, 1)[0] item = QTreeWidgetItem(item, ['Skills', parent_id]) else: @@ -656,28 +646,27 @@ def _create_wm_tree(self, item, scene, elem): for rel in skillPropRel: self._create_wm_tree(item, scene, scene[rel['dst']]) - - def fill_properties_table(self, elem): self.wm_properties_widget.setRowCount(0) - type = elem.id[:elem.id.rfind('-')] - id = elem.id[elem.id.rfind('-')+1:] + type = elem.id[:elem.id.rfind('-')] + id = elem.id[elem.id.rfind('-') + 1:] self._add_properties_table_row(Property('ID', id), editable_value=False) self._add_properties_table_row(Property('Type', type), editable_value=False) self._add_properties_table_row(Property('Label', elem.label), editable_value=True) props = sorted(elem.properties, key=lambda e: e.key) [self._add_properties_table_row(p, False, True) for p in props] - def _add_properties_table_row(self, prop, editable_key=False, editable_value=True): key = QTableWidgetItem(utils.ontology_type2name(prop.key)) - if not editable_key: key.setFlags(key.flags() & ~Qt.ItemIsEditable) + if not editable_key: + key.setFlags(key.flags() & ~Qt.ItemIsEditable) value = prop.values if prop.isList() else prop.value if prop.dataTypeIs(float): val = QTableWidgetItem(format(value, '.4f') if value is not None else '') else: val = QTableWidgetItem(str(value) if value is not None else '') - if not editable_value: val.setFlags(val.flags() & ~Qt.ItemIsEditable) + if not editable_value: + val.setFlags(val.flags() & ~Qt.ItemIsEditable) if prop.dataTypeIs(bool): val.setText('') @@ -689,9 +678,8 @@ def _add_properties_table_row(self, prop, editable_key=False, editable_value=Tru key.id = val.id = prop.key self.wm_properties_widget.insertRow(self.wm_properties_widget.rowCount()) - self.wm_properties_widget.setItem(self.wm_properties_widget.rowCount()-1, 0, key) - self.wm_properties_widget.setItem(self.wm_properties_widget.rowCount()-1, 1, val) - + self.wm_properties_widget.setItem(self.wm_properties_widget.rowCount() - 1, 0, key) + self.wm_properties_widget.setItem(self.wm_properties_widget.rowCount() - 1, 1, val) def fill_relations_table(self, elem): self.wm_relations_widget.setRowCount(0) @@ -699,12 +687,11 @@ def fill_relations_table(self, elem): rel = sorted(rel, key=lambda r: r['src'], reverse=True) rel = map(lambda r: { 'src': r['src'] if r['src'] != '-1' else elem.id, - 'type':r['type'], + 'type': r['type'], 'dst': r['dst'] if r['dst'] != '-1' else elem.id - }, rel) + }, rel) [self._add_relations_table_row(r, r['src'] != elem.id, r['dst'] != elem.id) for r in rel] - def _add_relations_table_row(self, relation, editable_src=False, editable_dst=False): src = QTableWidgetItem(utils.ontology_type2name(relation['src'])) rel = QTableWidgetItem(utils.ontology_type2name(relation['type'])) @@ -718,27 +705,27 @@ def _add_relations_table_row(self, relation, editable_src=False, editable_dst=Fa rel.setTextAlignment(Qt.AlignHCenter) dst.setTextAlignment(Qt.AlignLeft) - if not editable_src: src.setFlags(src.flags() & ~Qt.ItemIsEditable) + if not editable_src: + src.setFlags(src.flags() & ~Qt.ItemIsEditable) rel.setFlags(rel.flags() & ~Qt.ItemIsEditable) - if not editable_dst: dst.setFlags(dst.flags() & ~Qt.ItemIsEditable) + if not editable_dst: + dst.setFlags(dst.flags() & ~Qt.ItemIsEditable) self.wm_relations_widget.insertRow(self.wm_relations_widget.rowCount()) # self.wm_relations_widget.setSpan(self.wm_relations_widget.rowCount()-1, 0, 1, 3) - self.wm_relations_widget.setItem(self.wm_relations_widget.rowCount()-1, 0, src) - self.wm_relations_widget.setItem(self.wm_relations_widget.rowCount()-1, 1, rel) - self.wm_relations_widget.setItem(self.wm_relations_widget.rowCount()-1, 2, dst) - - + self.wm_relations_widget.setItem(self.wm_relations_widget.rowCount() - 1, 0, src) + self.wm_relations_widget.setItem(self.wm_relations_widget.rowCount() - 1, 1, rel) + self.wm_relations_widget.setItem(self.wm_relations_widget.rowCount() - 1, 2, dst) @Slot('QTreeWidgetItem*', 'QTreeWidgetItem*') def wm_tree_widget_currentItemChanged(self, item, prev_item): - while self.wm_properties_widget.rowCount()>0: - self.wm_properties_widget.removeRow(0) + while self.wm_properties_widget.rowCount() > 0: + self.wm_properties_widget.removeRow(0) item = QTableWidgetItem(str(item.text(0))) self.wm_properties_widget.insertRow(0) self.wm_properties_widget.setItem(0, 0, item) - while self.wm_relations_widget.rowCount()>0: - self.wm_relations_widget.removeRow(0) + while self.wm_relations_widget.rowCount() > 0: + self.wm_relations_widget.removeRow(0) item = QTableWidgetItem("") self.wm_relations_widget.insertRow(0) self.wm_relations_widget.setItem(0, 0, item) @@ -748,33 +735,33 @@ def wm_tree_widget_currentItemChanged(self, item, prev_item): # Skill #============================================================================== def _update_progress_table(self, msg): - last_msg = self.tableWidget_output.item(self.tableWidget_output.rowCount()-1, 5).text() if self.tableWidget_output.rowCount()>0 else "" - if msg.progress_message!="Start" and (msg.label.find("task")>=0 or (msg.progress_message!="End" and msg.progress_message!=last_msg)): + last_msg = self.tableWidget_output.item(self.tableWidget_output.rowCount() - 1, 5).text() if self.tableWidget_output.rowCount() > 0 else "" + if msg.progress_message != "Start" and (msg.label.find("task") >= 0 or (msg.progress_message != "End" and msg.progress_message != last_msg)): self.tableWidget_output.insertRow(self.tableWidget_output.rowCount()) - self.tableWidget_output.setItem(self.tableWidget_output.rowCount()-1, 0, QTableWidgetItem("{:0.3f}".format(msg.progress_time))) - self.tableWidget_output.setItem(self.tableWidget_output.rowCount()-1, 1, QTableWidgetItem(msg.parent_label)) - self.tableWidget_output.setItem(self.tableWidget_output.rowCount()-1, 2, QTableWidgetItem(msg.label)) - self.tableWidget_output.setItem(self.tableWidget_output.rowCount()-1, 3, QTableWidgetItem(State(msg.state).name)) - self.tableWidget_output.setItem(self.tableWidget_output.rowCount()-1, 4, QTableWidgetItem(str(msg.progress_code))) + self.tableWidget_output.setItem(self.tableWidget_output.rowCount() - 1, 0, QTableWidgetItem("{:0.3f}".format(msg.progress_time))) + self.tableWidget_output.setItem(self.tableWidget_output.rowCount() - 1, 1, QTableWidgetItem(msg.parent_label)) + self.tableWidget_output.setItem(self.tableWidget_output.rowCount() - 1, 2, QTableWidgetItem(msg.label)) + self.tableWidget_output.setItem(self.tableWidget_output.rowCount() - 1, 3, QTableWidgetItem(State(msg.state).name)) + self.tableWidget_output.setItem(self.tableWidget_output.rowCount() - 1, 4, QTableWidgetItem(str(msg.progress_code))) progress = QTableWidgetItem(str(msg.progress_message)) progress.setToolTip(str(msg.progress_message)) - self.tableWidget_output.setItem(self.tableWidget_output.rowCount()-1, 5, progress) + self.tableWidget_output.setItem(self.tableWidget_output.rowCount() - 1, 5, progress) self.tableWidget_output.scrollToBottom() @Slot() def on_progress_update(self, msg): self._update_progress_table(msg) self._save_log(msg, "skill") - #Update buttons - if msg.type.find("Root")>=0: + # Update buttons + if msg.type.find("Root") >= 0: if not self.skill_stop_button.isEnabled(): self.create_task_tree(msg.id) self._toggle_task_active() for manager in self._sli.agents.values(): manager.reset_tick_rate() - if abs(msg.progress_code)==1: + if abs(msg.progress_code) == 1: self._toggle_task_active() - #Update task tree + # Update task tree with self._task_mutex: items = self.task_tree_widget.findItems(str(msg.id), Qt.MatchRecursive | Qt.MatchFixedString, 1) if items: @@ -824,7 +811,7 @@ def _add_parameter(self, param): cbox = QCheckBox() if param.isSpecified(): cbox.setChecked(param.default) - self.skill_params_table.setCellWidget(row, 1,cbox) + self.skill_params_table.setCellWidget(row, 1, cbox) elif param.dataTypeIs(Element): combobox = QComboBox() self.skill_params_table.setCellWidget(row, 1, combobox) @@ -839,7 +826,6 @@ def _add_parameter(self, param): lineedit.setText(str(param.value)) self.skill_params_table.setCellWidget(row, 1, lineedit) - def _add_available_skill(self, s): stype = self.skill_tree_widget.findItems(s.type, Qt.MatchRecursive | Qt.MatchFixedString, 1) if not stype: @@ -852,10 +838,10 @@ def _add_available_skill(self, s): skill.setData(2, 0, s) def _add_frequently_used_skill(self, s): - self.last_executed_skill=s.name + self.last_executed_skill = s.name fu = self.skill_tree_widget.findItems("fu", Qt.MatchRecursive | Qt.MatchFixedString, 1)[0] - for i in range(0, fu.childCount()):#avoid adding same node multiple times - if s.name==fu.child(i).data(1, 0): + for i in range(0, fu.childCount()): # avoid adding same node multiple times + if s.name == fu.child(i).data(1, 0): return skill = QTreeWidgetItem(fu, [s.name, s.name]) skill.setData(2, 0, s) @@ -881,9 +867,9 @@ def on_skill_tree_widget_item_selection_changed(self, item): return skill = item.data(2, 0) if skill: - #Clean + # Clean self.skill_params_table.setRowCount(0) - #Add params + # Add params self.skill_name_label.setText(skill.name) for p in skill.ph.values(): if self.modality_checkBox.isChecked() or not p.paramTypeIs(ParamTypes.Optional): @@ -902,13 +888,13 @@ def on_skill_exe_button_clicked(self): if skill is None: return self._add_frequently_used_skill(self.skill_tree_widget.currentItem().data(2, 0)) - #Start logger + # Start logger if not self._sli.has_active_tasks: prefix = self.logs_file_lineEdit.text() prefix = prefix[0:prefix.rfind("/")] self.logs_file_lineEdit.setText("{}/{}_{}".format(prefix, datetime.now().strftime("%Y-%m-%d:%H:%M:%S"), skill.name)) self.on_save_logs_checkBox_clicked() - #Send command + # Send command if not self._get_parameters(skill.ph): return self._sli.execute(skill.manager, [skill]) @@ -940,9 +926,10 @@ def on_save_logs_checkBox_clicked(self): self.log_file = open(file_name, "a") def _save_log(self, msg, log_type): - if log_type=="skill": - string = "{};{:0.4f};{};{};{};{};{};{};{}".format(datetime.now().strftime("%H:%M:%S"), msg.progress_time, msg.parent_label, msg.parent_id, msg.label, msg.id, State(msg.state).name, msg.progress_code, msg.progress_message) - elif log_type=="wm_edit": + if log_type == "skill": + string = "{};{:0.4f};{};{};{};{};{};{};{}".format(datetime.now().strftime("%H:%M:%S"), msg.progress_time, msg.parent_label, msg.parent_id, + msg.label, msg.id, State(msg.state).name, msg.progress_code, msg.progress_message) + elif log_type == "wm_edit": if not msg.relation: string = "{} {} {} {}".format(datetime.now(), "WM", msg.action, [e.id for e in msg.elements]) else: @@ -950,4 +937,4 @@ def _save_log(self, msg, log_type): string = "{} {} {}_relation {}-{}-{}".format(datetime.now().strftime("%H:%M:%S"), "WM", msg.action, relation['src'], relation['type'], relation['dst']) if self.save_logs_checkBox.isChecked(): self.logs_textEdit.append(string) - self.log_file.write(string+"\n") \ No newline at end of file + self.log_file.write(string + "\n") diff --git a/skiros2_gui/src/skiros2_gui/gui.py b/skiros2_gui/src/skiros2_gui/gui.py index 89fc32e..641bd1d 100644 --- a/skiros2_gui/src/skiros2_gui/gui.py +++ b/skiros2_gui/src/skiros2_gui/gui.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright (c) 2017, Francesco Rovida +# Copyright (c) 2017, Francesco Rovida # All rights reserved. # @@ -34,6 +34,7 @@ from rqt_gui_py.plugin import Plugin from core.skiros_widget import SkirosWidget + class Skiros(Plugin): def __init__(self, context): super(Skiros, self).__init__(context) @@ -51,4 +52,4 @@ def restore_settings(self, plugin_settings, instance_settings): self._widget.restore_settings(plugin_settings, instance_settings) def trigger_configuration(self): - self._widget.trigger_configuration() \ No newline at end of file + self._widget.trigger_configuration() diff --git a/skiros2_skill/nodes/utils/cmd b/skiros2_skill/nodes/utils/cmd index 02fa648..4298201 100755 --- a/skiros2_skill/nodes/utils/cmd +++ b/skiros2_skill/nodes/utils/cmd @@ -35,23 +35,23 @@ import skiros2_skill.ros.skill_layer_interface as sli import sys if __name__ == '__main__': - if len(sys.argv)<4: + if len(sys.argv) < 4: print "usage: cmd skill_mgr_name start|stop skill_name|execution_id wait" else: mgr = sys.argv[1] action = sys.argv[2] skill = sys.argv[3] wait = False - if len(sys.argv)>4: + if len(sys.argv) > 4: wait = True wmi = wmi.WorldModelInterface() sli = sli.SkillLayerInterface() agent = sli.getAgent(mgr) - if action=="start": + if action == "start": eid = agent.execute([agent.getSkill(skill)], "cmd_line_tool") if wait: - agent.waitResult(eid) #Bugged + agent.waitResult(eid) # Bugged else: print eid - if action=="stop": - agent.preempt(int(skill), "cmd_line_tool") \ No newline at end of file + if action == "stop": + agent.preempt(int(skill), "cmd_line_tool") diff --git a/skiros2_skill/setup.py b/skiros2_skill/setup.py index 0fd27f7..e5b99c8 100644 --- a/skiros2_skill/setup.py +++ b/skiros2_skill/setup.py @@ -1,4 +1,4 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup diff --git a/skiros2_skill/src/skiros2_skill/core/processors.py b/skiros2_skill/src/skiros2_skill/core/processors.py index ce12075..a9ecbff 100644 --- a/skiros2_skill/src/skiros2_skill/core/processors.py +++ b/skiros2_skill/src/skiros2_skill/core/processors.py @@ -3,10 +3,13 @@ """ Collection of processor, defining how a skill visits its children """ + + class Serial(): """ @brief Process children serially. Also succeded one are re-executed """ + def printType(self): return '->' @@ -19,14 +22,16 @@ def processChildren(self, children, visitor): """ for c in children: state = c.visit(visitor) - if state!=State.Success: + if state != State.Success: return state return State.Success + class Sequential(): """ @brief Process children sequentially. Succeded ones are skipped """ + def __init__(self): self.reset() @@ -43,16 +48,18 @@ def processChildren(self, children, visitor): for i in range(self.index, len(children)): c = children[i] state = c.visit(visitor) - if state!=State.Success: + if state != State.Success: return state - self.index+=1 + self.index += 1 self.index = 0 return State.Success + class Enforce(): """ @brief Process children sequentially. Succeded ones are skipped, Failed are restarted """ + def printType(self): return '->*!' @@ -70,10 +77,12 @@ def processChildren(self, children, visitor): return State.Running return State.Success + class Selector(): """ @brief Process children sequentially. """ + def printType(self): return '?' @@ -86,21 +95,23 @@ def processChildren(self, children, visitor): """ for c in children: state = c.visit(visitor) - if state==State.Success or state==State.Running: - if state==State.Success: + if state == State.Success or state == State.Running: + if state == State.Success: self.stopAll(children, visitor) return state return state def stopAll(self, children, visitor): for c in children: - if c.state==State.Running: + if c.state == State.Running: c.visitPreempt(visitor) + class SelectorStar(): """ @brief Process children sequentially. Skips failed """ + def printType(self): return '?*' @@ -112,24 +123,26 @@ def processChildren(self, children, visitor): Serial processor - return on first running/success, or return failure """ for c in children: - if c.state==State.Failure: + if c.state == State.Failure: continue state = c.visit(visitor) - if state==State.Success or state==State.Running: - if state==State.Success: + if state == State.Success or state == State.Running: + if state == State.Success: self.stopAll(children, visitor) return state return State.Failure def stopAll(self, children, visitor): for c in children: - if c.state==State.Running: + if c.state == State.Running: c.visitPreempt(visitor) + class ParallelFf(): """ @brief Parallel First Fail - Process children in parallel. Stop all processes if a child fails. """ + def printType(self): return '|ff|' @@ -140,24 +153,26 @@ def processChildren(self, children, visitor): state = State.Success for c in children: cstate = c.visit(visitor) - if cstate==State.Running: + if cstate == State.Running: state = State.Running - if cstate==State.Idle and state!=State.Running: + if cstate == State.Idle and state != State.Running: state = State.Idle - if cstate==State.Failure: + if cstate == State.Failure: self.stopAll(children, visitor) return State.Failure return state def stopAll(self, children, visitor): for c in children: - if c.state==State.Running: + if c.state == State.Running: c.visitPreempt(visitor) + class ParallelFs(): """ Parallel First Stop - Process children in parallel. Stop all processes if a child ends (success or fail). """ + def printType(self): return '|fs|' @@ -167,16 +182,18 @@ def reset(self): def processChildren(self, children, visitor): for c in children: state = c.visit(visitor) - if state!=State.Running and state!=State.Idle: + if state != State.Running and state != State.Idle: self.stopAll(children, visitor) return state return State.Running def stopAll(self, children, visitor): for c in children: - if c.state==State.Running: + if c.state == State.Running: c.visitPreempt(visitor) -#Decorators +# Decorators + + class Loop(): def __init__(self, processor): self._processor = processor @@ -192,14 +209,16 @@ def processChildren(self, children, visitor): Repeat execution """ state = self._processor.processChildren(children, visitor) - if state==State.Failure: + if state == State.Failure: return state return State.Running + class NoFail(): """ @brief Returns only running or success """ + def __init__(self, processor): self._processor = processor @@ -214,7 +233,7 @@ def processChildren(self, children, visitor): Ignore failed execution """ state = self._processor.processChildren(children, visitor) - if state==State.Running: + if state == State.Running: return state else: return State.Success diff --git a/skiros2_skill/src/skiros2_skill/core/skill.py b/skiros2_skill/src/skiros2_skill/core/skill.py index d3f0231..1955e07 100644 --- a/skiros2_skill/src/skiros2_skill/core/skill.py +++ b/skiros2_skill/src/skiros2_skill/core/skill.py @@ -6,30 +6,34 @@ from collections import OrderedDict, defaultdict from skiros2_common.core.abstract_skill import SkillDescription, SkillCore, State, ParamOptions + class SkillPreempted(Exception): - def __init__(self, value): - self.value = value - def __str__(self): - return repr(self.value) + def __init__(self, value): + self.value = value + + def __str__(self): + return repr(self.value) + class SkillInterface(SkillCore): """ """ #--------Class functions-------- + def __init__(self, children_processor=Sequential()): super(SkillInterface, self).__init__() - #Params - self._remaps=OrderedDict() - #Connections + # Params + self._remaps = OrderedDict() + # Connections self._parent = None - self._children=[] + self._children = [] self._children_processor = children_processor - #Caches + # Caches self._was_simulated = False self._max_cache = 2 self._params_cache = [] self._input_cache = [] - self._remaps_cache=defaultdict(list) + self._remaps_cache = defaultdict(list) def __call__(self, *children): """ @@ -50,15 +54,15 @@ def getLightCopy(self): """ p = self.__class__(self._children_processor) p._children_processor = deepcopy(self._children_processor) - p._type=copy(self._type) - p._label=copy(self._label) + p._type = copy(self._type) + p._label = copy(self._label) p._params = deepcopy(self._params) p._remaps = deepcopy(self._remaps) p._description = deepcopy(self._description) p._pre_conditions = deepcopy(self._pre_conditions) p._hold_conditions = deepcopy(self._hold_conditions) p._post_conditions = deepcopy(self._post_conditions) - if self._state!=State.Uninitialized: + if self._state != State.Uninitialized: p._state = copy(self._state) p._wmi = self._wmi return p @@ -73,8 +77,8 @@ def _clearRemaps(self): """ for r1, r2 in reversed(self._remaps.items()): self.remap(r2, r1) - self._remaps=OrderedDict() - self._remaps_cache=defaultdict(list) + self._remaps = OrderedDict() + self._remaps_cache = defaultdict(list) def _copyRemaps(self, skill): """ @@ -96,7 +100,7 @@ def _revertRemaps(self): self._remaps.pop(remap[0]) self.remap(remap[1], remap[0], False) del self._remaps_cache[remapid] - except: + except BaseException: print self.printInfo(True) print self._remaps_cache raise @@ -115,7 +119,7 @@ def get_remap(self, key): """ @brief Return the key remapping if existing or otherwise the key unchanged """ - return self._remaps[key] if self._remaps.has_key(key) else key + return self._remaps[key] if key in self._remaps else key def remap(self, initial_key, target_key, record=True): """ @@ -123,25 +127,25 @@ def remap(self, initial_key, target_key, record=True): All skill's children are remapped too. """ - #Ignore harmful remappings - if self._remaps.has_key(initial_key): - if self._remaps[initial_key]==target_key:#Redundant + # Ignore harmful remappings + if initial_key in self._remaps: + if self._remaps[initial_key] == target_key: # Redundant #log.warn(self.type, "Ignoring redundant remap {}->{}".format(initial_key, target_key)) return - else:#Already remapped + else: # Already remapped log.warn(self.type, "Key {} already remapped to {}. Can t remap to {}".format(initial_key, self._remaps[initial_key], target_key)) return #log.warn("Remap", "{} {} {}. Existing: {}".format(self.type, initial_key, target_key, self._remaps)) - #The current 2->3 is related to an existing remap 1->2 + # The current 2->3 is related to an existing remap 1->2 # if initial_key in self._remaps.values(): # log.info("ChainRemap", "{}: {}->{}->{}".format(self.type, self._remaps.keys()[self._remaps.values().index(initial_key)], initial_key, target_key)) for c in self._children: c.remap(initial_key, target_key) - if self._remaps.has_key(target_key): - #Asking remap 1->2 but exists a remap 2->3. Records a remap 1->3 + if target_key in self._remaps: + # Asking remap 1->2 but exists a remap 2->3. Records a remap 1->3 target_key = self.get_remap(target_key) if self.params.hasParam(target_key): @@ -150,7 +154,7 @@ def remap(self, initial_key, target_key, record=True): if self.params.hasParam(initial_key): #log.warn("{}".format(self.type), "REMAPPING: {} {}".format(initial_key, target_key)) - #Remaps + # Remaps self._params.remap(initial_key, target_key) for c in self._pre_conditions: c.remap(initial_key, target_key) @@ -158,11 +162,11 @@ def remap(self, initial_key, target_key, record=True): c.remap(initial_key, target_key) for c in self._post_conditions: c.remap(initial_key, target_key) - #Records + # Records if record: self._remaps[initial_key] = target_key remapid = len(self._params_cache) - if remapid>0: #If I have params cached, I cache also the remap to revert it afterwards + if remapid > 0: # If I have params cached, I cache also the remap to revert it afterwards self._remaps_cache[remapid].append((initial_key, target_key)) def init(self, wmi): @@ -225,13 +229,13 @@ def getParent(self): return self._parent def hasChildren(self): - return len(self._children)>0 + return len(self._children) > 0 def addChild(self, p, latch=False): p._parent = self self._children.append(p) p._copyRemaps(self) - if latch and len(self._children)>1: + if latch and len(self._children) > 1: for c in self._children[-2]._post_conditions: for key in c.getKeys(): if not p.params.hasParam(key): @@ -270,18 +274,18 @@ def specifyParams(self, input_params, keep_default=True): self._params_cache.append(self._params.getCopy()) self._input_cache.append(input_params.getCopy()) super(SkillInterface, self).specifyParams(input_params, keep_default) - #Erase the oldest cache if the max lenght is reached - if len(self._params_cache)>self._max_cache: + # Erase the oldest cache if the max lenght is reached + if len(self._params_cache) > self._max_cache: self._popCache() def _popCache(self): self._params_cache.pop(0) - for i in range(self._max_cache+1): - if not self._remaps_cache.has_key(i): + for i in range(self._max_cache + 1): + if i not in self._remaps_cache: continue del self._remaps_cache[i] - if self._remaps_cache.has_key(i+1): - self._remaps_cache[i] = self._remaps_cache[i+1] + if i + 1 in self._remaps_cache: + self._remaps_cache[i] = self._remaps_cache[i + 1] def revertInput(self): if not self._params_cache: @@ -300,7 +304,7 @@ def tick(self): # raise Exception("Node must be started before ticking. Curr state: {}".format(self._state)) res = self.execute() # print "{} {}".format(self, res) - if res==None: + if res is None: self._setState(State.Idle) return None else: @@ -344,7 +348,7 @@ def addParam(self, key, value, param_type, options=[], description=""): See SkillDescription for more details """ - if self._remaps.has_key(key): + if key in self._remaps: key = self._remaps[key] SkillDescription.addParam(self, key, value, param_type, options, description) @@ -383,6 +387,7 @@ def setProcessor(self, processor_type): """ self._children_processor = processor_type #--------Virtual functions-------- + def hasInstance(self): """ Wrapper skills override this """ return True @@ -394,29 +399,32 @@ def execute(self): self._setProgress("End", 1) return None + class SkillWrapper(SkillInterface): """ A skill wrapper is a placeholder for a real skill It must be connected to an implementation (derived from SkillBase), that will be called when the wrapper is executed """ + def __init__(self, ptype, plabel, instanciator=None): super(SkillWrapper, self).__init__() - #Description - self._type=ptype - self._label=plabel - self._has_instance=False + # Description + self._type = ptype + self._label = plabel + self._has_instance = False self._progress_code = 0 self._progress_msg = "" if instanciator: instanciator.assignDescription(self) - ## HACK for accessing progress member variable due to shitty implementation -.- + # HACK for accessing progress member variable due to shitty implementation -.- @property def label(self): - if not self._has_instance: return super(SkillWrapper, self).label + if not self._has_instance: + return super(SkillWrapper, self).label return self._instance.label - ### END of hacky stuff + # END of hacky stuff def getLightCopy(self): """ @@ -430,9 +438,9 @@ def getLightCopy(self): p._hold_conditions = deepcopy(self._hold_conditions) p._post_conditions = deepcopy(self._post_conditions) if self._has_instance: - p._has_instance=self._has_instance - p._instance=self._instance - if self._state!=State.Uninitialized: + p._has_instance = self._has_instance + p._instance = self._instance + if self._state != State.Uninitialized: p._state = copy(self._state) p._wmi = self._wmi return p @@ -454,7 +462,7 @@ def setInstance(self, instance): self._instance = instance self._has_instance = True self._wmi = instance._wmi - #self._setState(self._instance.getState()) + # self._setState(self._instance.getState()) self._instance.modifyDescription(self) self._children = list() self._instance.specifyParams(self.getParamsNoRemaps(), False) @@ -465,9 +473,9 @@ def getInstance(self): def _copyInstanceParams(self): self._params.specifyParams(self._instance._params, False) - for k, p in self._instance._params._params.iteritems():#Hack to get remapped key back + for k, p in self._instance._params._params.iteritems(): # Hack to get remapped key back if k in self._remaps: - while self._remaps.has_key(self._remaps[k]):#Hack to work with chained remmapping + while self._remaps[k] in self._remaps: # Hack to work with chained remmapping k = self._remaps[k] self._params.specify(self._remaps[k], p.getValues()) @@ -481,7 +489,7 @@ def onPreempt(self): self._progress_msg = self._instance.progress_msg self._progress_time = self._instance.progress_time self._progress_code = self._instance.progress_code - if res!=None: + if res is not None: return res else: return self._state @@ -490,7 +498,7 @@ def onStart(self): self._progress_code = 0 self._progress_time = 0.0 self._progress_msg = "" - return self._instance.start(self.getParamsNoRemaps())==State.Running + return self._instance.start(self.getParamsNoRemaps()) == State.Running def execute(self): #print '{}:{}'.format(self._label, self._instance) @@ -500,19 +508,21 @@ def execute(self): self._progress_msg = self._instance.progress_msg self._progress_time = self._instance.progress_time self._progress_code = self._instance.progress_code - if res!=None: + if res is not None: return res else: return self._state + class SkillBase(SkillInterface, object): """ Base class for user's skills """ + def _parse_type(self, ptype): - if ptype.find("skiros:")>=0: + if ptype.find("skiros:") >= 0: return ptype - elif ptype.find(":")>=0: + elif ptype.find(":") >= 0: return "skiros" + ptype else: return "skiros:" + ptype @@ -522,7 +532,7 @@ def getLightCopy(self): Makes a light copy (only description, params and state) """ p = super(SkillBase, self).getLightCopy() - if self._state!=State.Uninitialized: + if self._state != State.Uninitialized: p._instanciator = self._instanciator return p @@ -574,15 +584,17 @@ def getNode(self, children_processor): """ return Skill(children_processor.printType(), children_processor, self._wmi) + class Root(SkillInterface): """ Special root node """ + def __init__(self, name, wmi=None): super(Root, self).__init__() - self._type=":Root" - self._label=name - self._instance = self#TODO: fix + self._type = ":Root" + self._label = name + self._instance = self # TODO: fix self._children_processor = Sequential() if wmi: self.init(wmi) @@ -591,23 +603,25 @@ def createDescription(self): return def execute(self): - if self._state==State.Idle: + if self._state == State.Idle: return State.Running - if self._state==State.Success: + if self._state == State.Success: self._setProgress("End", 1) - if self._state==State.Failure: + if self._state == State.Failure: self._setProgress("End", -1) return self._state + class Skill(SkillInterface): """ Generic skill node """ - def __init__(self, name, children_processor=Sequential(),wmi=None): + + def __init__(self, name, children_processor=Sequential(), wmi=None): super(Skill, self).__init__() - self._type=":Skill" - self._label=name - self._instance = self#TODO: fix + self._type = ":Skill" + self._label = name + self._instance = self # TODO: fix self._children_processor = children_processor if wmi: self.init(wmi) @@ -617,4 +631,4 @@ def createDescription(self): def execute(self): self._setProgress("End", 1) - return self._state \ No newline at end of file + return self._state diff --git a/skiros2_skill/src/skiros2_skill/core/skill_instanciator.py b/skiros2_skill/src/skiros2_skill/core/skill_instanciator.py index 217402a..1c8f340 100644 --- a/skiros2_skill/src/skiros2_skill/core/skill_instanciator.py +++ b/skiros2_skill/src/skiros2_skill/core/skill_instanciator.py @@ -33,10 +33,11 @@ from skiros2_common.core.abstract_skill import State from copy import deepcopy + class SkillInstanciator: def __init__(self, wmi): - self._available_descriptions={} - self._available_instances=defaultdict(list) + self._available_descriptions = {} + self._available_instances = defaultdict(list) self._wm = wmi def addDescription(self, skill): @@ -83,12 +84,12 @@ def assignInstance(self, skill, ignore_list=list()): """ to_set = None for p in self._available_instances[skill.type]: - if (p.label == skill.label or skill.label=="") and p.label not in ignore_list: + if (p.label == skill.label or skill.label == "") and p.label not in ignore_list: to_set = p - if not p.hasState(State.Running):#The skill is available, just go forward + if not p.hasState(State.Running): # The skill is available, just go forward break if to_set is not None: - if to_set.hasState(State.Running):#The skill instance is busy, create a new one + if to_set.hasState(State.Running): # The skill instance is busy, create a new one to_set = self.duplicate_instance(to_set) skill.setInstance(to_set) else: @@ -97,14 +98,13 @@ def assignInstance(self, skill, ignore_list=list()): def printState(self, verbose=True, filter_type=""): s = 'Descriptions:\n' for t, p in self._available_descriptions.iteritems(): - if p.type==filter_type or filter_type=="": + if p.type == filter_type or filter_type == "": s += p.printInfo(verbose) s += '\n' s += '\nInstances:\n' for k, l in self._available_instances.iteritems(): for p in l: - if p.type==filter_type or filter_type=="": + if p.type == filter_type or filter_type == "": s += p.printInfo(verbose) s += '\n' return s - diff --git a/skiros2_skill/src/skiros2_skill/core/skill_utils.py b/skiros2_skill/src/skiros2_skill/core/skill_utils.py index 7d7b9b8..c68461f 100644 --- a/skiros2_skill/src/skiros2_skill/core/skill_utils.py +++ b/skiros2_skill/src/skiros2_skill/core/skill_utils.py @@ -23,14 +23,14 @@ def unindend(self): self._indend -= 2 def printTree(self, skill, verbose=True): - s = "-"*self._indend + self._prefix + skill.printState(verbose) + s = "-" * self._indend + self._prefix + skill.printState(verbose) #s = "-"*self._indend + self._prefix + skill.printInfo(verbose) print s - def printParams(self, params): + def printParams(self, params): to_ret = "\n" for _, p in params.getParamMap().iteritems(): - if p.dataType() == type(Element()): + if isinstance(Element(), p.dataType()): to_ret += p._key + ": " for e in p.getValues(): to_ret += e.printState() + "\n" @@ -38,13 +38,14 @@ def printParams(self, params): to_ret += p.printState() + "\n" return to_ret + class NodeExecutor(): def __init__(self, wmi, instanciator): self._wm = wmi - self._simulate=False - self._verbose=False + self._simulate = False + self._verbose = False self._tracked_params = [] - self._params=params.ParamHandler() + self._params = params.ParamHandler() self._instanciator = instanciator def syncParams(self): @@ -52,7 +53,7 @@ def syncParams(self): vs = p.values if p.dataTypeIs(Element): for i, e in enumerate(vs): - if e.id!="": + if e.id != "": vs[i] = self._wm.get_element(e.id) p.values = vs @@ -60,7 +61,7 @@ def trackParam(self, key, prop="", relation="", print_all=False): """ Prints out every update of the requested param key -> property or relation """ - self._tracked_params.append((key, prop, relation,print_all)) + self._tracked_params.append((key, prop, relation, print_all)) def _printTracked(self, params, prefix): to_print = prefix @@ -77,11 +78,11 @@ def _printTracked(self, params, prefix): to_print += "{} {} ".format(key, e) else: to_print += key + ' not available. ' - if to_print!=prefix: + if to_print != prefix: print to_print def setSimulate(self, sim=True): - self._simulate=sim + self._simulate = sim # def specifyParams(self, input_params): # self._params.reset(input_params) @@ -100,7 +101,7 @@ def inferUnvalidParams(self, skill): for k in unvalid_params: skill._params.setDefault(k) p = skill._params.getParam(k) - if p.dataTypeIs(Element()) and p.getValue().getIdNumber()>=0: + if p.dataTypeIs(Element()) and p.getValue().getIdNumber() >= 0: skill._params.specify(k, self._wm.get_element(p.getValue()._id)) return self._autoParametrizeBB(skill) return True @@ -122,7 +123,8 @@ def _importParentsConditions(self, skill, to_resolve): for sc in skill._pre_conditions: if sc.isEqual(pc): dont_add = True - if dont_add: continue + if dont_add: + continue print "{} Adding condition {}".format(skill.type, pc.getDescription()) for key in pc.getKeys(): if not skill.params.hasParam(key): @@ -133,7 +135,7 @@ def _autoParametrizeBB(self, skill): """ @brief ground undefined parameters with parameters in the Black Board """ - to_resolve = [key for key, param in skill._params.getParamMap().iteritems() if param.paramType!=params.ParamTypes.Optional and param.dataTypeIs(Element) and param.getValue().getIdNumber() < 0] + to_resolve = [key for key, param in skill._params.getParamMap().iteritems() if param.paramType != params.ParamTypes.Optional and param.dataTypeIs(Element) and param.getValue().getIdNumber() < 0] if not to_resolve: return True log.assertInfo(self._verbose, "[Autoparametrize]", "Resolving {}:{}".format(skill.type, to_resolve)) @@ -150,24 +152,24 @@ def _autoParametrizeBB(self, skill): if p.getValue().isInstance(cp.getParamValue(key), self._wm): remap[key].append(k) else: - pass#log.info("Not instance", "{} Model: {} Match: {}".format(key, cp.getParamValue(key).printState(True), p.getValue().printState(True))) + pass # log.info("Not instance", "{} Model: {} Match: {}".format(key, cp.getParamValue(key).printState(True), p.getValue().printState(True))) l = np.zeros(len(to_resolve), dtype=int) unvalid_params = to_resolve loop = True while loop: loop = False - #Set params and check preCond + # Set params and check preCond for index, key in enumerate(to_resolve): #print '{}: {}'.format(key, remap[key]) if not remap[key]: return self._autoParametrizeWm(skill, to_resolve, cp) skill._params.specify(key, self._params.getParamValues(remap[key][l[index]])) unvalid_params = skill.checkPreCond() - #If there are unvalid params, increment 1 of the counters. Loop breaks when all counters are at last element + # If there are unvalid params, increment 1 of the counters. Loop breaks when all counters are at last element for key in unvalid_params: if key in to_resolve: - if l[to_resolve.index(key)]0 + return len(self._tree) > 0 def memorize(self, skill, tag): #self._debug("Memorize " + str(tag)) self._tree.append((skill, tag)) def hasIndex(self, index): - return abs(index)0: #Issue 3: node with childs + + if False and self.back.hasMemory() and len(procedure._children) > 0: # Issue 3: node with childs if not self.checkRule4(procedure): raise log.error("UNDO", procedure._label) - while procedure!=self.forward.recall()[0]: - self.undo() - self.undo() - while procedure!=self.forward.recall()[0]: - self.undo() + while procedure != self.forward.recall()[0]: + self.undo() + self.undo() + while procedure != self.forward.recall()[0]: + self.undo() log.error("DONE") if procedure.hasChildren(): self.redoAll() self.indend() return True - + def checkRule4(self, procedure): """ Return true if the rule 4 is respected: @@ -212,35 +215,34 @@ def checkRule4(self, procedure): raise if not self.back.hasMemory(): return True - if not self.redo(Parallel):#Issue 2: parallel and serial of all nexts... + if not self.redo(Parallel): # Issue 2: parallel and serial of all nexts... raise self.erase() return False return True - - + def processChildren(self, procedure): """ Bypass procedure processor, and use serial processor always """ return self._processor.processChildren(procedure._children, self) - + def postProcessNode(self, procedure): if not procedure.checkPostCond() and procedure.hasPostCond(): - log.info('discarded','Redundand procedure {}'.format(procedure._label)) - return True + log.info('discarded', 'Redundand procedure {}'.format(procedure._label)) + return True while not self.checkRule4(procedure): self.redo() if self._verbose: log.info("Insert procedure done. Redo all. ".format()) start_time = rospy.Time.now() - self.redoAll() + self.redoAll() self.unindend() if self._verbose: - log.info("Redoall done ({}).".format((rospy.Time.now()-start_time).to_sec())) - #self.visitor.traverse(self.getExecutionRoot()) + log.info("Redoall done ({}).".format((rospy.Time.now() - start_time).to_sec())) + # self.visitor.traverse(self.getExecutionRoot()) return True - + def processingDone(self, procedure): self.freezeExecutionTree() if not self.undoAll(): return False - return True \ No newline at end of file + return True diff --git a/skiros2_skill/src/skiros2_skill/ros/discovery_interface.py b/skiros2_skill/src/skiros2_skill/ros/discovery_interface.py index 91691dd..277a0ae 100644 --- a/skiros2_skill/src/skiros2_skill/ros/discovery_interface.py +++ b/skiros2_skill/src/skiros2_skill/ros/discovery_interface.py @@ -2,16 +2,18 @@ from skiros2_msgs.msg import ManagerDescription import rospy + class DiscoverableNode(object): """ A node that publish its name on /description whenever a msg on /discovery is delivered """ + def init_discovery(self, namespace, node_id): self._ns = namespace self._node_id = node_id self._sub_discovery = rospy.Subscriber('/{}/discovery'.format(self._ns), Empty, self._on_discovery) self._pub_description = rospy.Publisher('/{}/description'.format(self._ns), ManagerDescription, queue_size=10) - rospy.sleep(0.5)#Give time to ROS connection to initialize + rospy.sleep(0.5) # Give time to ROS connection to initialize self.publish_description() def publish_description(self, state=ManagerDescription.ACTIVE): @@ -25,10 +27,12 @@ def shutdown_discovery(self): def _on_discovery(self, msg): self.publish_description(ManagerDescription.ACTIVE) + class DiscoveryInterface(object): """ Interface to discoverable nodes """ + def init_discovery(self, namespace, callback): self._ns = namespace self._pub_discovery = rospy.Publisher('/{}/discovery'.format(self._ns), Empty, queue_size=10) diff --git a/skiros2_skill/src/skiros2_skill/ros/skill_layer_interface.py b/skiros2_skill/src/skiros2_skill/ros/skill_layer_interface.py index 26c6968..d4ff409 100644 --- a/skiros2_skill/src/skiros2_skill/ros/skill_layer_interface.py +++ b/skiros2_skill/src/skiros2_skill/ros/skill_layer_interface.py @@ -2,6 +2,7 @@ from skiros2_skill.ros.skill_manager_interface import SkillManagerInterface from discovery_interface import DiscoveryInterface + class SkillLayerInterface(DiscoveryInterface): def __init__(self, author="unknown"): self._author = author @@ -40,7 +41,7 @@ def execute(self, skill_mgr, skill_list): @brief Start a task execution """ tid = self.get_agent(skill_mgr).execute(skill_list, self._author) - if tid>=0: + if tid >= 0: self._active_sm.insert(0, skill_mgr) return tid @@ -73,19 +74,19 @@ def set_monitor_cb(self, cb): self._monitor_cb = cb def _discovery_cb(self, msg): - #TODO: make a call for changes - if msg.state==msg.ACTIVE and not self._agents.has_key(msg.name): + # TODO: make a call for changes + if msg.state == msg.ACTIVE and msg.name not in self._agents: log.info("[SkillLayerInterface]", "New skill manager detected: {}".format(msg.name)) self._agents[msg.name] = SkillManagerInterface(msg.name) self._agents[msg.name].set_monitor_cb(self._progress_cb) self._new_changes = True - elif msg.state==msg.INACTIVE and self._agents.has_key(msg.name): + elif msg.state == msg.INACTIVE and msg.name in self._agents: log.info("[SkillLayerInterface]", "Skill manager {} went down.".format(msg.name)) del self._agents[msg.name] self._new_changes = True def _progress_cb(self, msg): - if msg.label.find("task")>=0 and msg.progress_message=="End" and msg.state!=msg.IDLE: + if msg.label.find("task") >= 0 and msg.progress_message == "End" and msg.state != msg.IDLE: try: self._active_sm.remove(msg.robot) except Exception: @@ -95,4 +96,4 @@ def _progress_cb(self, msg): def print_state(self): for k, a in self._agents.iteritems(): - print k + ":" + a.printState() \ No newline at end of file + print k + ":" + a.printState() diff --git a/skiros2_skill/src/skiros2_skill/ros/skill_manager.py b/skiros2_skill/src/skiros2_skill/ros/skill_manager.py index 47e0f8c..d1445b2 100644 --- a/skiros2_skill/src/skiros2_skill/ros/skill_manager.py +++ b/skiros2_skill/src/skiros2_skill/ros/skill_manager.py @@ -19,6 +19,7 @@ log.setLevel(log.INFO) + def skill2msg(skill): msg = msgs.ResourceDescription() msg.type = skill._type @@ -26,6 +27,7 @@ def skill2msg(skill): msg.params = utils.serializeParamMap(skill._description._params.getParamMap()) return msg + class BtTicker: """ Manager of a set of Behavior Trees (Tasks) and a visitor @@ -89,8 +91,8 @@ def is_running(self): def publish_progress(self, uid, visitor): finished_skill_ids = BtTicker._finished_skill_ids - for (id,desc) in visitor.snapshot(): - if finished_skill_ids.has_key(id): + for (id, desc) in visitor.snapshot(): + if id in finished_skill_ids: if finished_skill_ids[id]['state'] == desc['state'] and finished_skill_ids[id]['msg'] == desc['msg']: continue finished_skill_ids[id] = desc @@ -134,7 +136,7 @@ def preempt(self, uid): BtTicker._tasks_to_preempt.append(uid) starttime = rospy.Time.now() timeout = rospy.Duration(5.0) - while(self.is_running() and rospy.Time.now()-starttime=0 and msg.progress_message=="End" and msg.state!=msg.IDLE: + if msg.label.find("task") >= 0 and msg.progress_message == "End" and msg.state != msg.IDLE: try: self._active_tasks.remove(int(msg.task_id)) except Exception: diff --git a/skiros2_skill/src/skiros2_skill/ros/utils.py b/skiros2_skill/src/skiros2_skill/ros/utils.py index 476d85e..e340b64 100644 --- a/skiros2_skill/src/skiros2_skill/ros/utils.py +++ b/skiros2_skill/src/skiros2_skill/ros/utils.py @@ -2,8 +2,10 @@ import skiros2_common.core.params as params import skiros2_common.ros.utils as utils + class SkillHolder(object): __slots__ = ['manager', 'type', 'name', 'ph'] + def __init__(self, manager, type_in, name, params_in=None): self.manager = manager self.name = name @@ -11,13 +13,13 @@ def __init__(self, manager, type_in, name, params_in=None): self.ph = params.ParamHandler() if params_in: self.ph.reset(params_in) - + def __str__(self): return "{}:{} [{}]".format(self.type, self.name, self.ph.printState()) - + def toMsg(self): msg = msgs.ResourceDescription() msg.type = self.type msg.name = self.name msg.params = utils.serializeParamMap(self.ph.getParamMap()) - return msg \ No newline at end of file + return msg diff --git a/skiros2_task/nodes/test_domain b/skiros2_task/nodes/test_domain index 584fc79..26299da 100755 --- a/skiros2_task/nodes/test_domain +++ b/skiros2_task/nodes/test_domain @@ -37,4 +37,4 @@ import rospkg if __name__ == '__main__': #rospy.init_node("test_pddl", anonymous=False) pddl_interface = pddl.PddlInterface(rospkg.RosPack().get_path("skiros2_task")) - print pddl_interface.invokePlanner(False) \ No newline at end of file + print pddl_interface.invokePlanner(False) diff --git a/skiros2_task/setup.py b/skiros2_task/setup.py index 69a7a17..18c4ec8 100644 --- a/skiros2_task/setup.py +++ b/skiros2_task/setup.py @@ -1,4 +1,4 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup diff --git a/skiros2_task/src/skiros2_task/core/pddl_interface.py b/skiros2_task/src/skiros2_task/core/pddl_interface.py index 59e26fe..bf0fa0e 100644 --- a/skiros2_task/src/skiros2_task/core/pddl_interface.py +++ b/skiros2_task/src/skiros2_task/core/pddl_interface.py @@ -32,6 +32,7 @@ from os import walk, remove import skiros2_common.tools.logger as log + class PddlTypes(object): __slots__ = '_types' @@ -39,9 +40,9 @@ def __init__(self): self._types = {} def addType(self, name, supertype): - if name==supertype: + if name == supertype: return - if not self._types.has_key(supertype): + if supertype not in self._types: self._types[supertype] = [] if not name in self._types[supertype]: self._types[supertype].append(name) @@ -55,11 +56,12 @@ def toPddl(self): string += ")" return string + class Predicate(object): __slots__ = 'name', 'params', 'negated', 'operator', 'value', 'abstracts' def __eq__(self, other): - if self.name!=other.name: + if self.name != other.name: return False return True @@ -67,20 +69,20 @@ def __ne__(self, other): return not self.__eq__(other) def isEqualOf(self, other): - if self.name==other.name and len(self.params)==len(other.params): + if self.name == other.name and len(self.params) == len(other.params): for p1, p2 in zip(self.params, other.params): - if not p1["key"]==p2["key"]: + if not p1["key"] == p2["key"]: return False - if self.negated==other.negated: + if self.negated == other.negated: return True return False def isNegatedOf(self, other): - if self.name==other.name and len(self.params)==len(other.params): + if self.name == other.name and len(self.params) == len(other.params): for p1, p2 in zip(self.params, other.params): - if not p1["key"]==p2["key"]: + if not p1["key"] == p2["key"]: return False - if self.negated!=other.negated: + if self.negated != other.negated: return True return False @@ -101,7 +103,7 @@ def __init__(self, predicate, params, abstracts): self.value = predicate.getProperty("skiros:desiredValue").value def isFunction(self): - return self.operator!=None and not isinstance(self.value, str) + return self.operator is not None and not isinstance(self.value, str) def toActionPddl(self, invert=False): string = '' @@ -132,6 +134,7 @@ def toUngroundPddl(self): string += ")" return string + class GroundPredicate(object): __slots__ = 'name', 'params', 'operator', 'value' @@ -142,16 +145,16 @@ def __init__(self, name, params, operator=None, value=None): self.value = value def __eq__(self, other): - #TODO: check with the functions.. - if self.name==other.name and len(self.params)==len(other.params): - return set(self.params)==set(other.params) + # TODO: check with the functions.. + if self.name == other.name and len(self.params) == len(other.params): + return set(self.params) == set(other.params) return False def __ne__(self, other): return not self.__eq__(other) def isFunction(self): - return self.operator!=None and not isinstance(self.value, str) + return self.operator is not None and not isinstance(self.value, str) def toPddl(self): string = '' @@ -168,6 +171,7 @@ def toPddl(self): string += ")" return string + class ForallPredicate(object): __slots__ = 'predicate' @@ -177,6 +181,7 @@ def __init__(self, predicate): def toPddl(self): return self.predicate + class Action(object): __slots__ = 'name', 'params', 'preconditions', 'effects' @@ -187,7 +192,7 @@ def __init__(self, skill, params, precons, postcons): self.effects = postcons def __eq__(self, other): - if self.name!=other.name: + if self.name != other.name: return False return True @@ -200,7 +205,7 @@ def toPddl(self): for p, t in self.params.iteritems(): string += "?{} - {} ".format(p, t) string += ")\n" - string +='\t:duration (= ?duration 1)\n' + string += '\t:duration (= ?duration 1)\n' string += '\t:condition (and\n' for p in self.preconditions: string += '\t\t(at start {})\n'.format(p.toActionPddl()) @@ -227,6 +232,7 @@ class PddlInterface: It generates a pddl definition and invoke a task planner """ + def __init__(self, workspace, title="untitled"): self._title = title self._workspace = workspace @@ -242,7 +248,7 @@ def clear(self): self._goal = [] def getSubTypes(self, supertype): - if self._types._types.has_key(supertype): + if supertype in self._types._types: return self._types._types[supertype] def _addSuperTypes(self, predicate): @@ -250,10 +256,10 @@ def _addSuperTypes(self, predicate): if predicate.isFunction(): lookuplist = self._functions for p in lookuplist: - if p==predicate: + if p == predicate: for param1, param2 in zip(p.params, predicate.params): - if param1["valueType"]!=param2["valueType"]: - supertypeId = p.name+param1["paramType"] + if param1["valueType"] != param2["valueType"]: + supertypeId = p.name + param1["paramType"] self._types.addType(param1["valueType"], supertypeId) self._types.addType(param2["valueType"], supertypeId) param1["valueType"] = supertypeId @@ -292,7 +298,7 @@ def setObjects(self, objects): def addInitState(self, state): for s in self._init_state: - if s==state: + if s == state: return self._init_state.append(state) @@ -301,7 +307,7 @@ def addGoal(self, g): def printDomain(self, to_file=False): string = "(define (domain {})\n".format(self._title) - string += "(:requirements :typing :fluents :universal-preconditions)\n" #TODO: make this dynamic? + string += "(:requirements :typing :fluents :universal-preconditions)\n" # TODO: make this dynamic? string += self._types.toPddl() string += "\n" string += "(:predicates \n" @@ -319,7 +325,7 @@ def printDomain(self, to_file=False): string += "\n" string += ")\n" if to_file: - with open(self._workspace+"/domain.pddl", 'w') as f: + with open(self._workspace + "/domain.pddl", 'w') as f: f.write(string) else: return string @@ -329,9 +335,9 @@ def printProblem(self, to_file=False): string += "(:objects \n" for objType, objects in self._objects.iteritems(): if len(objects): - string += '\t' - string += ' '.join(objects) - string += ' - {}\n'.format(objType) + string += '\t' + string += ' '.join(objects) + string += ' - {}\n'.format(objType) string += ")\n" string += "(:init \n" for state in self._init_state: @@ -347,7 +353,7 @@ def printProblem(self, to_file=False): string += "))\n" string += ")\n" if to_file: - with open(self._workspace+"/p01.pddl", 'w') as f: + with open(self._workspace + "/p01.pddl", 'w') as f: f.write(string) else: return string @@ -358,22 +364,23 @@ def invokePlanner(self, generate_pddl=True): self.printDomain(True) self.printProblem(True) - output = subprocess.Popen(["plan.py", "y+Y+a+T+10+t+5+e+r+O+1+C+1", self._workspace+"/domain.pddl", self._workspace+"/p01.pddl", self._workspace+"/pddlplan"], stdout=subprocess.PIPE).communicate()[0] + output = subprocess.Popen(["plan.py", "y+Y+a+T+10+t+5+e+r+O+1+C+1", self._workspace + "/domain.pddl", self._workspace + + "/p01.pddl", self._workspace + "/pddlplan"], stdout=subprocess.PIPE).communicate()[0] outpath = None for (dirpath, dirnames, filenames) in walk(self._workspace): for name in filenames: - if name.find('pddlplan')>=0: - outpath = dirpath+'/'+name + if name.find('pddlplan') >= 0: + outpath = dirpath + '/' + name if outpath: with open(outpath, 'r') as f: - data=f.read() + data = f.read() try: remove(outpath) remove("output") remove("all.groups") remove("variables.groups") remove("output.sas") - except: + except BaseException: log.warn("[TOCHECK]", "Not all files were generated while planning.") return data else: @@ -385,6 +392,6 @@ def getActionParamMap(self, name, values): """ a = self._actions[name] to_ret = dict() - for k,v in zip(a.params.keys(), values): + for k, v in zip(a.params.keys(), values): to_ret[k] = v - return to_ret \ No newline at end of file + return to_ret diff --git a/skiros2_task/src/skiros2_task/ros/task_manager.py b/skiros2_task/src/skiros2_task/ros/task_manager.py index 508686d..92a157d 100644 --- a/skiros2_task/src/skiros2_task/ros/task_manager.py +++ b/skiros2_task/src/skiros2_task/ros/task_manager.py @@ -30,7 +30,6 @@ class TaskManagerNode(PrettyObject): In case of execution failure replans until all goals are reached. """ - def __init__(self): """Initialization of the task manager. @@ -50,15 +49,14 @@ def __init__(self): self._sli.set_monitor_cb(self._onMonitorMsg) self._pddl_interface = pddl.PddlInterface(rospkg.RosPack().get_path("skiros2_task")) - self._verbose=rospy.get_param('~verbose', True) - self._assign_task_action = actionlib.SimpleActionServer('~assign_task', msgs.AssignTaskAction, execute_cb = self._assign_task_cb, auto_start = False) + self._verbose = rospy.get_param('~verbose', True) + self._assign_task_action = actionlib.SimpleActionServer('~assign_task', msgs.AssignTaskAction, execute_cb=self._assign_task_cb, auto_start=False) self._assign_task_action.start() self._sub_robot_discovery = rospy.Subscriber('/skiros/robot_discovery', Empty, self._onRobotDiscovery) self._pub_robot_description = rospy.Publisher('/skiros/robot_description', RobotDescription, queue_size=10) self._is_ready = False - @property def skills(self): """Get available skills. @@ -76,16 +74,15 @@ def skills(self): self._skills[sk] = s return self._skills - def _onMonitorMsg(self, msg): if self._assign_task_action.is_active() and self._is_ready: - if msg.label.find("task")>=0 and msg.state<3:#1 success or 2 failure - if msg.state==1: + if msg.label.find("task") >= 0 and msg.state < 3: # 1 success or 2 failure + if msg.state == 1: self._done = True self._result = msgs.AssignTaskResult(msg.state, "{}:{}".format(msg.label, msg.progress_message)) self._assign_task_action.set_succeeded(self._result) else: - if self._replan_count>20: + if self._replan_count > 20: self._assign_task_action.set_aborted() else: log.warn("Task execution failed. Replanning.") @@ -165,39 +162,37 @@ def buildTask(self, plan): self._task = list() skills = plan.splitlines() for s in skills: - s = s[s.find('(')+1: s.find(')')] + s = s[s.find('(') + 1: s.find(')')] tokens = s.split(' ') skill = deepcopy(self.skills[tokens.pop(0)]) planned_map = self._pddl_interface.getActionParamMap(skill.name, tokens) # print "{}".format(planned_map) - for k,v in planned_map.iteritems(): + for k, v in planned_map.iteritems(): skill.ph[k].setValue(self.get_element(v)) self._task.append(skill) - def initDomain(self): skills = self._wmi.resolve_elements(wmi.Element(":Skill")) for skill in skills: params = {} preconds = [] postconds = [] - #Note: Only skills with pre AND post conditions are considered for planning + # Note: Only skills with pre AND post conditions are considered for planning for p in skill.getRelations(pred="skiros:hasParam"): e = self._wmi.get_element(p['dst']) params[e._label] = e.getProperty("skiros:DataType").value for p in skill.getRelations(pred="skiros:hasPreCondition"): e = self._wmi.get_element(p['dst']) - if e._type.find("ConditionRelation")!=-1 or e._type == "skiros:ConditionProperty" or e._type == "skiros:ConditionHasProperty": - preconds.append(pddl.Predicate(e, params, e._type.find("Abs")!=-1)) + if e._type.find("ConditionRelation") != -1 or e._type == "skiros:ConditionProperty" or e._type == "skiros:ConditionHasProperty": + preconds.append(pddl.Predicate(e, params, e._type.find("Abs") != -1)) for p in skill.getRelations(pred="skiros:hasPostCondition"): e = self._wmi.get_element(p['dst']) - if e._type.find("ConditionRelation")!=-1 or e._type == "skiros:ConditionProperty" or e._type == "skiros:ConditionHasProperty": - postconds.append(pddl.Predicate(e, params, e._type.find("Abs")!=-1)) + if e._type.find("ConditionRelation") != -1 or e._type == "skiros:ConditionProperty" or e._type == "skiros:ConditionHasProperty": + postconds.append(pddl.Predicate(e, params, e._type.find("Abs") != -1)) self._pddl_interface.addAction(pddl.Action(skill, params, preconds, postconds)) if self._verbose: log.info("[Domain]", self._pddl_interface.printDomain(False)) - def get_element(self, uid): return self._elements[uid] @@ -205,11 +200,11 @@ def initProblem(self): objects = {} elements = {} self._elements = {} - #Find objects + # Find objects for objType in self._pddl_interface._types._types["thing"]: temp = self._wmi.resolve_elements(wmi.Element(objType)) elements[objType] = temp - if len(temp)>0: + if len(temp) > 0: objects[objType] = [] for e in temp: objects[objType].append(e.id) @@ -217,17 +212,17 @@ def initProblem(self): self._elements[e.id.lower()] = e for e in self._abstract_objects: ctype = self._wmi.get_super_class(e._type) - if not objects.has_key(ctype): - objects[ctype] = [] - elements[ctype] = [] + if ctype not in objects: + objects[ctype] = [] + elements[ctype] = [] e._id = e.label - if not e.label in objects[ctype]:#Avoids duplicates + if not e.label in objects[ctype]: # Avoids duplicates objects[ctype].append(e._label) elements[ctype].append(e) self._elements[e.id] = e self._elements[e.id.lower()] = e self._pddl_interface.setObjects(objects) - #Evaluate inital state + # Evaluate inital state for supertype, types in self._pddl_interface._types._types.iteritems(): elements[supertype] = [] for t in types: @@ -237,8 +232,8 @@ def initProblem(self): params.addParam("x", wm.Element(), skirosp.ParamTypes.Required) params.addParam("y", wm.Element(), skirosp.ParamTypes.Required) for p in self._pddl_interface._predicates: - if len(p.params)==1: - if p.value!=None: + if len(p.params) == 1: + if p.value is not None: c = cond.ConditionProperty("", p.name, "x", p.operator, p.value, True) else: c = cond.ConditionHasProperty("", p.name, "x", True) @@ -285,7 +280,7 @@ def initProblem(self): for line in answer: tokens = line.strip().split(" ") self._pddl_interface.addInitState(pddl.GroundPredicate(p.name, tokens)) - #TODO: add reasoner's relations calculation + # TODO: add reasoner's relations calculation for p in self._pddl_interface._functions: c = cond.ConditionProperty("", p.name, "x", p.operator, p.value, True) @@ -300,25 +295,24 @@ def initProblem(self): def setGoal(self, goal): try: for g in goal: - if g.find("forall")!=-1: + if g.find("forall") != -1: self._pddl_interface.addGoal(pddl.ForallPredicate(g)) else: g = g[1:-1] tokens = g.split(" ") - if len(tokens)==3: + if len(tokens) == 3: self._pddl_interface.addGoal(pddl.GroundPredicate(tokens[0], [tokens[1], tokens[2]])) - if tokens[1].find("-")==-1: #If isAbstractObject + if tokens[1].find("-") == -1: # If isAbstractObject self._abstract_objects.append(self._wmi.get_template_element(tokens[1])) - if tokens[2].find("-")==-1: #If isAbstractObject + if tokens[2].find("-") == -1: # If isAbstractObject self._abstract_objects.append(self._wmi.get_template_element(tokens[2])) else: self._pddl_interface.addGoal(pddl.GroundPredicate(tokens[0], [tokens[1]])) - if tokens[1].find("-")==-1: #If isAbstractObject + if tokens[1].find("-") == -1: # If isAbstractObject self._abstract_objects.append(self._wmi.get_template_element(tokens[1])) - except: + except BaseException: raise Exception("Error while parsing input goal: {}".format(goal)) - def plan(self): return self._pddl_interface.invokePlanner() @@ -333,7 +327,6 @@ def run(self): rospy.spin() - if __name__ == '__main__': node = TaskManagerNode() node.run() diff --git a/skiros2_task/src/skiros2_task/ros/task_manager_interface.py b/skiros2_task/src/skiros2_task/ros/task_manager_interface.py index 3fdaf95..b063f65 100644 --- a/skiros2_task/src/skiros2_task/ros/task_manager_interface.py +++ b/skiros2_task/src/skiros2_task/ros/task_manager_interface.py @@ -6,9 +6,10 @@ import skiros2_msgs.msg as msgs import skiros2_common.tools.logger as log + class TaskManagerInterface(PrettyObject): def __init__(self): - self._assign_task_client = actionlib.SimpleActionClient('/tm/assign_task', msgs.AssignTaskAction) + self._assign_task_client = actionlib.SimpleActionClient('/tm/assign_task', msgs.AssignTaskAction) self._assign_task_client.wait_for_server() def start_task(self, goals, done_cb=None, feedback_cb=None): @@ -20,7 +21,7 @@ def start_task(self, goals, done_cb=None, feedback_cb=None): if not self._assign_task_client.wait_for_server(rospy.Duration(0.1)): log.error("[TaskManagerInterface]", "Action server is not available.") return False - self._assign_task_client.send_goal(req, done_cb= done_cb, feedback_cb = feedback_cb) + self._assign_task_client.send_goal(req, done_cb=done_cb, feedback_cb=feedback_cb) return True def preempt(self): @@ -29,4 +30,3 @@ def preempt(self): def wait_for_result(self): self._assign_task_client.wait_for_result() return self._assign_task_client.get_result() - diff --git a/skiros2_world_model/nodes/utils/edit_scene b/skiros2_world_model/nodes/utils/edit_scene index 973c720..51f8c51 100755 --- a/skiros2_world_model/nodes/utils/edit_scene +++ b/skiros2_world_model/nodes/utils/edit_scene @@ -34,25 +34,27 @@ import sys import skiros2_world_model.ros.world_model_interface as wmi import skiros2_common.tools.logger as log + def printUsage(): - print """usage: scene [] []. -Possible commands: load, save, add. -Examples: + print """usage: scene [] []. +Possible commands: load, save, add. +Examples: edit_scene load file.owl edit_scene add large_box_test_starter""" + if __name__ == '__main__': print sys.argv - if len(sys.argv)==1: + if len(sys.argv) == 1: printUsage() else: - wmi = wmi.WorldModelInterface() - if sys.argv[1]=="save": - wmi.save(sys.argv[2]) - elif sys.argv[1]=="load": - wmi.load(sys.argv[2]) - elif sys.argv[1]=="add": - print "ID: {}".format(wmi.instanciate(sys.argv[2])._id) - else: - log.error("InputError", "Command {} not recognized".format(sys.argv[1])) - printUsage() + wmi = wmi.WorldModelInterface() + if sys.argv[1] == "save": + wmi.save(sys.argv[2]) + elif sys.argv[1] == "load": + wmi.load(sys.argv[2]) + elif sys.argv[1] == "add": + print "ID: {}".format(wmi.instanciate(sys.argv[2])._id) + else: + log.error("InputError", "Command {} not recognized".format(sys.argv[1])) + printUsage() diff --git a/skiros2_world_model/nodes/utils/simple_query b/skiros2_world_model/nodes/utils/simple_query index ddde44c..e60bc71 100755 --- a/skiros2_world_model/nodes/utils/simple_query +++ b/skiros2_world_model/nodes/utils/simple_query @@ -4,18 +4,18 @@ import skiros2_world_model.ros.world_model_interface as wmi import sys if __name__ == '__main__': - if len(sys.argv)!=4: + if len(sys.argv) != 4: print "usage: simple_query []. Example: ?x rdf:type skiros:Alternator " else: query_str = "SELECT " for i in range(1, 4): - if sys.argv[i].find("?")>-1: + if sys.argv[i].find("?") > -1: query_str += "{} ".format(sys.argv[i]) query_str += "WHERE { " - query_str += "{} {} {}".format(sys.argv[1],sys.argv[2],sys.argv[3]) + query_str += "{} {} {}".format(sys.argv[1], sys.argv[2], sys.argv[3]) query_str += ".}" wmi = wmi.WorldModelInterface() print "Query: {}".format(query_str) answer = wmi.query_ontology(query_str) for a in answer: - print "\t {}".format(a) \ No newline at end of file + print "\t {}".format(a) diff --git a/skiros2_world_model/nodes/utils/view_scene b/skiros2_world_model/nodes/utils/view_scene index 81324fb..8d64c2f 100755 --- a/skiros2_world_model/nodes/utils/view_scene +++ b/skiros2_world_model/nodes/utils/view_scene @@ -34,11 +34,13 @@ import skiros2_world_model.core.local_world_model as wm import skiros2_world_model.ros.world_model_interface as wmi import sys + def _print_recursive(wmi, name, d, indentation=""): e = d[name] print "{}->{}".format(indentation, e.printState()) for r in e.getRelations(subj="-1", pred=wmi.get_sub_properties("skiros:spatiallyRelated")): - _print_recursive(wmi, r['dst'], d, indentation+"--") + _print_recursive(wmi, r['dst'], d, indentation + "--") + def print_scene(wmi): (scene, scene_id) = wmi.get_scene() @@ -47,10 +49,11 @@ def print_scene(wmi): utility_dict[e.id] = e _print_recursive(wmi, "skiros:Scene-0", utility_dict) + if __name__ == '__main__': print "usage: view_scene []. Default: skiros:spatiallyRelated. Other common predicates: skiros:sceneProperty, skiros:skillProperty." predicate = "skiros:spatiallyRelated" - if len(sys.argv)>1: - predicate = sys.argv[1] + if len(sys.argv) > 1: + predicate = sys.argv[1] wmi = wmi.WorldModelInterface() - print_scene(wmi) \ No newline at end of file + print_scene(wmi) diff --git a/skiros2_world_model/setup.py b/skiros2_world_model/setup.py index 0379fca..76b2b34 100644 --- a/skiros2_world_model/setup.py +++ b/skiros2_world_model/setup.py @@ -1,4 +1,4 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup diff --git a/skiros2_world_model/src/skiros2_world_model/core/local_world_model.py b/skiros2_world_model/src/skiros2_world_model/core/local_world_model.py index d9282a7..3681d9a 100644 --- a/skiros2_world_model/src/skiros2_world_model/core/local_world_model.py +++ b/skiros2_world_model/src/skiros2_world_model/core/local_world_model.py @@ -7,94 +7,95 @@ from copy import copy, deepcopy import numpy as np + class WorldModel: """ This world model implementation is made to remain local and interface when necessary with the global wm """ - _id=0 - _graph=sn.Graph() - _types={} + _id = 0 + _graph = sn.Graph() + _types = {} def __init__(self, wmi, scene_name=None): self._id_gen = IdGen() self._wmi = wmi - self._keep_sync=False - self._verbose=False - self.reset(scene_name) - + self._keep_sync = False + self._verbose = False + self.reset(scene_name) + def __copy__(self): wm = WorldModel(self._wmi) - wm._verbose=self._verbose + wm._verbose = self._verbose wm._id = self._id wm._graph = self._graph wm._types = self._types return wm - + def __deepcopy__(self, memo): result = self.__copy__() memo[id(self)] = result return result - + def __enter__(self): self.syncKeep(True) return self - + def __exit__(self, type, value, traceback): self.syncKeep(False) - + def reset(self, scene_name=None): - self._id=0 - self._graph=sn.Graph() - self._types=dict() + self._id = 0 + self._graph = sn.Graph() + self._types = dict() if scene_name: root = Element(":Scene", scene_name, 0) - props = { "type" : root._type, "label" : root._label} - self._graph.add_node(dict(chain(props.items(),root._properties.items())), root._id) - + props = {"type": root._type, "label": root._label} + self._graph.add_node(dict(chain(props.items(), root._properties.items())), root._id) + def _addType(self, etype, eid): - if not self._types.has_key(etype): + if etype not in self._types: self._types[etype] = [] self._types[etype].append(eid) - + def _removeType(self, etype, eid): try: self._types[etype].remove(eid) - except: - log.error("_removeType", "No element id: {} type: {}".format(eid,etype)) - + except BaseException: + log.error("_removeType", "No element id: {} type: {}".format(eid, etype)) + def _getTypes(self, etype): """ Fast retrieval of elements of same type """ to_ret = [] - if self._types.has_key(etype): + if etype in self._types: to_ret += [self._graph.get_node(t) for t in self._types[etype] if self._graph.has_node(t)] for c in self._wmi.get_sub_classes(etype, True): - if self._types.has_key(c): + if c in self._types: to_ret += [self._graph.get_node(t) for t in self._types[c] if self._graph.has_node(t)] - #print "{} {}".format(etype, len(to_ret)) + #print "{} {}".format(etype, len(to_ret)) return to_ret - + def syncKeep(self, value=True): """ When set, the modifications are syncronized with main wm """ - self._keep_sync=value - + self._keep_sync = value + def pushElement(self, e, action): """ Update element to main wm """ if self._keep_sync: #print "Pushing {} {}".format(e.printState(), action) - if action=="add": - self._wmi.add_element(e) - elif action=="update": - e._relations = self.getContextRelations(e) - self._wmi.update_element(e) - elif action=="remove": - self._wmi.remove_element(e) - + if action == "add": + self._wmi.add_element(e) + elif action == "update": + e._relations = self.getContextRelations(e) + self._wmi.update_element(e) + elif action == "remove": + self._wmi.remove_element(e) + def pushRelation(self, sub, rel, obj, value): """ Update relation to main wm @@ -102,13 +103,13 @@ def pushRelation(self, sub, rel, obj, value): if self._keep_sync: #print "Pushing {} {} {} {}".format(sub, rel, obj, value) self._wmi.set_relation(sub, rel, obj, value) - + def sync(self): """ Pull the graph from the main world model """ - self.importGraph(self._wmi.get_branch("skiros:Scene-0")) - + self.importGraph(self._wmi.get_branch("skiros:Scene-0")) + def importGraph(self, elements): self.reset() for e in elements: @@ -116,42 +117,42 @@ def importGraph(self, elements): for e in elements: for r in e._relations: try: - if r['src']=="-1": - self._addEdge(e._id, r['type'], r['dst']) #NOTE: i have to skip passive relations...this could create problems - except: + if r['src'] == "-1": + self._addEdge(e._id, r['type'], r['dst']) # NOTE: i have to skip passive relations...this could create problems + except BaseException: log.error("[importGraph]", "Skipping relation {}. The child node was not imported.".format(r)) continue - + def importRelations(self, relations): for r in relations: self.set_relation(*r) - + def _printRecursive(self, root, indend, relation_filter): s = root.printState() print indend + s - indend = "-"*(len(indend)+len(s))+"->" - for e in self.getChildren(root._id, relation_filter):#sceneProperty + indend = "-" * (len(indend) + len(s)) + "->" + for e in self.getChildren(root._id, relation_filter): # sceneProperty self._printRecursive(e, indend, relation_filter) - + def printModel(self, relation_filter="skiros:sceneProperty"): root = self.get_element("skiros:Scene-0") - #print str(self._graph) + #print str(self._graph) self._printRecursive(root, "", relation_filter) - #nx.draw(self._graph.networkx_graph()) + # nx.draw(self._graph.networkx_graph()) return - + def getAbstractElement(self, etype, elabel): e = Element(etype, elabel) self.add_element(e, 0, 'hasAbstract') return e - + def resolve_elements2(self, keys, ph): """ Return all elements matching the profile in input (type, label, properties and relations) Keys: a key list pointing out the params to be resolved - ph: a ParamHandler class - + ph: a ParamHandler class + """ first = {} couples = {} @@ -164,45 +165,52 @@ def resolve_elements2(self, keys, ph): coupled_keys = [] overlap_keys = [] relations_done = set([]) - #Build tuples of concording parameters - for i in range(len(all_keys)):#Loop over all keys + # Build tuples of concording parameters + for i in range(len(all_keys)): # Loop over all keys key_base = all_keys[i] - if not isinstance(ph.getParamValue(key_base), Element): continue - for j in ph.getParamValue(key_base)._relations:#Loop over relation constraints + if not isinstance(ph.getParamValue(key_base), Element): + continue + for j in ph.getParamValue(key_base)._relations: # Loop over relation constraints #print j - if j["src"]=="-1":#-1 is the special autoreferencial value - key2 = j["dst"] + if j["src"] == "-1": # -1 is the special autoreferencial value + key2 = j["dst"] key = key_base - rel_id = key_base+j["type"]+j["dst"] - if rel_id in relations_done:#Skip relation with previous indexes, already considered + rel_id = key_base + j["type"] + j["dst"] + if rel_id in relations_done: # Skip relation with previous indexes, already considered continue else: #print rel_id relations_done.add(rel_id) else: key2 = key_base - key = j["src"] - rel_id = j["src"]+j["type"]+key_base - if rel_id in relations_done:#Skip relation with previous indexes, already considered + key = j["src"] + rel_id = j["src"] + j["type"] + key_base + if rel_id in relations_done: # Skip relation with previous indexes, already considered continue else: #print rel_id relations_done.add(rel_id) - if not ph.hasParam(key) or not ph.hasParam(key2): #Check necessary because at the moment ._relations contains a mix Toclean + if not ph.hasParam(key) or not ph.hasParam(key2): # Check necessary because at the moment ._relations contains a mix Toclean continue this = ph.getParamValue(key) other = ph.getParamValue(key2) #print "{} {}".format(key, key2) - if this.getIdNumber()>=0 and other.getIdNumber()>=0:#If both parameters are already set, no need to resolve.. + if this.getIdNumber() >= 0 and other.getIdNumber() >= 0: # If both parameters are already set, no need to resolve.. continue - if this.getIdNumber()>=0: set1 = [this] - else: - if ph.getParam(key).paramType()==params.ParamTypes.Optional: continue - else: set1 = first[key] - if other.getIdNumber()>=0: set2 = [other] + if this.getIdNumber() >= 0: + set1 = [this] + else: + if ph.getParam(key).paramType() == params.ParamTypes.Optional: + continue + else: + set1 = first[key] + if other.getIdNumber() >= 0: + set2 = [other] else: - if ph.getParam(key2).paramType()==params.ParamTypes.Optional: continue - else: set2 = first[key2] + if ph.getParam(key2).paramType() == params.ParamTypes.Optional: + continue + else: + set2 = first[key2] if (key, key2) in couples: temp = [np.array([e1, e2]) for e1 in set1 for e2 in set2 if bool(self.get_relations(e1._id, j["type"], e2._id)) == j['state']] if temp: @@ -210,51 +218,56 @@ def resolve_elements2(self, keys, ph): else: log.warn("resolve_elements", "No input for params {} {}. Resolving: {} {}".format(key, key2, ph.getParamValue(key).printState(True), ph.getParamValue(key2).printState(True))) else: - if key in coupled_keys: overlap_keys.append(key) - else: coupled_keys.append(key) - if key2 in coupled_keys: overlap_keys.append(key2) - else: coupled_keys.append(key2) + if key in coupled_keys: + overlap_keys.append(key) + else: + coupled_keys.append(key) + if key2 in coupled_keys: + overlap_keys.append(key2) + else: + coupled_keys.append(key2) temp = [np.array([e1, e2]) for e1 in set1 for e2 in set2 if bool(self.get_relations(e1._id, j["type"], e2._id)) == j['state']] couples[(key, key2)] = np.array(temp) if not temp: log.warn("resolve_elements", "No input for params {} {}. Resolving: {} {}".format(key, key2, ph.getParamValue(key).printState(True), ph.getParamValue(key2).printState(True))) - #Merge the tuples with an overlapping key + # Merge the tuples with an overlapping key if overlap_keys: loop = True iters = 5 - while loop:#Iterate until no shared keys are found - iters-=1 - if iters==0: + while loop: # Iterate until no shared keys are found + iters -= 1 + if iters == 0: raise loop = False coupled_keys2 = [] merged = {} - #print 'qui:' + #print 'qui:' for k1, s1 in couples.iteritems(): for k2, s2 in couples.iteritems(): shared_k = [k for k in k1 if k in k2] - if k1==k2 or not shared_k: + if k1 == k2 or not shared_k: continue loop = True skip = True for i in k1: if not i in coupled_keys2: coupled_keys2.append(i) - skip=False + skip = False for i in k2: if not i in coupled_keys2: coupled_keys2.append(i) - skip=False - if skip: continue#If it was already considered, skip - rk, rs = self._intersect(k1,k2,s1,s2, shared_k) - merged[rk] = rs#Temporary store merged tuple - for key in keys:#Add not merged tuples + skip = False + if skip: + continue # If it was already considered, skip + rk, rs = self._intersect(k1, k2, s1, s2, shared_k) + merged[rk] = rs # Temporary store merged tuple + for key in keys: # Add not merged tuples if not key in coupled_keys2: for k1, s1 in couples.iteritems(): if key in k1: - merged[k1] = s1 - couples = merged - #Add back keys that are not coupled to others + merged[k1] = s1 + couples = merged + # Add back keys that are not coupled to others for key in keys: if not key in coupled_keys: couples[key] = first[key] @@ -267,18 +280,18 @@ def resolve_elements2(self, keys, ph): for j in i: s += "{},".format(j) s += "]" - else: + else: s += "{},".format(i) print s return couples - + def _concatenate(self, a, b): if not isinstance(a, np.ndarray): a = np.array([a]) if not isinstance(b, np.ndarray): b = np.array([b]) - return np.concatenate((a,b)) - + return np.concatenate((a, b)) + def _intersect(self, k1, k2, s1, s2, shared_k): a = [k1.index(k) for k in shared_k] b = [k2.index(k) for k in shared_k] @@ -286,7 +299,7 @@ def _intersect(self, k1, k2, s1, s2, shared_k): d = np.arange(len(k2)) d = np.delete(d, b) keys = [] - #Remove constant sets + # Remove constant sets for k in k1: keys.append(k) for k in k2: @@ -298,15 +311,15 @@ def _intersect(self, k1, k2, s1, s2, shared_k): #print d for v1 in s1: for v2 in s2: - append=True + append = True for i in range(len(shared_k)): #print str(v1[a[i]].printState()) + 'vs' + str(v1[b[i]].printState()) + '=' + str(v1[a[i]]!=v2[b[i]]) - if v1[a[i]]!=v2[b[i]]: - append=False + if v1[a[i]] != v2[b[i]]: + append = False if append: sets.append(np.array(self._concatenate(v1[c], v2[d]))) return tuple(keys), np.array(sets) - + def resolve_element(self, description): """ Return all elements matching the profile in input (type, label, properties) @@ -314,22 +327,22 @@ def resolve_element(self, description): first = [] to_ret = [] #print 'description ' + description.printState(True) - #Get all nodes matching type and label + # Get all nodes matching type and label #print get_sub_classes(STMN[description._type], True) for e in self._getTypes(description._type): - if description._label=="" or description._label=="Unknown" or e['label']==description._label: + if description._label == "" or description._label == "Unknown" or e['label'] == description._label: first.append(self._makeElement(e)) - #Filter by properties + # Filter by properties for e in first: add = True for k, p in description._properties.iteritems(): if not e.hasProperty(k): add = False break - for v in p.getValues(): - if v == "" or v==None: + for v in p.getValues(): + if v == "" or v is None: break - if e.getProperty(k).find(v)<0: + if e.getProperty(k).find(v) < 0: add = False break if not add: @@ -347,56 +360,55 @@ def _makeElement(self, props): e._label = copy.pop("label") e._properties = copy return e - + def get_element(self, eid): try: eprops = self._graph.get_node(eid) except KeyError: raise KeyError("{} not found. Debug: {} {}".format(eid, self._graph, self._types)) return self._makeElement(eprops) - 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) + 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 _resolve_local_relations(self, e, lr): + + def _resolve_local_relations(self, e, lr): for r in lr: sub_e = r['dst'] sub_e.addRelation(e._id, r['type'], "-1") - if sub_e._id=="": - if self.add_element2(sub_e)<0: + if sub_e._id == "": + if self.add_element2(sub_e) < 0: log.error("[{}]".format(self.__class__.__name__), "Failed to add local element {}".format(sub_e)) else: - if self.update_element(sub_e)<0: + if self.update_element(sub_e) < 0: log.error("[{}]".format(self.__class__.__name__), "Failed to update local element {}".format(sub_e)) - + def add_element2(self, element): lr = copy(element._local_relations) element._local_relations = list() self.pushElement(element, "add") self._addNode(element) for r in element._relations: - if r['src']=="-1": + if r['src'] == "-1": self.set_relation(element._id, r['type'], r['dst'], True, push=False) else: self.set_relation(r['src'], r['type'], element._id, True, push=False) self._resolve_local_relations(element, lr) return element._id - + def add_element(self, element, parent_id, relation): self.pushElement(element, "add") self._addNode(element) self.set_relation(parent_id, relation, element._id, True, push=False) return element._id - + def update_element(self, element): for r in element._relations: - if r['src']=="-1": + if r['src'] == "-1": self.set_relation(element._id, r['type'], r['dst'], True, push=False) else: self.set_relation(r['src'], r['type'], element._id, True, push=False) @@ -404,10 +416,10 @@ def update_element(self, element): if not self._graph.has_node(element._id): log.warn("update_element", "No element found with key {}".format(element._id)) return - props = { "type" : element._type, "label" : element._label} - self._graph.add_node(dict(chain(props.items(),element._properties.items())), element._id) + props = {"type": element._type, "label": element._label} + self._graph.add_node(dict(chain(props.items(), element._properties.items())), element._id) return element._id - + def remove_element(self, eid): if self._verbose: log.debug('remove', str(eid)) @@ -415,28 +427,28 @@ def remove_element(self, eid): eprops = self._graph.get_node(eid) self._removeType(eprops["type"], eid) self._graph.remove_node(eid) - + def _check_relation(self, esubject, relation, eobject, value, push): """ Remove the old contain relation, to maintain the tree structure """ - if(self.isRelationType(relation, "skiros:sceneProperty") and value): + if(self.isRelationType(relation, "skiros:sceneProperty") and value): self.set_relation("-1", "skiros:sceneProperty", eobject, False, push) - + def isElementType(self, etype, abstract_type): - return etype==abstract_type or (self._wmi.addPrefix(etype) in self._wmi.get_sub_classes(abstract_type, True)) - + return etype == abstract_type or (self._wmi.addPrefix(etype) in self._wmi.get_sub_classes(abstract_type, True)) + def isRelationType(self, relation, rtype="skiros:sceneProperty"): #print "{}={} is {}".format(relation, rtype, relation==rtype or self._wmi.addPrefix(relation) in self._wmi.get_sub_properties(rtype, True)) - return relation==rtype or self._wmi.addPrefix(relation) in self._wmi.get_sub_properties(rtype, True) - + return relation == rtype or self._wmi.addPrefix(relation) in self._wmi.get_sub_properties(rtype, True) + 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 }) - + log.debug('add', str(esubject) + "-" + relation + "-" + str(eobject)) + self._graph.add_edge(esubject, eobject, {"type": relation}) + def set_relation(self, esubject, relation, eobject, value=True, push=True): - if self.get_relations(esubject, relation, eobject) and value: #Avoid adding twice the same statement + if self.get_relations(esubject, relation, eobject) and value: # Avoid adding twice the same statement return True self._check_relation(esubject, relation, eobject, value, push) try: @@ -447,28 +459,28 @@ def set_relation(self, esubject, relation, eobject, value=True, push=True): self._graph.remove_edge(e) if push: self.pushRelation(esubject, relation, eobject, value) - except: + except BaseException: self.printModel() raise return True - + def getAssociatedReasoner(self, relation): for cls in DiscreteReasoner.__subclasses__(): instance = cls() if relation in instance.getAssociatedRelations(): return instance return None - + def get_relations(self, esubject, relation, eobject, getId=False): rel = [] for _, edge in self._graph.get_edges().items(): - if (esubject=="" or edge['src']==esubject) and (eobject=="" or edge['dst']==eobject) and (relation=="" or self.isRelationType(edge['type'], relation)): + if (esubject == "" or edge['src'] == esubject) and (eobject == "" or edge['dst'] == eobject) and (relation == "" or self.isRelationType(edge['type'], relation)): if getId: rel.append(edge['id']) else: new_edge = deepcopy(edge) rel.append(new_edge) - if not getId and relation!="" and esubject!="" and eobject!="": + if not getId and relation != "" and esubject != "" and eobject != "": try: s = self.get_element(esubject) o = self.get_element(eobject) @@ -478,33 +490,32 @@ def get_relations(self, esubject, relation, eobject, getId=False): except KeyError: pass return rel - + def getContextRelations(self, esubject): """ Get all relations related to a subject """ rel = [] for _, edge in self._graph.get_edges().items(): - if edge['src']==esubject._id: + if edge['src'] == esubject._id: new_edge = deepcopy(edge) del new_edge['id'] - new_edge['src']="-1" + new_edge['src'] = "-1" rel.append(new_edge) - elif edge['dst']==esubject._id: + elif edge['dst'] == esubject._id: new_edge = deepcopy(edge) del new_edge['id'] - new_edge['dst']="-1" + new_edge['dst'] = "-1" rel.append(new_edge) return rel - - + def getChildren(self, eid, relation="skiros:sceneProperty"): - to_ret=[] + to_ret = [] for edge in self.get_relations(eid, relation, ""): e = self.get_element(edge['dst']) to_ret.append(e) return to_ret - + def getParent(self, eid): for edge in self.get_relations("", "skiros:sceneProperty", eid, getReasonersRel=False): - return self.get_element(edge['src']) \ No newline at end of file + return self.get_element(edge['src']) diff --git a/skiros2_world_model/src/skiros2_world_model/core/ontology_rdflib.py b/skiros2_world_model/src/skiros2_world_model/core/ontology_rdflib.py index c2c9e98..a00917a 100644 --- a/skiros2_world_model/src/skiros2_world_model/core/ontology_rdflib.py +++ b/skiros2_world_model/src/skiros2_world_model/core/ontology_rdflib.py @@ -4,12 +4,13 @@ import os.path from wrapt.decorators import synchronized + class Ontology: def __init__(self, graph=None): if graph is not None: self._ontology = graph else: - self._ontology = rdflib.ConjunctiveGraph()#store='Sleepycat' #TODO: + self._ontology = rdflib.ConjunctiveGraph() # store='Sleepycat' #TODO: def ontology(self, context_id=""): """ @@ -17,17 +18,17 @@ def ontology(self, context_id=""): @param context_id can be a rdflib.Graph, an rdflib.URIRef or a string """ - if context_id=="" or context_id==self._ontology.identifier: + if context_id == "" or context_id == self._ontology.identifier: return self._ontology else: return self._ontology.get_context(context_id) def _add_prefix(self, uri, prefix=None): if prefix is None: - prefix = uri[uri.rfind("/")+1:].lower() - if prefix.rfind(".")!=-1: + prefix = uri[uri.rfind("/") + 1:].lower() + if prefix.rfind(".") != -1: prefix = prefix[:prefix.rfind(".")] - uri = uri+"#" + uri = uri + "#" self._bind(prefix, uri) log.info("[{}]".format(self.__class__.__name__), "Set id: {} for ontology: {}".format(prefix, uri)) return prefix @@ -54,28 +55,28 @@ def uri2lightstring(self, uri): tokens = uri.split("#") for prefix, uri1 in self._ontology.namespaces(): if tokens[0] == uri1[:-1]: - return "{}:{}".format(prefix, tokens[1])#TODO: can it be optimized? + return "{}:{}".format(prefix, tokens[1]) # TODO: can it be optimized? return uri def lightstring2uri(self, name): if isinstance(name, rdflib.URIRef): return name - if name=="": + if name == "": return None if name.find("#") > 0: return name if name.find(":") < 1: - if name.find(":")==0: + if name.find(":") == 0: name = name[1:] return self.add_default_prefix(name) tokens = name.split(":") for prefix, uri in self._ontology.namespaces(): if tokens[0] == prefix: - return rdflib.term.URIRef("{}{}".format(uri, tokens[1]))#TODO: can it be optimized? + return rdflib.term.URIRef("{}{}".format(uri, tokens[1])) # TODO: can it be optimized? return rdflib.term.URIRef(name) def has_context(self, context_id): - #TODO: + # TODO: return def add_context(self, context_id, uri=None, imports=[]): @@ -105,7 +106,7 @@ def load(self, ontology_uri, context_id="", initialize=False): if initialize and context_id is not None: self._ontology.remove_context(context_id) if not context_id: - context_id = ontology_uri[ontology_uri.rfind("/")+1:ontology_uri.rfind(".")].lower() + context_id = ontology_uri[ontology_uri.rfind("/") + 1:ontology_uri.rfind(".")].lower() contextg = self._ontology.parse(ontology_uri, publicID=context_id) context = contextg.value(predicate=RDF.type, object=OWL.Ontology) if context: diff --git a/skiros2_world_model/src/skiros2_world_model/core/world_model.py b/skiros2_world_model/src/skiros2_world_model/core/world_model.py index 795225c..7bb08a2 100644 --- a/skiros2_world_model/src/skiros2_world_model/core/world_model.py +++ b/skiros2_world_model/src/skiros2_world_model/core/world_model.py @@ -9,10 +9,12 @@ from skiros2_common.tools.id_generator import IdGen from skiros2_common.tools.time_keeper import TimeKeepers + class IndividualsDataset(Ontology): """ An ontology with methods to handle individuals using the Element class """ + def __init__(self, verbose, context_id, graph=None, init=False): Ontology.__init__(self, graph) self._reasoners = {} @@ -73,18 +75,19 @@ def get_individual(self, name, context_id=""): """ subject = self.lightstring2uri(name) if not self.uri_exists(subject, context_id): - raise Exception("Element {} doesn't exist in ontology. Uri: {}. Context: {}. Existing: {}".format(name, subject, context_id, [self.uri2lightstring(s) for s in self.context.subjects(RDF.type, OWL.NamedIndividual)])) + raise Exception("Element {} doesn't exist in ontology. Uri: {}. Context: {}. Existing: {}".format( + name, subject, context_id, [self.uri2lightstring(s) for s in self.context.subjects(RDF.type, OWL.NamedIndividual)])) e = Element() for predicate, obj in self.ontology(context_id).predicate_objects(subject): - if OWL.DatatypeProperty in self.ontology().objects(predicate, RDF.type) or predicate==RDFS.comment: + if OWL.DatatypeProperty in self.ontology().objects(predicate, RDF.type) or predicate == RDFS.comment: e.setProperty(self.uri2lightstring(predicate), obj.value, self.uri2lightstring(obj.datatype), force_convertion=True) elif OWL.ObjectProperty in self.ontology().objects(predicate, RDF.type): e.addRelation("-1", self.uri2lightstring(predicate), self.uri2lightstring(obj)) - elif predicate==RDF.type and obj!=OWL.NamedIndividual: + elif predicate == RDF.type and obj != OWL.NamedIndividual: e._type = self.uri2lightstring(str(obj)) - elif obj==OWL.NamedIndividual: + elif obj == OWL.NamedIndividual: pass - elif predicate==RDFS.label: + elif predicate == RDFS.label: e._label = obj.value else: log.error("[get_individual]", "Ignoring {}-{}-{}. Predicate is not defined in the ontology.".format(name, self.uri2lightstring(predicate), self.uri2lightstring(obj))) @@ -137,7 +140,7 @@ def get_element(self, uri): """ @brief Get an element from the scene """ - if self._elements_cache.has_key(uri): + if uri in self._elements_cache: return self._elements_cache[uri] e = self.get_individual(uri, self.context.identifier) e._id = uri @@ -221,10 +224,10 @@ def update_properties(self, e, author, reasoner=None): prop_to_update = reasoner.getAssociatedData() else: prop_to_update = e.available_properties() - #Horrible hack to update also the label. Label should be moved with other properties to make this clean + # Horrible hack to update also the label. Label should be moved with other properties to make this clean old_e.label = e.label self._set((subject, RDFS.label, rdflib.term.Literal(e.label)), author) - #Set properties + # Set properties for k in prop_to_update: predicate = self.lightstring2uri(k) p = e.getProperty(k) @@ -233,7 +236,7 @@ def update_properties(self, e, author, reasoner=None): old_e.setProperty(k, values) for i in range(0, len(values)): self._add((subject, predicate, rdflib.term.Literal(values[i], datatype=self._get_datatype(p))), author) - elif old_e.getProperty(k).values!=values: + elif old_e.getProperty(k).values != values: old_e.setProperty(k, values) if values: self._set((subject, predicate, rdflib.term.Literal(values[0], datatype=self._get_datatype(p))), author) @@ -249,9 +252,9 @@ def resolve_elements(self, description): first = [] to_ret = [] for e in self._get_types(description._type): - if description._label=="" or description._label=="Unknown" or e._label==description._label: + if description._label == "" or description._label == "Unknown" or e._label == description._label: first.append(e) - #Filter by properties + # Filter by properties for e in first: add = True for k, p in description._properties.iteritems(): @@ -259,7 +262,7 @@ def resolve_elements(self, description): add = False break for v in p.values: - if v == "" or v==None: + if v == "" or v is None: break if not v in e.getProperty(k).values: add = False @@ -277,7 +280,7 @@ def remove_element(self, e, author): """ try: e = self.get_element(e.id) - except: + except BaseException: log.warn("[remove_element]", "Trying to remove element {}, but doesn't exist.".format(e.id)) return False for name, r in self._reasoners.iteritems(): @@ -296,9 +299,9 @@ def remove_recursive(self, e, author, rel_filter="", type_filter=""): """ rels_filter = [] types_filter = [] - if rel_filter!="": + if rel_filter != "": rels_filter = self.get_sub_relations(rel_filter) - if type_filter!="": + if type_filter != "": types_filter = self.get_sub_classes(type_filter) self._remove_recursive(e, author, rels_filter, types_filter) @@ -318,15 +321,15 @@ def get_recursive(self, eid, rel_filter="", type_filter=""): to_ret = {} rels_filter = [] types_filter = [] - if rel_filter!="": + if rel_filter != "": rels_filter = self.get_sub_relations(rel_filter) - if type_filter!="": + if type_filter != "": types_filter = self.get_sub_classes(type_filter) self._get_recursive(self.get_element(eid), rels_filter, types_filter, to_ret) return to_ret def get_reasoner(self, reasoner_class): - if not self._reasoners.has_key(reasoner_class): + if reasoner_class not in self._reasoners: return None return self._reasoners[reasoner_class] @@ -342,7 +345,7 @@ def _add_reasoners_prop(self, e): """ if e.hasProperty("skiros:DiscreteReasoner"): for v in e.getProperty("skiros:DiscreteReasoner").values: - if self._reasoners.has_key(v): + if v in self._reasoners: self._reasoners[v].addProperties(e) def _stop_reasoners(self): @@ -354,14 +357,14 @@ def _start_reasoners(self): r.execute() def _make_unique_uri(self, e): - if e.id=="": + if e.id == "": if not e.label: e.label = "unknown" e._id = e.label i = 1 while self.uri_exists(self.lightstring2uri(e.id)): e._id = "{}_{}".format(e.label, i) - i+=1 + i += 1 def _get_recursive(self, e, rels_filter, types_filter, elist): """ @@ -371,7 +374,7 @@ def _get_recursive(self, e, rels_filter, types_filter, elist): for r in e.getRelations("-1", rels_filter): if self.uri_exists(self.lightstring2uri(r['dst']), self.context.identifier): e2 = self.get_element(r['dst']) - if (e2._type in types_filter or not types_filter) and not elist.has_key(e2._id): + if (e2._type in types_filter or not types_filter) and e2._id not in elist: self._get_recursive(e2, rels_filter, types_filter, elist) def _get_datatype(self, param): @@ -396,8 +399,10 @@ def _set(self, statement, author, time=None, probability=1.0): """ self.context.set(statement) if self._verbose: - log.info("{}->{}".format(author, self.context.identifier.n3()), log.logColor.RED + log.logColor.BOLD + "[-] ({}) - ({}) - (*)) . ".format(self.uri2lightstring(statement[0]), self.uri2lightstring(statement[1]))) - log.info("{}->{}".format(author, self.context.identifier.n3()), log.logColor.GREEN + log.logColor.BOLD + "[+] ({}) - ({}) - ({}) .".format(self.uri2lightstring(statement[0]), self.uri2lightstring(statement[1]), self.uri2lightstring(statement[2]))) + log.info("{}->{}".format(author, self.context.identifier.n3()), log.logColor.RED + log.logColor.BOLD + + "[-] ({}) - ({}) - (*)) . ".format(self.uri2lightstring(statement[0]), self.uri2lightstring(statement[1]))) + log.info("{}->{}".format(author, self.context.identifier.n3()), log.logColor.GREEN + log.logColor.BOLD + + "[+] ({}) - ({}) - ({}) .".format(self.uri2lightstring(statement[0]), self.uri2lightstring(statement[1]), self.uri2lightstring(statement[2]))) def _remove(self, statement, author, is_relation=False): """ @@ -408,12 +413,13 @@ def _remove(self, statement, author, is_relation=False): s0 = self.uri2lightstring(statement[0]) s1 = self.uri2lightstring(statement[1]) s2 = self.uri2lightstring(statement[2]) - if self._elements_cache.has_key(s0): + if s0 in self._elements_cache: self._elements_cache[s0].removeRelation2("-1", s1, s2) - if self._elements_cache.has_key(s2): + if s2 in self._elements_cache: self._elements_cache[s2].removeRelation2(s0, s1, "-1") if self._verbose: - log.info("{}->{}".format(author, self.context.identifier.n3()), log.logColor.RED + log.logColor.BOLD + "[-] ({}) - ({}) - ({}) .".format(self.uri2lightstring(statement[0]), self.uri2lightstring(statement[1]), self.uri2lightstring(statement[2]))) + log.info("{}->{}".format(author, self.context.identifier.n3()), log.logColor.RED + log.logColor.BOLD + + "[-] ({}) - ({}) - ({}) .".format(self.uri2lightstring(statement[0]), self.uri2lightstring(statement[1]), self.uri2lightstring(statement[2]))) def _add(self, statement, author, is_relation=False): """ @@ -424,12 +430,13 @@ def _add(self, statement, author, is_relation=False): s0 = self.uri2lightstring(statement[0]) s1 = self.uri2lightstring(statement[1]) s2 = self.uri2lightstring(statement[2]) - if self._elements_cache.has_key(s0): + if s0 in self._elements_cache: self._elements_cache[s0].addRelation("-1", s1, s2) - if self._elements_cache.has_key(s2): + if s2 in self._elements_cache: self._elements_cache[s2].addRelation(s0, s1, "-1") if self._verbose: - log.info("{}->{}".format(author, self.context.identifier.n3()), log.logColor.GREEN + log.logColor.BOLD + "[+] ({}) - ({}) - ({}) . ".format(self.uri2lightstring(statement[0]), self.uri2lightstring(statement[1]), self.uri2lightstring(statement[2]))) + log.info("{}->{}".format(author, self.context.identifier.n3()), log.logColor.GREEN + log.logColor.BOLD + + "[+] ({}) - ({}) - ({}) . ".format(self.uri2lightstring(statement[0]), self.uri2lightstring(statement[1]), self.uri2lightstring(statement[2]))) def _element2statements(self, e): to_ret = [] @@ -443,7 +450,7 @@ def _element2statements(self, e): value = rdflib.term.Literal(v, datatype=self._get_datatype(p)) to_ret.append(((subject, predicate, value), False)) for r in list(e._relations): - if r['src']=="-1" or r['src']==e.id: + if r['src'] == "-1" or r['src'] == e.id: if not self.uri_exists(self.lightstring2uri(r['dst'])): log.error("[element2statements]", "Element with key {} is not defined in ontology. Skipped relation: {}".format(r['dst'], r)) e.removeRelation(r) @@ -468,10 +475,12 @@ def _get_types(self, eclass): to_ret.append(self.get_element(self.uri2lightstring(subj))) return to_ret + class WorldModel(IndividualsDataset): """ @brief A set of individuals with unique ID generation """ + def __init__(self, verbose, context_id, change_cb): self._id_gen = IdGen() self._change_cb = change_cb @@ -510,7 +519,7 @@ def _uri2type(self, uri): return uri.split('-')[0] def _uri2id(self, uri): - if uri.find('-')<0: + if uri.find('-') < 0: return -1 return int(uri.split('-')[1]) @@ -531,7 +540,7 @@ def load_context(self, filename): for i in individuals: i = self.uri2lightstring(i[0]) iid = self._uri2id(i) - if iid>=0: + if iid >= 0: self._id_gen.getId(iid) self._start_reasoners() log.info("[load_scene]", "Loaded scene {}. ".format(self.filename)) diff --git a/skiros2_world_model/src/skiros2_world_model/core/world_model_abstract_interface.py b/skiros2_world_model/src/skiros2_world_model/core/world_model_abstract_interface.py index 19def38..40a45b9 100644 --- a/skiros2_world_model/src/skiros2_world_model/core/world_model_abstract_interface.py +++ b/skiros2_world_model/src/skiros2_world_model/core/world_model_abstract_interface.py @@ -28,6 +28,7 @@ # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ################################################################################# + class OntologyAbstractInterface: def lock(self): """ Not implemented in abstract class. """ @@ -56,6 +57,7 @@ def add_individual(self, element, ontology_name): def add_prefix(self, parent_class): """ Not implemented in abstract class. """ raise NotImplementedError("Not implemented in abstract class") + def remove_prefix(self, parent_class): """ Not implemented in abstract class. """ raise NotImplementedError("Not implemented in abstract class") @@ -72,6 +74,7 @@ def get_sub_properties(self, parent_property, recursive=False): """ Not implemented in abstract class. """ raise NotImplementedError("Not implemented in abstract class") + class WorldModelAbstractInterface: def add_element(self, e): """ Not implemented in abstract class. """ diff --git a/skiros2_world_model/src/skiros2_world_model/ros/ontology_interface.py b/skiros2_world_model/src/skiros2_world_model/ros/ontology_interface.py index 796aa30..7dbe6b9 100644 --- a/skiros2_world_model/src/skiros2_world_model/ros/ontology_interface.py +++ b/skiros2_world_model/src/skiros2_world_model/ros/ontology_interface.py @@ -37,10 +37,12 @@ from skiros2_common.core.world_element import Element from skiros2_world_model.core.world_model_abstract_interface import OntologyAbstractInterface + class OntologyInterface(OntologyAbstractInterface): """ Interface for ontology services on a world model node """ + def __init__(self, author_name="test"): self._author_name = author_name self._lock = rospy.ServiceProxy('wm/lock', SetBool) @@ -75,7 +77,6 @@ def unlock(self): """ return self._call(self._lock, SetBoolRequest(False)).success - def add_class(self, class_uri, parent_uri, context=""): req = srvs.WoModifyRequest() req.context = context @@ -94,7 +95,7 @@ def add_individual(self, element, context=""): """ @brief Add an individual """ - #TODO: + # TODO: pass def add_ontology(self, uri): @@ -175,9 +176,9 @@ def add_prefix(self, uri): return uri def remove_prefix(self, parent_class): - if parent_class.find("#")>=0: + if parent_class.find("#") >= 0: return parent_class.split("#")[-1] - elif parent_class.find(":")>=0: + elif parent_class.find(":") >= 0: return parent_class.split(":")[-1] else: return parent_class @@ -191,10 +192,10 @@ def get_individuals(self, parent_class, recursive=True): if recursive: return self.query_ontology("SELECT ?x WHERE { ?x rdf:type/rdfs:subClassOf* " + self.add_prefix(parent_class) + " . } ") else: - return self.query_ontology("SELECT ?x where {?x rdf:type+ "+self.add_prefix(parent_class)+"}") + return self.query_ontology("SELECT ?x where {?x rdf:type+ " + self.add_prefix(parent_class) + "}") def get_type(self, uri): - return self.query_ontology("SELECT ?x where {"+self.add_prefix(uri)+" rdf:type ?x}") + return self.query_ontology("SELECT ?x where {" + self.add_prefix(uri) + " rdf:type ?x}") def get_triples(self, subj=None, pred=None, obj=None): """ @@ -202,19 +203,25 @@ def get_triples(self, subj=None, pred=None, obj=None): Note: at least one between subj, pred or obj must left blank for this function to work. """ - if subj: subj = self.add_prefix(subj) - else: subj = "?x" - if pred: pred = self.add_prefix(pred) - else: pred = "?y" - if obj: obj = self.add_prefix(obj) - else: obj = "?z" - return self.query_ontology("SELECT * WHERE { "+ "{} {} {}".format(subj, pred, obj)+" . } ") + if subj: + subj = self.add_prefix(subj) + else: + subj = "?x" + if pred: + pred = self.add_prefix(pred) + else: + pred = "?y" + if obj: + obj = self.add_prefix(obj) + else: + obj = "?z" + return self.query_ontology("SELECT * WHERE { " + "{} {} {}".format(subj, pred, obj) + " . } ") def get_super_class(self, child_class): """ @brief Return the parent class of child_class """ - to_ret = self.query_ontology("SELECT ?x WHERE { "+ self.add_prefix(child_class) +" rdfs:subClassOf ?x. } ") + to_ret = self.query_ontology("SELECT ?x WHERE { " + self.add_prefix(child_class) + " rdfs:subClassOf ?x. } ") if not to_ret: log.error("[get_super_class]", "No super class found for {}".format(child_class)) return to_ret[0] @@ -223,7 +230,7 @@ def get_sub_classes(self, parent_class, recursive=True): """ @brief Return the child classes of parent_class. If recursive=True, returns also sub childs classes """ - if(self._sub_classes_cache.has_key(parent_class)): + if(parent_class in self._sub_classes_cache): return self._sub_classes_cache[parent_class] if recursive: to_ret = self.query_ontology("SELECT ?x WHERE { ?x rdfs:subClassOf* " + self.add_prefix(parent_class) + " . } ") @@ -236,7 +243,7 @@ def get_sub_properties(self, parent_property, recursive=True): """ @brief Return the child properties of parent_property. If recursive=True, returns also sub childs properties """ - if(self._sub_properties_cache.has_key(parent_property)): + if(parent_property in self._sub_properties_cache): return self._sub_properties_cache[parent_property] if recursive: to_ret = self.query_ontology("SELECT ?x WHERE { ?x rdfs:subPropertyOf* " + self.add_prefix(parent_property) + " . } ") @@ -249,6 +256,6 @@ def _call(self, service, msg): try: resp1 = service(msg) return resp1 - except rospy.ServiceException, e: - print "Service call failed: %s"%e - raise Exception("Service call failed: %s"%e) \ No newline at end of file + except rospy.ServiceException as e: + print "Service call failed: %s" % e + raise Exception("Service call failed: %s" % e) diff --git a/skiros2_world_model/src/skiros2_world_model/ros/ontology_server.py b/skiros2_world_model/src/skiros2_world_model/ros/ontology_server.py index 8ee5821..207f245 100644 --- a/skiros2_world_model/src/skiros2_world_model/ros/ontology_server.py +++ b/skiros2_world_model/src/skiros2_world_model/ros/ontology_server.py @@ -38,11 +38,12 @@ from threading import Lock from pyparsing import ParseException + class OntologyServer(object): def __init__(self, anonymous=False): rospy.init_node("ontology", anonymous=anonymous) self._verbose = rospy.get_param('~verbose', False) - self._ontology = Ontology() + self._ontology = Ontology() self.init_ontology_services() def init_ontology_services(self): @@ -61,7 +62,7 @@ def _lock_cb(self, msg): else: try: self._mutex.release() - except: + except BaseException: return SetBoolResponse(False, "Mutex already unlocked.") return SetBoolResponse(True, "Ok") @@ -79,12 +80,12 @@ def _wo_query_cb(self, msg): temp += self._ontology.uri2lightstring(r) else: temp += r.n3() - if len(s)>1: + if len(s) > 1: temp += " " to_ret.answer.append(temp) log.assertInfo(self._verbose, "[WoQuery]", "Answer: {}. Time: {:0.3f} sec".format(to_ret.answer, self._times.getLast())) except (AttributeError, ParseException) as e: - #TODO: test if the bug is fixed, and remove the exception handling + # TODO: test if the bug is fixed, and remove the exception handling log.error("[WoQuery]", "Parse error with following query: {}.".format(msg.query_string)) raise e return to_ret @@ -102,6 +103,7 @@ def _wo_modify_cb(self, msg): def run(self): rospy.spin() + if __name__ == '__main__': node = OntologyServer() node.run() diff --git a/skiros2_world_model/src/skiros2_world_model/ros/world_model_interface.py b/skiros2_world_model/src/skiros2_world_model/ros/world_model_interface.py index 7eee903..387bed1 100644 --- a/skiros2_world_model/src/skiros2_world_model/ros/world_model_interface.py +++ b/skiros2_world_model/src/skiros2_world_model/ros/world_model_interface.py @@ -6,6 +6,7 @@ import copy import numpy as np + class WorldModelInterface(OntologyInterface, WorldModelAbstractInterface): """ Interface for scene services on a world model node @@ -25,7 +26,7 @@ def __init__(self, author_name="test", make_cache=False): def _monitor_cb(self, msg): if self._make_cache: - if self._last_snapshot_id!=msg.prev_snapshot_id or msg.action=='reset': + 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: @@ -33,15 +34,15 @@ def _monitor_cb(self, msg): 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 WorldModelInterface._elements_cache.has_key(elem.id): + 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 WorldModelInterface._elements_cache.has_key(rel['src']): + if rel['src'] in WorldModelInterface._elements_cache: del WorldModelInterface._elements_cache[rel['src']] - if WorldModelInterface._elements_cache.has_key(rel['dst']): + if rel['dst'] in WorldModelInterface._elements_cache: del WorldModelInterface._elements_cache[rel['dst']] if self._external_monitor_cb: self._external_monitor_cb(msg) @@ -85,11 +86,11 @@ def _resolve_local_relations(self, e): for r in e._local_relations: sub_e = r['dst'] sub_e.addRelation(e._id, r['type'], "-1") - if sub_e._id=="": - if self.add_element(sub_e)<0: + if sub_e._id == "": + if self.add_element(sub_e) < 0: log.error("[{}]".format(self.__class__.__name__), "Failed to add local element {}".format(sub_e)) else: - if self.update_element(sub_e)<0: + if self.update_element(sub_e) < 0: log.error("[{}]".format(self.__class__.__name__), "Failed to update local element {}".format(sub_e)) e._local_relations = list() @@ -203,7 +204,7 @@ def instanciate(self, uri, recursive=False, relations=list(), relation_filter=[" if recursive: antiloop_bind.add(template.id) for r in relcopy: - if (r['type'] in relation_filter or not relation_filter) and r['src']=="-1": + if (r['type'] in relation_filter or not relation_filter) and r['src'] == "-1": if not r['dst'] in antiloop_bind: rcopy = copy.deepcopy(r) rcopy['src'] = template.id @@ -224,7 +225,7 @@ def get_template_element(self, uri, context_id='scene'): return utils.msg2element(res.elements[0]) def get_element(self, eid, context_id='scene'): - if not WorldModelInterface._elements_cache.has_key(eid): + if eid not in WorldModelInterface._elements_cache: msg = srvs.WmGetRequest() e = msgs.WmElement() e.id = eid @@ -260,16 +261,16 @@ def get_reasoner_relations(self, subj, pred, obj): try: reasoner = subj._getReasoner(pred) if pred in reasoner.computeRelations(subj, obj): - return [{"src": subj.id, "type": pred, "dst": obj.id}] + return [{"src": subj.id, "type": pred, "dst": obj.id}] else: return list() except KeyError: - #No reasoner associated with the relation + # No reasoner associated with the relation pass return None def get_relations(self, subj, pred, obj): - if pred!="" and subj!="" and obj!="" and subj!="-1" and obj!="-1": + if pred != "" and subj != "" and obj != "" and subj != "-1" and obj != "-1": subj = self.get_element(subj) obj = self.get_element(obj) rels = self.get_reasoner_relations(subj, pred, obj) @@ -316,17 +317,18 @@ def resolve_elements2(self, keys, ph, verbose=False): coupled_keys = [] overlap_keys = [] relations_done = set([]) - #Build tuples of concording parameters - for i in range(len(all_keys)):#Loop over all keys + # Build tuples of concording parameters + for i in range(len(all_keys)): # Loop over all keys key_base = all_keys[i] - if not isinstance(ph.getParamValue(key_base), Element): continue - for j in ph.getParamValue(key_base)._relations:#Loop over relation constraints + if not isinstance(ph.getParamValue(key_base), Element): + continue + for j in ph.getParamValue(key_base)._relations: # Loop over relation constraints #print j - if j["src"]=="-1":#-1 is the special autoreferencial value + if j["src"] == "-1": # -1 is the special autoreferencial value key2 = j["dst"] key = key_base - rel_id = key_base+j["type"]+j["dst"] - if rel_id in relations_done:#Skip relation with previous indexes, already considered + rel_id = key_base + j["type"] + j["dst"] + if rel_id in relations_done: # Skip relation with previous indexes, already considered continue else: #print rel_id @@ -334,30 +336,32 @@ def resolve_elements2(self, keys, ph, verbose=False): else: key2 = key_base key = j["src"] - rel_id = j["src"]+j["type"]+key_base - if rel_id in relations_done:#Skip relation with previous indexes, already considered + rel_id = j["src"] + j["type"] + key_base + if rel_id in relations_done: # Skip relation with previous indexes, already considered continue else: #print rel_id relations_done.add(rel_id) - if not ph.hasParam(key) or not ph.hasParam(key2): #Check necessary because at the moment ._relations contains a mix Toclean + if not ph.hasParam(key) or not ph.hasParam(key2): # Check necessary because at the moment ._relations contains a mix Toclean continue this = ph.getParamValue(key) other = ph.getParamValue(key2) - if this.getIdNumber()>=0 and other.getIdNumber()>=0:#If both parameters are already set, no need to resolve.. + if this.getIdNumber() >= 0 and other.getIdNumber() >= 0: # If both parameters are already set, no need to resolve.. continue - if this.getIdNumber()>=0: set1 = [this] + if this.getIdNumber() >= 0: + set1 = [this] else: - if ph.getParam(key).paramType==params.ParamTypes.Optional: + if ph.getParam(key).paramType == params.ParamTypes.Optional: abstract = ph.getParam(key).value other = ph.getParam(key2).value abstract._id = abstract.label set1 = [abstract] else: set1 = first[key] - if other.getIdNumber()>=0: set2 = [other] + if other.getIdNumber() >= 0: + set2 = [other] else: - if ph.getParam(key2).paramType==params.ParamTypes.Optional: + if ph.getParam(key2).paramType == params.ParamTypes.Optional: abstract = ph.getParam(key2).value other = ph.getParam(key).value abstract._id = abstract.label @@ -369,26 +373,32 @@ def resolve_elements2(self, keys, ph, verbose=False): if temp: try: couples[(key, key2)] = np.concatenate(couples[(key, key2)], np.array(temp)) - except: + except BaseException: log.error("", "MERGING: {} and {} ".format(couples[(key, key2)], np.array(temp))) else: - log.warn("resolve_elements", "No input for params {} {}. Resolving: {} {}".format(key, key2, ph.getParamValue(key).printState(verbose), ph.getParamValue(key2).printState(verbose))) + log.warn("resolve_elements", "No input for params {} {}. Resolving: {} {}".format( + key, key2, ph.getParamValue(key).printState(verbose), ph.getParamValue(key2).printState(verbose))) else: - if key in coupled_keys: overlap_keys.append(key) - else: coupled_keys.append(key) - if key2 in coupled_keys: overlap_keys.append(key2) - else: coupled_keys.append(key2) + if key in coupled_keys: + overlap_keys.append(key) + else: + coupled_keys.append(key) + if key2 in coupled_keys: + overlap_keys.append(key2) + else: + coupled_keys.append(key2) temp = [np.array([e1, e2]) for e1 in set1 for e2 in set2 if self.check_relation(e1, j["type"], e2, j['state'], j['abstract'])] couples[(key, key2)] = np.array(temp) if not temp: - log.warn("resolve_elements", "No input for params {} {}. Resolving: {} {}".format(key, key2, ph.getParamValue(key).printState(verbose), ph.getParamValue(key2).printState(verbose))) - #Merge the tuples with an overlapping key + log.warn("resolve_elements", "No input for params {} {}. Resolving: {} {}".format( + key, key2, ph.getParamValue(key).printState(verbose), ph.getParamValue(key2).printState(verbose))) + # Merge the tuples with an overlapping key if overlap_keys: loop = True iters = 5 - while loop:#Iterate until no shared keys are found - iters-=1 - if iters==0: + while loop: # Iterate until no shared keys are found + iters -= 1 + if iters == 0: raise loop = False coupled_keys2 = [] @@ -397,28 +407,29 @@ def resolve_elements2(self, keys, ph, verbose=False): for k1, s1 in couples.iteritems(): for k2, s2 in couples.iteritems(): shared_k = [k for k in k1 if k in k2] - if k1==k2 or not shared_k: + if k1 == k2 or not shared_k: continue loop = True skip = True for i in k1: if not i in coupled_keys2: coupled_keys2.append(i) - skip=False + skip = False for i in k2: if not i in coupled_keys2: coupled_keys2.append(i) - skip=False - if skip: continue#If it was already considered, skip - rk, rs = self._intersect(k1,k2,s1,s2, shared_k) - merged[rk] = rs#Temporary store merged tuple - for key in keys:#Add not merged tuples + skip = False + if skip: + continue # If it was already considered, skip + rk, rs = self._intersect(k1, k2, s1, s2, shared_k) + merged[rk] = rs # Temporary store merged tuple + for key in keys: # Add not merged tuples if not key in coupled_keys2: for k1, s1 in couples.iteritems(): if key in k1: merged[k1] = s1 couples = merged - #Add back keys that are not coupled to others + # Add back keys that are not coupled to others for key in keys: if not key in coupled_keys: couples[key] = first[key] @@ -441,7 +452,7 @@ def _concatenate(self, a, b): a = np.array([a]) if not isinstance(b, np.ndarray): b = np.array([b]) - return np.concatenate((a,b)) + return np.concatenate((a, b)) def _intersect(self, k1, k2, s1, s2, shared_k): a = [k1.index(k) for k in shared_k] @@ -450,7 +461,7 @@ def _intersect(self, k1, k2, s1, s2, shared_k): d = np.arange(len(k2)) d = np.delete(d, b) keys = [] - #Remove constant sets + # Remove constant sets for k in k1: keys.append(k) for k in k2: @@ -462,11 +473,11 @@ def _intersect(self, k1, k2, s1, s2, shared_k): #print d for v1 in s1: for v2 in s2: - append=True + append = True for i in range(len(shared_k)): #print str(v1[a[i]].printState()) + 'vs' + str(v1[b[i]].printState()) + '=' + str(v1[a[i]]!=v2[b[i]]) - if v1[a[i]]!=v2[b[i]]: - append=False + if v1[a[i]] != v2[b[i]]: + append = False if append: sets.append(np.array(self._concatenate(v1[c], v2[d]))) return tuple(keys), np.array(sets) diff --git a/skiros2_world_model/src/skiros2_world_model/ros/world_model_server.py b/skiros2_world_model/src/skiros2_world_model/ros/world_model_server.py index 6bac6f4..970e294 100644 --- a/skiros2_world_model/src/skiros2_world_model/ros/world_model_server.py +++ b/skiros2_world_model/src/skiros2_world_model/ros/world_model_server.py @@ -12,11 +12,12 @@ import uuid from time import sleep + class WorldModelServer(OntologyServer): def __init__(self, anonymous=False): self._monitor = None rospy.init_node("wm", anonymous=anonymous) - rospy.on_shutdown(self._wait_clients_disconnection)#TODO: make this work + rospy.on_shutdown(self._wait_clients_disconnection) # TODO: make this work self._verbose = rospy.get_param('~verbose', False) self.contexts = dict() self._ontology = WorldModel(self._verbose, 'scene', self._wm_change_cb) @@ -25,7 +26,7 @@ def __init__(self, anonymous=False): self._init_wm() self._load_reasoners() #================Snapshot====================== - self._curr_snapshot = uuid.uuid4() # random UUID + self._curr_snapshot = uuid.uuid4() # random UUID #self._snapshots_log = [] #================ROS====================== self._set_relation = rospy.Service('~scene/set_relation', srvs.WmSetRelation, self._wm_set_rel_cb) @@ -38,16 +39,16 @@ def __init__(self, anonymous=False): def _init_wm(self): rospack = rospkg.RosPack() - self._skiros_dir = rospack.get_path('skiros2')+'/owl' + self._skiros_dir = rospack.get_path('skiros2') + '/owl' self._workspace = rospy.get_param('~workspace_dir', self._skiros_dir) for (dirpath, dirnames, filenames) in walk(self._skiros_dir): for name in filenames: - if name.find('.owl')>=0: - self._ontology.load(dirpath+'/'+name) + if name.find('.owl') >= 0: + self._ontology.load(dirpath + '/' + name) for (dirpath, dirnames, filenames) in walk(self._workspace): for name in filenames: - if name.find('.owl')>=0: - self._ontology.load(dirpath+'/'+name) + if name.find('.owl') >= 0: + self._ontology.load(dirpath + '/' + name) if not self._workspace: self._workspace = self._skiros_dir self._ontology.workspace = self._workspace @@ -55,16 +56,16 @@ def _init_wm(self): self._ontology.set_default_prefix('skiros', 'http://rvmi.aau.dk/ontologies/skiros.owl#') init_scene = rospy.get_param('~init_scene', "") self._ontology.reset() - if init_scene!="": + if init_scene != "": self._ontology.load_context(init_scene) for context in rospy.get_param('~load_contexts', []): context_id, filename = context.split(" ") log.info("[{}]".format(self.__class__.__name__), "Loading context {} from {}".format(context_id, filename)) graph = self._get_context(context_id) graph.load_context(filename) - #Clear prefixes from autogenerated default + # Clear prefixes from autogenerated default for prefix, uri1 in self._ontology._ontology.namespace_manager.store.namespaces(): - if prefix.find("default")>-1: + if prefix.find("default") > -1: self._ontology._bind(prefix, "") self._ontology._bind("", "") @@ -72,16 +73,16 @@ def _load_reasoners(self): """ Load reasoner plugins """ - #Load plugins descriptions + # Load plugins descriptions for package in rospy.get_param('~reasoners_pkgs', []): self._plug_loader.load(package, DiscreteReasoner) - #TODO: load reasoners in context + # TODO: load reasoners in context for p in self._plug_loader: self._ontology.load_reasoner(p) def _wait_clients_disconnection(self): if self._monitor: - while self._monitor.get_num_connections()>0: + while self._monitor.get_num_connections() > 0: sleep(0.1) def _wm_change_cb(self, author, action, element=None, relation=None): @@ -91,10 +92,10 @@ def _wm_change_cb(self, author, action, element=None, relation=None): self._publish_change(author, action, relation=utils.relation2msg(relation)) def _publish_change(self, author, action, elements=None, relation=None, context_id='scene'): - if context_id=='scene': - msg = msgs.WmMonitor() + if context_id == 'scene': + msg = msgs.WmMonitor() msg.prev_snapshot_id = self._curr_snapshot.hex - self._curr_snapshot = uuid.uuid4() # random UUID + self._curr_snapshot = uuid.uuid4() # random UUID msg.snapshot_id = self._curr_snapshot.hex msg.stamp = rospy.Time.now() msg.author = author @@ -106,7 +107,7 @@ def _publish_change(self, author, action, elements=None, relation=None, context_ self._monitor.publish(msg) def _get_context(self, context_id): - if not self.contexts.has_key(context_id): + if context_id not in self.contexts: log.info("[get_context]", "Creating context: {}.".format(context_id)) self.contexts[context_id] = IndividualsDataset(self._verbose, context_id, self._ontology._ontology) self.contexts[context_id].set_default_prefix('skiros', 'http://rvmi.aau.dk/ontologies/skiros.owl#') @@ -115,9 +116,9 @@ def _get_context(self, context_id): def _load_and_save_cb(self, msg): with self._times: - if msg.action==msg.SAVE: + if msg.action == msg.SAVE: self._get_context(msg.context).save_context(msg.filename) - elif msg.action==msg.LOAD: + elif msg.action == msg.LOAD: self._get_context(msg.context).load_context(msg.filename) self._publish_change("", "reset", elements=[], context_id=msg.context) else: @@ -127,7 +128,7 @@ def _load_and_save_cb(self, msg): return srvs.WoLoadAndSaveResponse(True) def _wm_query_rel_cb(self, msg): - #TODO: get rid of this. Replace implementation with a standard SPARQL query + # TODO: get rid of this. Replace implementation with a standard SPARQL query to_ret = srvs.WmQueryRelationsResponse() with self._times: to_ret.matches = [utils.relation2msg(x) for x in self._ontology.get_relations(utils.msg2relation(msg.relation))] From 84f200babfd803567905b5f0e98ca00523ec102d Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Wed, 10 Apr 2019 15:05:16 +0200 Subject: [PATCH 2/2] Enabled empty configuration function for skiros_widget. skiros_gui dies if rqt configuration button is pressed and this function doesn't exist. --- skiros2_gui/src/skiros2_gui/core/skiros_widget.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/skiros2_gui/src/skiros2_gui/core/skiros_widget.py b/skiros2_gui/src/skiros2_gui/core/skiros_widget.py index c3de464..ea3027e 100644 --- a/skiros2_gui/src/skiros2_gui/core/skiros_widget.py +++ b/skiros2_gui/src/skiros2_gui/core/skiros_widget.py @@ -357,11 +357,11 @@ def restore_settings(self, plugin_settings, instance_settings): self.last_executed_skill = instance_settings.value("last_executed_skill") print(self.last_executed_skill) -# def trigger_configuration(self): -# # Comment in to signal that the plugin has a way to configure -# # This will enable a setting button (gear icon) in each dock widget title bar -# # Usually used to open a modal configuration dialog -# pass + def trigger_configuration(self): + # Comment in to signal that the plugin has a way to configure + # This will enable a setting button (gear icon) in each dock widget title bar + # Usually used to open a modal configuration dialog + pass #==============================================================================