def _monitor_cb(self, msg): """ @brief Callback updating the cache when a change on wm is detected """ if self._make_cache: if self._last_snapshot_id != msg.prev_snapshot_id or msg.action == 'reset': WorldModelInterface._elements_cache.clear() self._last_snapshot_id = msg.snapshot_id for elem in msg.elements: elem = utils.msg2element(elem) if msg.action == 'update' or msg.action == 'update_properties' or msg.action == 'add': WorldModelInterface._elements_cache[elem.id] = elem elif msg.action == 'remove' or msg.action == 'remove_recursive': if elem.id in WorldModelInterface._elements_cache: del WorldModelInterface._elements_cache[elem.id] else: log.error("[WmMonitor]", "Command {} not recognized.".format(msg.action)) if msg.relation: rel = utils.msg2relation(msg.relation[0]) if rel['src'] in WorldModelInterface._elements_cache: del WorldModelInterface._elements_cache[rel['src']] if rel['dst'] in WorldModelInterface._elements_cache: del WorldModelInterface._elements_cache[rel['dst']] if self._external_monitor_cb: self._external_monitor_cb(msg)
def assign_instance(self, skill, ignore_list=list()): """ @brief Assign an instance to an abstract skill. If an instance with same label is not found, assign the last instance of the type. """ 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: to_set = p 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 to_set = self.duplicate_instance(to_set) skill.setInstance(to_set) elif skill.label != "" and not ignore_list: # No instance exist, try to load it skill.setInstance(self.add_instance(skill.label)) else: log.error("assign_instance", "No instance of type {} found.".format(skill.type)) return False return True
def getParam(self, key): if self.hasParam(key): return self._params[key] else: log.error( 'getParam', 'Param {} is not in the map. Debug: {}'.format( key, self.printState()))
def __getitem__(self, key): if self.hasParam(key): return self._params[key] else: log.error( 'ParamHandler', 'Param {} is not in the map. Debug: {}'.format( key, self.printState()))
def specify(self, key, values): if self.hasParam(key): self._params[key].setValues(values) else: log.error( 'specify', 'Param {} is not in the map. Debug: {}'.format( key, self.printState()))
def call(self, service, msg): try: resp1 = service(msg) return resp1 except rospy.ServiceException, e: log.error("[call]", "Service call failed: %s" % e) return
def removeRelation2(self, subj, predicate, obj, value=True, abstract=False): """ @brief Remove a relation from the element """ try: self._relations.remove({ 'src': subj, 'type': predicate, 'dst': obj, 'state': value, 'abstract': abstract }) except BaseException: log.error( "[removeRelation2]", "Can t remove {} from {}".format( { 'src': subj, 'type': predicate, 'dst': obj, 'state': value, 'abstract': abstract }, self._relations))
def on_properties_table_item_changed(self, item, row): item_key = self.wm_properties_widget.item(row, 0) item_val = self.wm_properties_widget.item(row, 1) key = item_key.id value = item_val.text() elem = self._wmi.get_element(item.text(1)) if elem.hasProperty(key): prop = elem.getProperty(key) if value == '': value = None if prop.dataTypeIs(bool): value = item_val.checkState() == Qt.Checked try: elem.setProperty(prop.key, value, is_list=prop.isList(), force_convertion=value is not None) log.debug(self.__class__.__name__, '<{}> property {} to {}'.format(item.text(1), prop.key, value)) except ValueError: log.error(self.__class__.__name__, 'Changing <{}> property {} to {} failed'.format(item.text(1), prop.key, value)) item_val.setText(str(prop.value)) elif hasattr(elem, key.lower()): key = key.lower() attr = getattr(elem, key) setattr(elem, key, type(attr)(value)) log.debug(self.__class__.__name__, '<{}> attribute {} to {}'.format(item.text(1), key, value)) else: log.error(self.__class__.__name__, 'Changing <{}> property {} to {} failed'.format(item.text(1), key, value)) self._wmi.update_element_properties(elem) name = utils.ontology_type2name(elem.id) if not elem.label else utils.ontology_type2name(elem.label) item.setText(0, name)
def on_progress_update(self, msg): self._update_progress_table(msg) self._save_log(msg, "skill") # Update buttons if msg.type.find("Root") >= 0: if not self.skill_stop_button.isEnabled(): self.create_task_tree(msg.id, msg.processor) self._toggle_task_active() for manager in self._sli.agents.values(): manager.reset_tick_rate() if abs(msg.progress_code) == 1: self._toggle_task_active() # Update task tree with self._task_mutex: items = self.task_tree_widget.findItems(str(msg.id), Qt.MatchRecursive | Qt.MatchFixedString, 1) #int(msg.progress_period*1000) if items: items[0].setData(0, 0, "{}({}) {}".format(msg.label, State(msg.state).name, "! SLOW !" if msg.progress_period>0.04 else "")) else: parents = self.task_tree_widget.findItems(str(msg.parent_id), Qt.MatchRecursive | Qt.MatchFixedString, 1) if not parents: log.error("No parent found. Debug: {}".format(msg)) return item = QTreeWidgetItem(parents[0], ["{}({})".format(msg.label, State(msg.state).name), str(msg.id)]) item.setIcon(0, self.get_icon(msg.processor)) item.setExpanded(True)
def computeRelations(self, sub, obj, with_metrics=False): """ @brief Compute semantic relations from geometrical data @with_metrics If true, return tuples (relation, float) instead of strings @return List of strings, either valid semantic relations or ':unknownT' """ to_ret = [] sub_frame = sub.getProperty("skiros:FrameId").value obj_base_frame = obj.getProperty("skiros:BaseFrameId").value # transform pose: object w.r.t. frame of subject if sub_frame != obj_base_frame: if obj_base_frame == "": return [':unknownT'] if not with_metrics else [(':unknownT', -1.0)] try: obj = deepcopy(obj) if sub_frame == "": # If the subject is not being published, I add it manually to the frames buffer sub = deepcopy(sub) sub_frame = "subj_temp_frame" sub.setProperty("skiros:FrameId", sub_frame) self._tlb.settransform(self.getData(sub, ":TransformMsg"), "AauSpatialReasoner") # If the object is not being published, I add it manually to the frames buffer if obj.getProperty("skiros:FrameId").value == "": obj.setProperty("skiros:FrameId", "obj_temp_frame") self._tlb.settransform(self.getData(obj, ":TransformMsg"), "AauSpatialReasoner") self._tlb.lookup_transform(obj_base_frame, sub_frame, rospy.Time(0), rospy.Duration(1.0)) obj.setData(":PoseStampedMsg", self._tlb.transform(obj.getData(":PoseStampedMsg"), sub_frame)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): log.error("[computeRelations]", "Couldn't transform object in frame {} to frame {}.".format( obj_base_frame, sub_frame)) return [':unknownT'] if not with_metrics else [(':unknownT', -1.0)] # Get corners a1,a2,b1,b2 sp = numpy.array([0, 0, 0]) ss = numpy.array(self.getData(sub, ":Size")) if ss[0] == None: ss = numpy.array([0, 0, 0]) a1 = sp-ss/2 a2 = sp+ss/2 op = numpy.array(self.getData(obj, ":Position")) os = numpy.array(self.getData(obj, ":Size")) oo = numpy.array(self.getData(obj, ":Orientation")) if op[0] is None: return [':unknownT'] if not with_metrics else [(':unknownT', -1.0)] if os[0] is None: os = numpy.array([0, 0, 0]) b1 = op-os/2 b2 = op+os/2 # Calculates allen intervals for the 3 axes + orientation alignment if with_metrics: to_ret.append(self._getAIRelations(a1[0], a2[0], b1[0], b2[0], 'X')) to_ret.append(self._getAIRelations(a1[1], a2[1], b1[1], b2[1], 'Y')) to_ret.append(self._getAIRelations(a1[2], a2[2], b1[2], b2[2], 'Z')) to_ret.append(self._get_orientation_relation(oo)) else: to_ret.append(self._getAIRelations(a1[0], a2[0], b1[0], b2[0], 'X')[0]) to_ret.append(self._getAIRelations(a1[1], a2[1], b1[1], b2[1], 'Y')[0]) to_ret.append(self._getAIRelations(a1[2], a2[2], b1[2], b2[2], 'Z')[0]) to_ret.append(self._get_orientation_relation(oo)[0]) # print to_ret return to_ret
def makeParentStatic(self, parent_name, static=False): if self._execution_branch[ -1]._label != parent_name and self._forget_branch[ -1]._label != parent_name and static: log.error( "makeParentStatic", "Exe: {} Forgot: {} Looking for: {}".format( self._execution_branch[-1]._label, self._forget_branch[-1]._label, parent_name)) return if static: if self._execution_branch[-1]._label == parent_name: self._static_branch.append( (self._execution_branch.pop(), True)) log.info("makeParentStatic", "Storing: {} {}".format(parent_name, True)) else: self._static_branch.append((self._forget_branch.pop(), False)) log.info("makeParentStatic", "Storing: {} {}".format(parent_name, False)) else: if self._static_branch: if parent_name == self._static_branch[-1][0]._label: p, tag = self._static_branch.pop() log.info("makeParentStatic", "Restoring: {} {}".format(parent_name, tag)) if tag: self._execution_branch.append(p) else: self._forget_branch.append(p)
def hasData(self, element, get_code): """ @brief Return true if the data is available in the element """ if get_code == ":Pose" or get_code == ":PoseStampedMsg": return element.hasData(":Position") and element.hasData( ":Orientation") elif get_code == ":Position": return (element.hasProperty("skiros:PositionX", not_none=True) and element.hasProperty("skiros:PositionY", not_none=True) and element.hasProperty("skiros:PositionZ", not_none=True)) elif get_code == ":Orientation": return ( element.hasProperty("skiros:OrientationX", not_none=True) and element.hasProperty("skiros:OrientationY", not_none=True) and element.hasProperty("skiros:OrientationZ", not_none=True) and element.hasProperty("skiros:OrientationW", not_none=True)) elif get_code == ":Size": return (element.hasProperty("skiros:SizeX", not_none=True) and element.hasProperty("skiros:SizeY", not_none=True) and element.hasProperty("skiros:SizeZ", not_none=True)) else: log.error( "[AauSpatialReasoner] Code {} not recognized".format(get_code)) return False
def _wo_query_cb(self, msg): to_ret = srvs.WoQueryResponse() try: log.assertInfo( self._verbose, "[WoQuery]", "Query: {}. Context: {}".format(msg.query_string, msg.context)) with self._times: for s in self._ontology.query(msg.query_string, context_id=msg.context): temp = "" for r in s: if r is None: continue elif msg.cut_prefix: temp += self._ontology.uri2lightstring(r) else: temp += r.n3() 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.get_last())) except (AttributeError, ParseException) as e: # 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
def get_super_class(self, child_class): """ @brief Returns the parent class of child_class """ 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]
def simulate(self): self._was_simulated = True #print "Simulate: {}".format(self.printInfo(True)) for c in self._post_conditions: if not c.setTrue(self._params, self._wmi): log.error(c.getDescription(), "Simulation failed.") return State.Failure return State.Success
def postRevert(self, skill): if not skill.revertSimulation(): log.error("undo", "Can't revert {}".format(skill.printState())) return False self.specifyParams(skill.revertInput()) self.restoreParentNode() #print 'miei ' + self.printParams(skill._params) return True
def revertHold(self): for c in reversed(self._hold_conditions): #print c.getDescription() if not c.revert(self._params, self._wmi): log.error(c.getDescription(), "Revert hold failed.") return False self._was_simulated = False return True
def addValue(self, value): """ @brief Append a value """ if isinstance(value, self._data_type): self._values.append(value) else: log.error("append", self._key + ": " + str(type(value)) + "!=" + str(self._data_type)) return
def specifyParam(self, key, values): """ Specify a parameter and update the input cache """ if not self._params.hasParam(key): log.error( "specifyParam", "No param '{}' found. Debug: {}".format( key, self.printInfo(True))) self._params.specify(key, values)
def specifyParamDefault(self, key, values): """ Specify a value and set it as default value too """ if not self._params.hasParam(key): log.error( "specifyParamDefault", "No param '{}' found. Debug: {}".format( key, self.printInfo(True))) self._params.specifyDefault(key, values)
def setData(self, element, data, set_code): """ @brief Convert user data to reasoner data and store it into given element """ if set_code == ":Pose": return (element.setData(":Position", data[0]), element.setData(":Orientation", data[1])) elif set_code == ":TransformMsg": element.getProperty("skiros:BaseFrameId").value = str(data.header.frame_id) element.getProperty("skiros:TfTimeStamp").value = data.header.stamp.to_sec() element.getProperty("skiros:PositionX").value = data.transform.translation.x element.getProperty("skiros:PositionY").value = data.transform.translation.y element.getProperty("skiros:PositionZ").value = data.transform.translation.z element.getProperty("skiros:OrientationX").value = data.transform.rotation.x element.getProperty("skiros:OrientationY").value = data.transform.rotation.y element.getProperty("skiros:OrientationZ").value = data.transform.rotation.z element.getProperty("skiros:OrientationW").value = data.transform.rotation.w elif set_code == ":PoseMsg": element.getProperty("skiros:PositionX").value = data.position.x element.getProperty("skiros:PositionY").value = data.position.y element.getProperty("skiros:PositionZ").value = data.position.z element.getProperty("skiros:OrientationX").value = data.orientation.x element.getProperty("skiros:OrientationY").value = data.orientation.y element.getProperty("skiros:OrientationZ").value = data.orientation.z element.getProperty("skiros:OrientationW").value = data.orientation.w elif set_code == ":PoseStampedMsg": element.getProperty("skiros:BaseFrameId").value = str(data.header.frame_id) element.getProperty("skiros:TfTimeStamp").value = data.header.stamp.to_sec() element.getProperty("skiros:PositionX").value = data.pose.position.x element.getProperty("skiros:PositionY").value = data.pose.position.y element.getProperty("skiros:PositionZ").value = data.pose.position.z element.getProperty("skiros:OrientationX").value = data.pose.orientation.x element.getProperty("skiros:OrientationY").value = data.pose.orientation.y element.getProperty("skiros:OrientationZ").value = data.pose.orientation.z element.getProperty("skiros:OrientationW").value = data.pose.orientation.w elif set_code == ":Position": element.getProperty("skiros:PositionX").value = data[0] element.getProperty("skiros:PositionY").value = data[1] element.getProperty("skiros:PositionZ").value = data[2] elif set_code == ":Orientation": element.getProperty("skiros:OrientationX").value = data[0] element.getProperty("skiros:OrientationY").value = data[1] element.getProperty("skiros:OrientationZ").value = data[2] element.getProperty("skiros:OrientationW").value = data[3] elif set_code == ":OrientationEuler": q = tf_conv.quaternion_from_euler(*data) element.getProperty("skiros:OrientationX").value = q[0] element.getProperty("skiros:OrientationY").value = q[1] element.getProperty("skiros:OrientationZ").value = q[2] element.getProperty("skiros:OrientationW").value = q[3] elif set_code == ":Size": element.getProperty("skiros:SizeX").value = data[0] element.getProperty("skiros:SizeY").value = data[1] element.getProperty("skiros:SizeZ").value = data[2] else: log.error("[AauSpatialReasoner] Code {} not recognized".format(set_code)) return None
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: log.error("[{}]".format(self.__class__.__name__), "Failed to add local element {}".format(sub_e)) else: if self.update_element(sub_e) < 0: log.error("[{}]".format(self.__class__.__name__), "Failed to update local element {}".format(sub_e))
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 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 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 getParamValues(self, key): """ @brief Return the parameter values (list) """ if self.hasParam(key): return self._params[key].getValues() else: log.error( 'getParamValues', 'Param {} is not in the map. Debug: {}'.format( key, self.printState()))
def update_element(self, e, author): """ @brief Update an element in the scene """ if not self._id_gen.hasId(self._uri2id(e.id)): log.error( "[update_element]", "Request update from {}, but Id {} is not present in the wm. ". format(author, self._uri2id(e.id))) return IndividualsDataset.update_element(self, e, author)
def processPreempt(self, procedure): try: super(VisitorExecutor, self).processPreempt(procedure) except Exception: log.error(self.__class__.__name__, traceback.format_exc()) procedure._instance.onEnd() procedure._setProgress( "Error on preemption: {}. Blackboard data: {}".format( traceback.format_exc(), self._params.printState()), -406) procedure._setState(State.Failure) self.memorizeProgress(procedure)
def getParamValues(self, key): """ Return the parameter values (list) If make_instance is True and the parameter is not specified, an instance list is returned rather than a None list """ if self.hasParam(key): return self._params[key].getValues() else: log.error('getParamValues', 'Param {} is not in the map. Debug: {}'.format(key, self.printState()))
def setValue(self, value, index=0): """ @brief Set the value at the index """ if isinstance(value, self._data_type): if self.isSpecified(): self._values[index] = value else: self._values.append(value) else: log.error("setValue", "{}: Input {} != {}. Debug: {}".format(self.key, type(value), self._data_type, self.printState())) return