def __init__(self, prefix, agent_name, verbose=True):
self._agent_name = agent_name
self._wmi = wmi.WorldModelInterface(agent_name, make_cache=True)
self._wmi.set_default_prefix(prefix)
self._local_wm = self._wmi
self._instanciator = SkillInstanciator(self._local_wm)
self._ticker = BtTicker()
self._verbose = verbose
self._ticker._verbose = verbose
self._register_agent(agent_name)
self._skills = []
class SkillManager:
"""
@brief The skill manager manage a sub-system of the robot
"""
def __init__(self, prefix, agent_name, verbose=True):
self._agent_name = agent_name
self._wmi = wmi.WorldModelInterface(agent_name, make_cache=True)
self._wmi.set_default_prefix(prefix)
self._local_wm = self._wmi
self._instanciator = SkillInstanciator(self._local_wm)
self._ticker = BtTicker()
self._verbose = verbose
self._ticker._verbose = verbose
self._register_agent(agent_name)
self._skills = []
# self._wmi.unlock() #Ensures the world model's mutex is unlocked
@property
def skills(self):
return self._skills
def observe_task_progress(self, func):
self._ticker.observe_progress(func)
def observe_tick(self, func):
self._ticker.observe_tick(func)
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 shutdown(self):
for s in self.skills:
self._wmi.remove_element(s)
self._wmi.unlock() # Ensures the world model's mutex gets unlocked
def load_skills(self, package):
"""
Load definitions from a package
"""
self._instanciator.load_library(package, self._verbose)
def add_skill(self, name, subclass="skiros:CompoundSkill"):
"""
@brief Add a skill to the available skill set
"""
skill = self._instanciator.add_instance(name)
e = skill.toElement()
e.addRelation(self._robot._id, "skiros:hasSkill", "-1")
#print skill.printInfo(True)
hierarchy = skill.__module__.split(".") + [e.type]
for c1, c2 in zip([None] + hierarchy[:-1], hierarchy):
if c1 is None:
c1 = "skiros:Skill"
c1 = c1 if c1.find(":") > 0 else "skiros:{}".format(inflection.camelize(c1))
c2 = c2 if c2.find(":") > 0 else "skiros:{}".format(inflection.camelize(c2))
if not self._wmi.get_type(c2):
self._wmi.add_class(c2, c1)
self._wmi.add_element(e)
self._skills.append(skill)
return SkillHolder(self._agent_name, skill.type, skill.label, skill.params.getCopy())
def add_primitive(self, name):
"""
@brief Add a local primitive
"""
self.add_skill(name, "skiros:PrimitiveSkill")
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 preempt_task(self, uid):
"""
@brief Preempt a task
"""
if uid==-1:
self._ticker.preempt_all()
else:
self._ticker.preempt(uid)
def pause(self, uid):
"""
@brief Stop ticking a task, but do not preempt it
"""
if uid==-1:
self._ticker.pause_all()
else:
self._ticker.pause(uid)
def tick_once(self, uid):
"""
@brief Set a task to go in pause state after 1 tick
"""
if uid==-1:
self._ticker.tick_once_all()
else:
self._ticker.tick_once(uid)
def print_task(self, uid):
self.visitor = visitors.VisitorPrint(self._local_wm, self._instanciator)
self.visitor.setVerbose(self._verbose)
return self._ticker.start(self.visitor, uid)
def execute_task(self, uid, sim=False, track_params=list()): # [("MotionChange",)]
"""
@brief Start or continue a task execution
"""
self.visitor = visitors.VisitorExecutor(self._local_wm, self._instanciator)
self.visitor.setSimulate(sim)
for t in track_params:
self.visitor.trackParam(*t)
self.visitor.setVerbose(self._verbose)
return self._ticker.start(self.visitor, uid)
def clear_tasks(self):
self._ticker.clear()
def execute_optimal(self):
# Optimize Procedure
self.optimizeTask()
self.print_task()
# Execute
return self.execute_task(False)
def simulate_task(self, uid):
self.visitor = visitors.VisitorReversibleSimulator(self._local_wm, self._instanciator)
self.visitor.setVerbose(self._verbose)
# self.visitor.trackParam("Initial")
# self.visitor.trackParam("Gripper")
if self.visitor.traverse(self._tasks[uid]):
self._task = self.visitor.getExecutionRoot()
def optimize_task(self):
self.visitor = optimizer.VisitorOptimizer(self._local_wm, self._instanciator)
# self.visitor.setVerbose(True)
# self.visitor.trackParam("PlacingCell")
# self.visitor.trackParam("Object")
# rospy.sleep(1.)
self.publish("Optimization", 1, "Start.")
try:
if self.visitor.traverse(self._task):
self._task = self.visitor.getExecutionRoot()
return True
else:
self._task = self.visitor.getExecutionRoot()
return False
except KeyError as e:
self._task = self.visitor.getExecutionRoot()
print("Exe: {}".format(self.visitor._execution_branch))
self.print_task()
raise e