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
