def __import_plugins(self, package, base): results = [] if isinstance(package, str): package = importlib.import_module(package) for loader, modname, is_pkg in pkgutil.walk_packages( path=package.__path__, prefix=package.__name__ + '.', onerror=lambda x: None): if is_pkg: continue if modname not in sys.modules: try: importlib.import_module(modname) except ImportError, e: log.warn( "[ImportError]", "Module {} not loaded, some dependencies are missing: {}" .format(modname, e)) continue module = sys.modules[modname] 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())
def load(self, folder, base_class): self._plugins += self.__import_plugins(folder, base_class) if self.size() == 0: log.warn( self.__class__.__name__, "No instances of {} found in {}! Check your package configuration." .format(base_class, folder))
def _update_wm_node(self, elem, cur_item_id): # check if element is already in tree items = self.wm_tree_widget.findItems(elem.id, Qt.MatchRecursive | Qt.MatchFixedString, 1) if not items: return item = items[0] # check if updated item is selected if elem.id == cur_item_id: # update properties table if selected item has changed self.wm_properties_widget.blockSignals(True) self.fill_properties_table(elem) self.wm_properties_widget.blockSignals(False) # get parent node in tree parent = item.parent() if not parent: return # check if the old parent is still parent of the updated element parent_rel = elem.getRelation(pred=self._wmi.get_sub_properties('skiros:spatiallyRelated'), obj='-1') if not parent.text(1) in parent_rel['src']: # elem moved spatially item.parent().removeChild(item) parents = self.wm_tree_widget.findItems(parent_rel['src'], Qt.MatchRecursive | Qt.MatchFixedString, 1) if not parents: log.warn("[update_wm_node]", "No parent found for {}".format(elem.id)) return item.setText(0, utils.ontology_type2name(elem.id) if not elem.label else utils.ontology_type2name(elem.label)) item.setText(1, elem.id) parents[0].addChild(item)
def _assign_task_cb(self, msg): """Callback for setting new goals. Executed whenever we receive a service call to set a new goal. Args: msg (skiros2_msgs.srv.TmSetGoals): Service message containing the goals """ try: log.info("[Goal]", msg.goals) self._current_goals = msg.goals plan = self._task_plan() log.info("[Plan]", plan) if plan is None: log.warn( self.class_name, "Planning failed for goals: {}".format( self._current_goals)) self._result = msgs.AssignTaskResult(1, "Planning failed.") self._assign_task_action.set_aborted(self._result) return if not plan: self._result = msgs.AssignTaskResult(2, "No skills to execute.") self._assign_task_action.set_succeeded(self._result) return task = self.build_task(plan) self._result = msgs.AssignTaskResult(3, task.toJson()) self._assign_task_action.set_succeeded(self._result) return except Exception, e: self._result = msgs.AssignTaskResult(1, str(e)) self._assign_task_action.set_aborted(self._result)
def revertInput(self): if not self._params_cache: log.warn("revertInput", "No cache available, can't revert input.") return None self._revertRemaps() self._params.reset(deepcopy(self._params_cache.pop())) return deepcopy(self._input_cache.pop())
def invokePlanner(self, generate_pddl=True): #subprocess.call(["plan.py", "y+Y+a+T+10+t+5+e+r+O+1+C+1", self._workspace+"/domain.pddl", self._workspace+"/p01.pddl", "mypddlplan"]) if generate_pddl: 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] outpaths = [] for (dirpath, dirnames, filenames) in walk(self._workspace): for name in filenames: if name.find('pddlplan') >= 0: outpaths.append(dirpath + '/' + name) if outpaths: data = self.selectMinDurationPlan(outpaths) try: remove("output") remove("all.groups") remove("variables.groups") remove("output.sas") except BaseException: log.warn("[TOCHECK]", "Not all files were generated while planning.") return data else: return None
def _add_wm_node(self, elem): #print "Adding {}".format(elem.id) parent_rel = elem.getRelation(pred=self._wmi.get_sub_properties('skiros:spatiallyRelated'), obj='-1') to_expand = True if not parent_rel: parent_rel = elem.getRelation(pred=self._wmi.get_sub_properties('skiros:skillProperty'), obj='-1') to_expand = False if not parent_rel: to_expand = False parent_rel = elem.getRelation(pred='skiros:hasSkill', obj='-1') if not parent_rel: log.warn("[add_wm_node]", "Skipping element without declared parent: {}".format(elem.id)) return True 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 item = self.wm_tree_widget.findItems(parent_rel['src'], Qt.MatchRecursive | Qt.MatchFixedString, 1)[0] item = QTreeWidgetItem(item, ['Skills', parent_id]) else: item = item[0] else: items = self.wm_tree_widget.findItems(parent_rel['src'], Qt.MatchRecursive | Qt.MatchFixedString, 1) if not items: log.warn("[add_wm_node]", "Parent {} of node {} is not in the known tree.".format(parent_rel['src'], elem.id)) return False item = items[0] name = utils.ontology_type2name(elem.id) if not elem.label else utils.ontology_type2name(elem.label) item = QTreeWidgetItem(item, [name, elem.id]) item.setExpanded(to_expand) return True
def revertSimulation(self): if not self._was_simulated: log.warn("revert", "No simulation was made, can't revert.") return False for c in reversed(self._post_conditions): if not c.revert(self._params, self._wmi): log.error(c.getDescription(), "Revert failed.") return False self._was_simulated = False return True
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: log.warn( self.__class__.__name__, "WARNING: Multiple plugins with name " + str(name) + " found!\n" + str(p)) return p[0]
def remap(self, initial_key, target_key, record=True): """ @brief Remap a parameter with initial_key to a new target_key All skill's children are remapped too. """ # 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 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 # 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 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): log.error( "Remap", "{}: Invalid remapping {}->{}, target key is already present.". format(self.type, initial_key, target_key)) return if self.params.hasParam(initial_key): #log.warn("{}".format(self.type), "REMAPPING: {} {}".format(initial_key, target_key)) # Remaps self._params.remap(initial_key, target_key) for c in self._pre_conditions: c.remap(initial_key, target_key) for c in self._hold_conditions: c.remap(initial_key, target_key) for c in self._post_conditions: c.remap(initial_key, target_key) # 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 self._remaps_cache[remapid].append((initial_key, target_key))
def setProperty(self, key, value, datatype=None, force_convertion=False): """ @brief Set the property to a value. If datatype is specified tries to convert. """ self._setLastUpdate() if key == 'skiros:DiscreteReasoner': old_reasoners = [] if self.hasProperty('skiros:DiscreteReasoner'): old_reasoners = self._properties[key].values if datatype: if datatype == "xsd:double" or datatype == "xsd:float": self._properties[key] = Property(key, float) elif datatype == "xsd:int" or datatype == "xsd:integer": self._properties[key] = Property(key, int) elif datatype == "xsd:boolean": self._properties[key] = Property(key, bool) elif datatype == "xsd:string": self._properties[key] = Property(key, str) else: log.warn( "[Element]", "Datatype {} not recognized. Set default.".format( datatype)) self._properties[key] = Property(key, value) if ispy2unicode(value): value = str(value) if self.hasProperty(key): if force_convertion: if isinstance(value, list): value = [ self._properties[key].dataType()(v) for v in value ] else: value = self._properties[key].dataType()(value) self._properties[key].setValues(value) else: self._properties[key] = Property(key, value) if key == 'skiros:DiscreteReasoner': 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 as e: log.error("WorldElement", e.message)
def makeStaticPrevious(self): skill = self.forward.recall()[0]._label log.warn("makeStaticPrevious", "skill {}".format(skill)) if not isinstance(self.getExecutionParent()._children_processor, ParallelFf): log.warn("makeStaticPrevious", "skill {} has a serial parent. Reverting till {}".format(skill, self.getExecutionParent()._label)) skill = self.getExecutionParent()._label self.makeStatic(True) while skill != self.forward.recall()[0]._label: print(self.forward.recall()[0]._label) self.makeStatic(True) self.makeStatic(True)
def update_element(self, element): for r in element._relations: 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.pushElement(element, "update") 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) return element._id
def invokePlanner(self, generate_pddl=True): """ @brief Generate pddl files and invoke TFD planner @param generate_pddl (bool) If False uses previously generated pddl files @return (string) The plan """ if generate_pddl: self.printDomain(True) self.printProblem(True) config = "y+Y+a+T+10+t+5+e+r+O+1+C+1" result_name = self._workspace + "/pddlplan" path = os.environ.get('TFD_HOME') #Copied from tfd-src-0.4/plan.py # run translator self.run(path + "/translate/translate.py", self._workspace + "/domain.pddl", self._workspace + "/p01.pddl") # run preprocessing self.run(path + "/preprocess/preprocess", input="output.sas") # run search self.run(path + "/search/search", config, "p", result_name, input="output") outpaths = [] for (dirpath, dirnames, filenames) in os.walk(self._workspace): for name in filenames: if name.find('pddlplan') >= 0: outpaths.append(dirpath + '/' + name) if outpaths: data = self.selectMinDurationPlan(outpaths) try: os.remove("output") os.remove("all.groups") os.remove("variables.groups") os.remove("output.sas") except BaseException: log.warn("[TOCHECK]", "Not all files were generated while planning.") return data else: return None
def create_wm_tree(self): scene_tuple = None while scene_tuple is None: try: scene_tuple = self._wmi.get_scene() except wmi.WmException: log.warn("[create_wm_tree]", "Failed to retrive scene, will try again.") #print "GOT SCENE {}".format([e.id for e in scene_tuple[0]]) self._snapshot_id = scene_tuple[1] self._snapshot_stamp = rospy.Time.now() scene = {elem.id: elem for elem in scene_tuple[0]} root = scene['skiros:Scene-0'] self.wm_tree_widget.clear() self.wm_tree_widget.setColumnCount(2) self.wm_tree_widget.hideColumn(1) self._create_wm_tree(self.wm_tree_widget, scene, root) self.wm_tree_widget.setCurrentIndex(self.wm_tree_widget.model().index(0, 0))
def onStart(self): self.fb = Queue.Queue(1) self.res = Queue.Queue(1) if self.build_client_onstart: self.client = self.buildClient() if not self.client.wait_for_server(rospy.Duration(0.5)): return self.startError( "Action server {} is not available.".format( self.client.action_client.ns), -101) if self.params["Reset"].value: self.params["Reset"].value = False try: srv = rospy.ServiceProxy( '/{}/config'.format(self.client.action_client.ns), Empty) srv(EmptyRequest()) except rospy.ServiceException, e: log.warn("[PrimitiveActionClient]", "Server reset failed. {}".format(e))
def _revertRemaps(self): """ Revert remaps. Just used in revertInput """ remapid = len(self._params_cache) try: if self._remaps_cache[remapid]: for remap in self._remaps_cache[remapid]: log.warn("REMAP", "Revert {} {}".format(remap[0], remap[1])) print self._remaps_cache self._remaps.pop(remap[0]) self.remap(remap[1], remap[0], False) del self._remaps_cache[remapid] except BaseException: print self.printInfo(True) print self._remaps_cache raise
def remove_element(self, e, author): """ @brief Remove an element from the scene """ try: e = self.get_element(e.id) 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(): if not r.parse(e, "remove"): raise Exception( "Reasoner {} rejected the element {} removal".format( name, e)) del self._elements_cache[e.id] statements = self._element2statements(e) for s, is_relation in statements: self._remove(s, author, is_relation) return True
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 """ first = {} couples = {} print_out = False for key in keys: first[key] = np.array(self.resolve_element(ph.getParamValue(key))) if not first[key].any(): log.warn("resolve_elements", "No input found for param {}. Resolving: {}".format(key, ph.getParamValue(key).printState(True))) all_keys = [key for key, _ in ph._params.iteritems()] coupled_keys = [] overlap_keys = [] relations_done = set([]) # 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 #print j 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 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 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 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.. 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] else: 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: couples[(key, key2)] = np.concatenate(couples[(key, key2)], np.array(temp)) 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) 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 if overlap_keys: loop = True iters = 5 while loop: # Iterate until no shared keys are found iters -= 1 if iters == 0: raise loop = False coupled_keys2 = [] merged = {} #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: continue loop = True skip = True for i in k1: if not i in coupled_keys2: coupled_keys2.append(i) 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 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 for key in keys: if not key in coupled_keys: couples[key] = first[key] if print_out: for k, v in couples.iteritems(): s = "{}:".format(k) for i in v: if not isinstance(i, Element): s += "[" for j in i: s += "{},".format(j) s += "]" else: s += "{},".format(i) print s return couples
def _doneCb(self, status, msg): if status == GoalStatus.RECALLED: log.warn("Ignoring recalled goal.") return self.res.put((msg, status))
def _resolve_elements2(self, keys, ph, verbose=False): """ @brief Find all possible inputs for one or more keys of a skill parameter handler @param keys list(string) a key list pointing out the params to be resolved @param ph (ParamHandler) @param verbose (bool) If true, prints out results @return list(Element) """ first = {} couples = {} for key in keys: first[key] = np.array(self.resolve_elements(ph.getParamValue(key))) if not first[key].any(): log.warn( "resolve_elements", "No input found for param {}. Resolving: {}".format( key, ph.getParamValue(key).printState(True))) all_keys = ph.keys() coupled_keys = [] overlap_keys = [] relations_done = set([]) # 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 # print j 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 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 continue else: # print rel_id relations_done.add(rel_id) # Check necessary because at the moment ._relations contains a mix Toclean if not ph.hasParam(key) or not ph.hasParam(key2): 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.. continue if this.getIdNumber() >= 0: set1 = [this] else: 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] else: if ph.getParam( key2).paramType == params.ParamTypes.Optional: abstract = ph.getParam(key2).value other = ph.getParam(key).value abstract._id = abstract.label set2 = [abstract] else: set2 = first[key2] if (key, key2) in couples: 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']) ] if temp: try: couples[(key, key2)] = np.concatenate( couples[(key, key2)], np.array(temp)) except BaseException: log.error( "", "MERGING: {} and {} ".format( couples[(key, key2)], np.array(temp))) else: log.warn( "resolve_elements", "No input for params {} {}. No match for: {} {} {}" .format(key, key2, set1, j["type"], set2)) 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) 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 {} {}. No match for: {} {} {}" .format(key, key2, set1, j["type"], set2)) # 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: raise loop = False coupled_keys2 = [] merged = {} # 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: continue loop = True skip = True for i in k1: if not i in coupled_keys2: coupled_keys2.append(i) 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 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 for key in keys: if not key in coupled_keys: couples[key] = first[key] if verbose: for k, v in couples.iteritems(): s = "{}:".format(k) for i in v: if not isinstance(i, Element): s += "[" for j in i: s += "{},".format(j) s += "]" else: s += "{},".format(i) print s return couples