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)
class TaskManagerNode(PrettyObject):
def __init__(self):
"""
This class manage the robot task planning. A list of goals can be set by
external agent (e.g. users) with the action server 'task_plan' The task
manager plans a sequence of skills to reach the goals and returns it.
Initialize task manager as a ros node. Establish access to the global
and local world model, skill manager and the task planner.
"""
rospy.init_node("task_manager", anonymous=False)
self._author_name = rospy.get_name()
self._goals = []
self._skills = {}
self._abstract_objects = []
self._wmi = wmi.WorldModelInterface(self._author_name)
self._sli = sli.SkillLayerInterface(self._author_name)
self._pddl_interface = pddl.PddlInterface()
self._verbose = rospy.get_param('~verbose', True)
log.setLevel(log.INFO)
self._assign_task_action = actionlib.SimpleActionServer(
'~task_plan', msgs.AssignTaskAction, execute_cb=self._assign_task_cb, auto_start=False)
self._assign_task_action.start()
self._is_ready = False
@property
def skills(self):
"""Get available skills.
Return the updated list of skills available in the system
Returns:
dict: {Skill name : instance? }
"""
if self._sli.has_changes:
self._skills.clear()
for ak, e in self._sli._agents.iteritems():
for sk, s in e._skill_list.iteritems():
s.manager = ak
self._skills[sk] = s
return self._skills
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 OSError, e:
self._result = msgs.AssignTaskResult(1, "FD task planner not found. Maybe is not installed?")
self._assign_task_action.set_aborted(self._result)
except Exception, e:
self._result = msgs.AssignTaskResult(1, str(e))
self._assign_task_action.set_aborted(self._result)