def _register_agent(self, agent_name):
res = self._wmi.resolve_element(Element("cora:Robot", agent_name))
if res:
log.info("[{}]".format(self.__class__.__name__),
"Found robot {}, skipping registration.".format(res))
self._robot = res
for r in self._robot.getRelations("-1", "skiros:hasSkill"):
self._wmi.remove_element(self._wmi.get_element(r['dst']))
self._robot = self._wmi.get_element(self._robot.id)
else:
self._robot = self._wmi.instanciate(agent_name, True)
startLocUri = self._wmi.get_template_element(
agent_name).getRelations(pred="skiros:hasStartLocation")
if startLocUri:
start_location = self._wmi.instanciate(startLocUri[0]["dst"],
False, [])
self._wmi.set_relation(self._robot._id, "skiros:at",
start_location._id)
self._robot = self._wmi.get_element(self._robot.id)
log.info("[{}]".format(self.__class__.__name__),
"Registered robot {}".format(self._robot))
self._robot.setProperty(
"skiros:SkillMgr",
self._agent_name[self._agent_name.rfind(":") + 1:])
self._wmi.update_element(self._robot)
def _init_wm(self):
rospack = rospkg.RosPack()
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)
for (dirpath, dirnames, filenames) in walk(self._workspace):
for name in filenames:
if name.find('.owl') >= 0:
self._ontology.load(dirpath + '/' + name)
if not self._workspace:
self._workspace = self._skiros_dir
self._ontology.workspace = self._workspace
log.info("[{}]".format(self.__class__.__name__),
"Workspace folder: {}".format(self._workspace))
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 != "":
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
for prefix, uri1 in self._ontology._ontology.namespace_manager.store.namespaces(
):
if prefix.find("default") > -1:
self._ontology._bind(prefix, "")
self._ontology._bind("", "")
def initDomain(self):
skills = self._wmi.resolve_elements(wmi.Element(":Skill"))
for skill in skills:
params = {}
preconds = []
holdconds = []
postconds = []
# 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))
for p in skill.getRelations(pred="skiros:hasHoldCondition"):
e = self._wmi.get_element(p['dst'])
if e.type.find("ConditionRelation") != -1 or e.type == "skiros:ConditionProperty" or e.type == "skiros:ConditionHasProperty":
holdconds.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))
self._pddl_interface.addAction(pddl.Action(skill, params, preconds, holdconds, postconds))
if self._verbose:
log.info("[Domain]", self._pddl_interface.printDomain(False))
def _wm_get_cb(self, msg):
with self._times:
to_ret = srvs.WmGetResponse()
if msg.action == msg.GET:
to_ret.elements.append(
utils.element2msg(
self._get_context(msg.context).get_element(
msg.element.id)))
elif msg.action == msg.GET_TEMPLATE:
to_ret.elements.append(
utils.element2msg(
self._get_context(msg.context).get_template_individual(
msg.element.label)))
elif msg.action == msg.GET_RECURSIVE:
for _, e in self._get_context(msg.context).get_recursive(
msg.element.id, msg.relation_filter,
msg.type_filter).items():
to_ret.elements.append(utils.element2msg(e))
elif msg.action == msg.RESOLVE:
for e in self._get_context(msg.context).resolve_elements(
utils.msg2element(msg.element)):
to_ret.elements.append(utils.element2msg(e))
output = ""
einput = utils.msg2element(msg.element)
for e in to_ret.elements:
output += "{} ".format(e.id)
if self._verbose:
log.info(
"[WmGet]",
"Done {} [{}]. Answer: {}. Time: {:0.3f} secs".format(
msg.action, einput, output, self._times.get_last()))
to_ret.snapshot_id = self._curr_snapshot.hex
return to_ret
def _wm_modify_cb(self, msg):
to_ret = srvs.WmModifyResponse()
with self._times:
if msg.action == msg.ADD:
for e in msg.elements:
updated_e = self._get_context(msg.context).add_element(
utils.msg2element(e), msg.author)
to_ret.elements.append(utils.element2msg(updated_e))
self._publish_change(msg.author,
"add",
elements=to_ret.elements,
context_id=msg.context)
elif msg.action == msg.UPDATE:
for e in msg.elements:
self._get_context(msg.context).update_element(
utils.msg2element(e), msg.author)
er = utils.element2msg(
self._get_context(msg.context).get_element(e.id))
to_ret.elements.append(er)
self._publish_change(msg.author,
"update",
elements=to_ret.elements,
context_id=msg.context)
elif msg.action == msg.UPDATE_PROPERTIES:
for e in msg.elements:
self._get_context(msg.context).update_properties(
utils.msg2element(e), msg.author,
self._ontology.get_reasoner(msg.type_filter), False)
er = utils.element2msg(
self._get_context(msg.context).get_element(e.id))
to_ret.elements.append(er)
self._publish_change(msg.author,
"update",
elements=to_ret.elements,
context_id=msg.context)
elif msg.action == msg.REMOVE:
for e in msg.elements:
self._get_context(msg.context).remove_element(
utils.msg2element(e), msg.author)
to_ret.elements = msg.elements
self._publish_change(msg.author,
"remove",
elements=msg.elements,
context_id=msg.context)
elif msg.action == msg.REMOVE_RECURSIVE:
for e in msg.elements:
self._get_context(msg.context).remove_recursive(
utils.msg2element(e), msg.author, msg.relation_filter,
msg.type_filter)
to_ret.elements = msg.elements
self._publish_change(msg.author,
"remove_recursive",
elements=msg.elements,
context_id=msg.context)
if self._verbose:
log.info(
"[WmModify]", "{} {} {}. Time: {:0.3f} secs".format(
msg.author, msg.action, [e.id for e in to_ret.elements],
self._times.get_last()))
return to_ret
def __init__(self, author_name="test"):
"""
@brief Interface to ontology services
This class uses ROS services to access and edit the
ontology knowledge on a server node
@param author_name (string) Id used to track changes on
the server
"""
self._author_name = author_name
self._lock = rospy.ServiceProxy('wm/lock', SetBool)
self._ontology_query = rospy.ServiceProxy('wm/ontology/query',
srvs.WoQuery)
self._ontology_modify = rospy.ServiceProxy('wm/ontology/modify',
srvs.WoModify)
self._load_and_save = rospy.ServiceProxy('wm/load_and_save',
srvs.WoLoadAndSave)
log.info("[{}] ".format(self.__class__.__name__),
"Waiting wm communications...")
self._ontology_modify.wait_for_service()
log.info("[{}] ".format(self.__class__.__name__),
"Wm communications active.")
self._def_prefix = ":"
self._sub_classes_cache = {}
self._sub_properties_cache = {}
def __init__(self):
rospy.init_node("skill_mgr", anonymous=False)
self.publish_runtime_parameters = False
robot_name = rospy.get_name()
prefix = ""
full_name = rospy.get_param('~prefix', prefix) + ':' + robot_name[robot_name.rfind("/") + 1:]
self.sm = SkillManager(rospy.get_param('~prefix', prefix), full_name, verbose=rospy.get_param('~verbose', True))
self.sm.observe_task_progress(self._on_progress_update)
self.sm.observe_tick(self._on_tick)
# Init skills
self._initialized = False
self._getskills = rospy.Service('~get_skills', srvs.ResourceGetDescriptions, self._get_descriptions_cb)
self._init_skills()
rospy.sleep(0.5)
self._initialized = True
# Start communications
self._command = rospy.Service('~command', srvs.SkillCommand, self._command_cb)
self._monitor = rospy.Publisher("~monitor", msgs.SkillProgress, queue_size=20)
self._tick_rate = rospy.Publisher("~tick_rate", Empty, queue_size=20)
self._set_debug = rospy.Subscriber('~set_debug', Bool, self._set_debug_cb)
rospy.on_shutdown(self.shutdown)
self.init_discovery("skill_managers", robot_name)
log.info("[{}]".format(rospy.get_name()), "Skill manager ready.")
def _get_context(self, 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#')
self.contexts[context_id].workspace = self._workspace
return self.contexts[context_id]
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 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
self._snapshot_id = data.snapshot_id
cur_item = self.wm_tree_widget.currentItem()
cur_item_id = cur_item.text(1)
if data.action == 'update' or data.action == 'update_properties':
for elem in data.elements:
elem = rosutils.msg2element(elem)
self._update_wm_node(elem, cur_item_id)
elif data.action == 'add':
for elem in data.elements:
elem = rosutils.msg2element(elem)
if not self._add_wm_node(elem):
self._snapshot_id = ""
elif data.action == 'remove' or data.action == 'remove_recursive':
for elem in data.elements:
elem = rosutils.msg2element(elem)
self._remove_wm_node(elem)
# reselect current item
items = self.wm_tree_widget.findItems(cur_item_id, Qt.MatchRecursive | Qt.MatchFixedString, 1)
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
log.info("[wm_update]", "Wm not in sync, querying wm scene")
self.create_wm_tree()
def _wm_query_rel_cb(self, msg):
# 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))]
if self._verbose:
log.info("[wmQueryRelation]", "Query: {} Answer: {}. Time: {:0.3f} secs".format(msg.relation, to_ret.matches, self._times.get_last()))
return to_ret
def _add_prefix(self, uri, prefix=None):
if prefix is None:
prefix = uri[uri.rfind("/") + 1:].lower()
if prefix.rfind(".") != -1:
prefix = prefix[:prefix.rfind(".")]
uri = uri + "#"
self._bind(prefix, uri)
log.info("[{}]".format(self.__class__.__name__), "Set id: {} for ontology: {}".format(prefix, uri))
return prefix
def _unregister(self, element):
"""
@brief Removes an element from tf publish list
"""
if element.id in self._tf_list:
log.info(
"[AauSpatialReasoner] Stop publishing {}.".format(element))
element.setProperty("skiros:PublishTf", False)
del self._tf_list[element.id]
self._trigger_children_update(element)
def execute(self, skill, processor=Serial):
if not self.initAndParametrize(skill):
return False
if self._verbose:
log.info("Execute {}.".format(skill._label))
self.addInExecutionTree(skill, processor)
skill.hold()
self.mergeParams(skill) # Update params
self.forward.memorize(skill, "execute")
return True
def forget(self):
# TODO: possible mess with parentNode in execution tree...
skill, tag = self.forward.forget()
if self._verbose:
log.info("Undo {} {}.".format(skill._label, tag))
if tag == 'execute':
self.revert(skill)
elif tag == 'postExecute':
self.postRevert(skill)
return (skill, tag)
def add_task(self, task):
"""
@brief Add a new task to the list
"""
root = skill.Root("root", self._local_wm)
for i in task:
log.info("[SkillManager]", "Add task {}:{} \n {}".format(i.type, i.name, i.ph.printState()))
root.addChild(skill.SkillWrapper(i.type, i.name, self._instanciator))
root.last().specifyParamsDefault(i.ph)
return self._ticker.add_task(root, root.id)
def start(self, visitor, uid):
if uid in BtTicker._tasks_to_pause:
log.info("[start]", "Resuming task {}.".format(uid))
del BtTicker._tasks_to_pause[uid]
else:
log.info("[start]", "Starting task {}.".format(uid))
if not self.is_running():
BtTicker._visitor = visitor
BtTicker._process = Process(target=BtTicker._run, args=(self, True))
BtTicker._process.start()
return True
def postExecute(self, skill):
"""
@brief Tick a skill
"""
skill.specifyParams(self._params)#Re-apply parameters.... Important!
self.syncParams(skill.params)
self._printTracked(skill._params, "[{}:SetParams] ".format(skill.type))
state = self._postExecute(skill)
if self._verbose:
log.info("[VisitorExecute]", "{}".format(skill.printState(self._verbose)))
self.mergeParams(skill) # Update params
return state
def checkHold(self, skill):
"""
@brief Check hold conditions
"""
if skill.checkHoldCond(self._verbose):
if self._verbose:
log.info("[ground]", "Hold-conditions fail for skill {}".format(skill.printInfo()))
self.processPreempt(skill)
skill.checkHoldCond()#This ensure the skill ends printing the failed conditions
return skill.state
else:
return State.Running
def _load_and_save_cb(self, msg):
with self._times:
if msg.action == msg.SAVE:
self._get_context(msg.context).save_context(msg.filename)
elif msg.action == msg.LOAD:
self._get_context(msg.context).load_context(msg.filename)
self._publish_change("", "reset", elements=[], context_id=msg.context)
else:
return srvs.WoLoadAndSaveResponse(False)
if self._verbose:
log.info("[wmLoadAndSave]", "{} {} to file {}. Time: {:0.3f} secs".format(msg.action, msg.context, self._get_context(msg.context).filename, self._times.get_last()))
return srvs.WoLoadAndSaveResponse(True)
def _run(self, _):
"""
@brief Tick tasks at 25hz
"""
BtTicker._finished_skill_ids = dict()
rate = rospy.Rate(25)
log.info("[BtTicker]", "Execution starts.")
while BtTicker._tasks:
self._tick()
rate.sleep()
self._tick_cb()
log.info("[BtTicker]", "Execution stops.")
def _register(self, element, parent_frame):
"""
@brief Adds an element to tf publish list
"""
if element.id in self._to_rebase_list:
del self._to_rebase_list[element.id]
if not element.id in self._tf_list:
log.info("[AauSpatialReasoner] Publishing {} parent: {}".format(element, parent_frame))
self._trigger_children_update(element)
element.setData(":Orientation", self._quaternion_normalize(element.getData(":Orientation")))
element.setProperty("skiros:PublishTf", True)
self._tf_list[element.id] = element
def syncParams(self, params):
for k, p in params.items():
vs = p.values
if p.dataTypeIs(Element):
for i in reversed(range(0, len(vs))):
if vs[i].getIdNumber() >= 0:
try:
vs[i] = self._wm.get_element(vs[i].id)
except WmException:
log.info("[syncParams]", "{} was deleted, removing from parameters".format(vs[i].id))
vs.pop(i)
p.values = vs
def inferUnvalidParams(self, skill):
#print '{}: {} '.format(skill._label, self.printParams(skill._params))
unvalid_params = skill.checkPreCond(self._verbose)
if unvalid_params:
log.info("[{}] Reset unvalid params {}".format(skill._label, unvalid_params))
for k in unvalid_params:
skill._params.setDefault(k)
p = skill._params.getParam(k)
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
def _wm_set_rel_cb(self, msg):
with self._times:
if msg.value:
temp = "+"
self._ontology.add_relation(utils.msg2relation(msg.relation), msg.author, is_relation=True)
self._publish_change(msg.author, "add", relation=msg.relation)
else:
temp = "-"
self._ontology.remove_relation(utils.msg2relation(msg.relation), msg.author, is_relation=True)
self._publish_change(msg.author, "remove", relation=msg.relation)
if self._verbose:
log.info("[wmSetRelCb]", "[{}] {} Time: {:0.3f} secs".format(temp, msg.relation, self._times.get_last()))
return srvs.WmSetRelationResponse(True)
def tryOther(self, skill):
"""
@brief If the skill label is not specified, try other instances
"""
if skill.label != "":
return False
ignore_list = [skill._instance.label]
while self._instanciator.assign_instance(skill, ignore_list):
log.info("tryOther", "Trying skill {}".format(skill._instance.label))
ignore_list.append(skill._instance.label)
if self._ground(skill):
return True
return False
def instance(self, plugin, args_dict):
names, req, opt = self.signature(plugin)
log.info(
self.__class__.__name__, "Instantiating " + str(plugin) +
" with arguments " + str(args_dict) + " || REQUIRED: " +
str(req) + " OPTIONAL: " + str(opt.keys()))
p = None
try:
p = plugin(**args_dict)
except Exception as e:
log.error(self.__class__.__name__,
" ERROR while instantiating: " + str(e))
return p
def postExecute(self, skill, remember=True):
skill.specifyParams(self._params) # Re-apply parameters, after processing the sub-tree.... Important, thay have been modified by the subtree!
if self._verbose:
log.info("postExecute {}.".format(skill._label))
if not self._postExecute(skill):
return False
self.mergeParams(skill) # Update params
self.forward.memorize(skill, "postExecute")
if skill._label != self.getExecutionParent()._label:
log.error("postExecution", "{} trying to close before the parent {}".format(skill._label, self.getExecutionParent()._label))
raise KeyError()
self.popParentNode()
if id(skill) in self._bound:
self.postExecute(self._bound.pop(id(skill)))
return True
def load_context(self, filename):
"""
@brief Load context from file
"""
if filename:
self._filename = filename
if not path.isfile(self.filedir):
log.error("[load_context]",
"Can't load {}. File not found. ".format(self.filename))
return
self._stop_reasoners()
self.reset()
self.context.parse(self.filedir, format='turtle')
self._start_reasoners()
log.info("[load_context]", "Loaded context {}. ".format(self.filename))
def _reset(self):
self._tf_list = {}
self._linked_list = {}
self._to_rebase_list = {}
root = self._wmi.get_element("skiros:Scene-0")
self._spatial_rels = self._wmi.get_sub_properties("skiros:spatiallyRelated")
if root.hasProperty("skiros:FrameId"):
self._base_frame = root.getProperty("skiros:FrameId").value
else:
self._base_frame = "map"
log.info(self.__class__.__name__, "Adding FrameId {} to scene.".format(self._base_frame))
root.setProperty("skiros:FrameId", self._base_frame)
self._wmi.update_element(root, self.__class__.__name__)
for e in self._wmi.get_recursive(root.id, "skiros:spatiallyRelated").values():
self.parse(e, "add")
