From 7d76370ae0fa1fe254d975b80275b8fc0dc8852f Mon Sep 17 00:00:00 2001 From: Felix Kolbe Date: Mon, 25 Nov 2013 14:44:25 +0100 Subject: [PATCH] Make Runner propagate preemption request into generated smach --- goap/src/goap/runner.py | 62 ++++++++++++++++++++++++++++++++++------- 1 file changed, 52 insertions(+), 10 deletions(-) diff --git a/goap/src/goap/runner.py b/goap/src/goap/runner.py index a992cc1..f14d0f5 100644 --- a/goap/src/goap/runner.py +++ b/goap/src/goap/runner.py @@ -78,9 +78,11 @@ def __init__(self, config_module=None): self.planner = Planner(self.actionbag, self.worldstate, None) self._introspector = None - self._last_goal = None + self._preempt_requested = False # preemption propagation workaround (not multi-threadable) + self._current_smach = None # used to propagate preempt into generated smach + def __repr__(self): return '<%s memory=%s worldstate=%s actions=%s planner=%s>' % (self.__class__.__name__, @@ -96,6 +98,23 @@ def _setup_introspection(self): thread.start_new_thread(rospy.spin, ()) print "introspection spinner started" + + def request_preempt(self): + self._preempt_requested = True + if self._current_smach is not None: + self._current_smach.request_preempt() + + def preempt_requested(self): + return self._preempt_requested or (self._current_smach.preempt_requested() + if self._current_smach is not None + else False) + + def service_preempt(self): + self._preempt_requested = False + if self._current_smach is not None: + self._current_smach.service_preempt() + + def _update_worldstate(self): """update worldstate to reality""" Condition.initialize_worldstate(self.worldstate) @@ -176,6 +195,9 @@ def plan_and_execute_goals_NOT_USED(self, goals): # introspection for goal, start_node in goal_dict.iteritems(): + if self.preempt_requested(): + self.service_preempt() + return 'preempted' if start_node is not None: self._introspector.publish( start_node, '/MULTIPLE_GOALS/' + goal.__class__.__name__) @@ -213,6 +235,9 @@ def plan_and_execute_goals(self, goals): if self._last_goal is goal: continue + if self.preempt_requested(): + self.service_preempt() + return 'preempted' plan = self.plan(goal) if plan is None: @@ -244,6 +269,10 @@ def update_and_plan_and_execute(self, goal, tries=1, introspection=False): outcome = None # replan and retry on failure as long as a plan is found while not rospy.is_shutdown(): + if self.preempt_requested(): + self.service_preempt() + return 'preempted' + start_node = self.update_and_plan(goal, tries, introspection) if start_node is None: @@ -269,14 +298,17 @@ def update_and_plan_and_execute(self, goal, tries=1, introspection=False): # until we are succeeding or are preempted return outcome + def execute_as_smach(self, start_node, introspection=False): - sm = self.path_to_smach(start_node) + sm = self._path_to_smach(start_node) # TODO: create proxies / userdata info for inner-sm introspection - outcome = execute_smach_container(sm, introspection, name='/SM_GENERATED') + self._current_smach = sm + outcome = execute_smach_container(sm, introspection, name='/GOAP_GENERATED_SMACH') + self._current_smach = None return outcome - def path_to_smach(self, start_node): + def _path_to_smach(self, start_node): sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) node = start_node @@ -319,14 +351,11 @@ def __init__(self, runner, **kwargs): self.runner = runner def execute(self, userdata): - # TODO: propagate preemption request into goap submachine # TODO: maybe make this class a smach.Container and add states dynamically? goal = self._build_goal(userdata) outcome = self.runner.update_and_plan_and_execute(goal, introspection=True) print "Generated GOAP sub state machine returns: %s" % outcome if self.preempt_requested(): - rospy.logwarn("Preempt request was ignored as GOAPPlannerState cannot" - " yet forward it to inner generated machine.") self.service_preempt() return 'preempted' return outcome @@ -335,6 +364,14 @@ def _build_goal(self, userdata): """Build and return a goap.Goal the planner should accomplish""" raise NotImplementedError + def request_preempt(self): + self.runner.request_preempt() + State.request_preempt(self) + + def service_preempt(self): + self.runner.service_preempt() + State.service_preempt(self) + # TODO: merge into GOAPRunnerState, renaming _build_goal to _build_goals class GOAPGoalsState(State): @@ -344,14 +381,11 @@ def __init__(self, runner, **kwargs): self.runner = runner def execute(self, userdata): -# # TODO: propagate preemption request into goap submachine # # TODO: maybe make this class a smach.Container and add states dynamically? goals = self._build_goals(userdata) outcome = self.runner.plan_and_execute_goals(goals) print "Generated GOAP sub state machine returns: %s" % outcome if self.preempt_requested(): - rospy.logwarn("Preempt request was ignored as GOAPGoalsState cannot" - " yet forward it to inner generated machine.") self.service_preempt() return 'preempted' return outcome @@ -360,3 +394,11 @@ def _build_goals(self, userdata): """Build and return a goap.Goal list the planner should accomplish""" raise NotImplementedError + def request_preempt(self): + self.runner.request_preempt() + State.request_preempt(self) + + def service_preempt(self): + self.runner.service_preempt() + State.service_preempt(self) +