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
