Pull ActionXY.apply_preconditions() into base class

Rename GOAPInspection to Introspector
Clean Introspector and Runner

goap/manifest.xml

@@ -16,6 +16,7 @@
   <depend package="metralabs_msgs"/>
   <depend package="actionlib"/>
   <depend package="smach_ros"/>
+  <depend package="uashh_smach"/>
 
 </package>
 

goap/src/goap/common_ros.py

@@ -78,40 +78,41 @@ def __init__(self):
                             [Effect(Condition.get('robot.bumpered'), False)])
         self._publisher = rospy.Publisher('/bumper_reset', Empty)
 
+    def check_freeform_context(self):
+        return self._publisher.get_num_connections() > 0  # unsafe
+
     def run(self, next_worldstate):
+        print 'num of subscribers: ', self._publisher.get_num_connections()
+        print 'sending bumper_reset message..'
         self._publisher.publish(Empty())
+        rospy.sleep(1)  # TODO: find solution without sleep
 
 # TODO: implement denial of trivial actions (not changing conditions)
 
 class MoveBaseAction(Action):
 
-    class PositionEffect(VariableEffect):
+    class PositionVarEffect(VariableEffect):
         def __init__(self, condition):
             VariableEffect.__init__(self, condition)
         def _is_reachable(self, value):
-            return True # TODO: change reachability from boolean to float
+            return True
 
     def __init__(self):
         self._condition = Condition.get('robot.pose')
         Action.__init__(self, [Precondition(Condition.get('robot.bumpered'), False)],
-                        [MoveBaseAction.PositionEffect(self._condition)])
+                        [MoveBaseAction.PositionVarEffect(self._condition)])
 
         self._client = actionlib.SimpleActionClient('move_base', move_base_msgs.msg.MoveBaseAction)
 
     def check_freeform_context(self):
         # TODO: cache freeform context?
         return self._client.wait_for_server(rospy.Duration(0.1))
 
-    def apply_preconditions(self, worldstate, start_worldstate): # TODO: move this to new class VariableAction?
-        Action.apply_preconditions(self, worldstate, start_worldstate) # apply fix preconditions
-        # calculate an ad hoc precondition for our variable effect and apply it
-        effect_value = worldstate.get_condition_value(self._condition)
-        precond_value = self._calc_preconditional_value(worldstate, start_worldstate, effect_value)
-        Precondition(self._condition, precond_value, None).apply(worldstate)
-
-    def _calc_preconditional_value(self, worldstate, start_worldstate, effect_value):
-        start_pose = start_worldstate.get_condition_value(Condition.get('robot.pose'))
-        return start_pose
+    def apply_adhoc_preconditions_for_vareffects(self, var_effects, worldstate, start_worldstate):
+        effect = var_effects.pop()  # this action has one variable effect
+        assert effect.__class__ == MoveBaseAction.PositionVarEffect
+        precond_value = start_worldstate.get_condition_value(Condition.get('robot.pose'))
+        Precondition(effect._condition, precond_value, None).apply(worldstate)
 
     def run(self, next_worldstate):
         goal = MoveBaseGoal()

goap/src/goap/goap.py

@@ -48,6 +48,7 @@ def __str__(self):
 
     def __repr__(self):
         return '<WorldState %X values=%s>' % (id(self), self._condition_values)
+#        return '<WorldState>'
 
     def get_condition_value(self, condition):
         return self._condition_values[condition]
@@ -69,28 +70,22 @@ def matches(self, start_worldstate):
 #        print 'start: ', start_ws_dict
         return matches
 
-#    def apply_effects(self, action): # TODO: replace by direct calls to action.apply_effects()
-# delete me       action.apply_effects(self)
-
     def get_state_name_dict(self):
         """Returns a dictionary with not the conditions themselves but their state_names as keys."""
-        d = {}
-        for c, v in self._condition_values.viewitems():
-            d[c._state_name] = v
-        return d
+        return {cond._state_name: val for cond, val in self._condition_values.viewitems()}
 
 
 
 ## known as state
 class Condition(object):
     """The object that makes any kind of robot or system state available.
-    
+
     This class, at least its static part, is a multiton:
-    * For each state_name only one instance is allowed to be in the 
+    * For each state_name only one instance is allowed to be in the
       _conditions_dict mapping.
     * If there is no mapping for a get(state_name) call an assertion is
       triggered, as creating a new instance makes no sense here.
-    
+
     self._state_name: id name of condition, must not be changed
     """
 
@@ -206,6 +201,8 @@ def matches_condition(self, worldstate):
         return self._is_reachable(worldstate.get_condition_value(self._condition))
 
     def _is_reachable(self, value):
+        """Returns a Boolean whether this variable effect can reach the given value"""
+        # TODO: change reachability from boolean to float
         raise NotImplementedError
 
 
@@ -280,11 +277,24 @@ def has_satisfying_effects(self, worldstate, unsatisfied_conditions):
 
     def apply_preconditions(self, worldstate, start_worldstate):
         """
+        worldstate: worldstate to apply this action's preconditions to
         start_worldstate: needed to let actions optimize their variable precondition parameters
         """
         # TODO: make required derivation of variable actions more obvious and fail-safe
         for precondition in self._preconditions:
             precondition.apply(worldstate)
+        # let the action apply ad hoc preconditions for its variable effects
+        var_effects = [effect for effect in self._effects if isinstance(effect, VariableEffect)]
+        if len(var_effects) > 0:
+            self.apply_adhoc_preconditions_for_vareffects(var_effects, worldstate, start_worldstate)
+
+    def apply_adhoc_preconditions_for_vareffects(self, var_effects, worldstate, start_worldstate):
+        """
+        Let the action itself apply preconditions for its variable effects.
+
+        Must be implemented if the action contains variable effects.
+        """
+        raise NotImplementedError
 
 
 

goap/src/goap/inheriting.py

@@ -111,7 +111,7 @@ class IncEffect(VariableEffect):
         def __init__(self, condition):
             VariableEffect.__init__(self, condition)
         def _is_reachable(self, value):
-            return True # TODO: change reachability from boolean to float
+            return True
 
 
     def __init__(self, memory, variable, increment=1):
@@ -138,14 +138,13 @@ def cost(self):
     def run(self, next_worldstate):
         self._memory.set_value(self._variable, self._memory.get_value(self._variable) + self._increment)
 
-    def apply_preconditions(self, worldstate, start_worldstate):
-        # calculate an ad hoc precondition for our variable effect and apply it
-        effect_value = worldstate.get_condition_value(self._condition)
-        precond_value = self._calc_preconditional_value(worldstate, start_worldstate, effect_value)
-        Precondition(self._condition, precond_value, None).apply(worldstate)
+    def apply_adhoc_preconditions_for_vareffects(self, var_effects, worldstate, start_worldstate):
+        effect = var_effects.pop()  # this action has one variable effect
+        assert effect.__class__ == MemoryIncrementerAction.IncEffect
+        precond_value = worldstate.get_condition_value(effect._condition) - self._increment
+        Precondition(effect._condition, precond_value, None).apply(worldstate)
+
 
-    def _calc_preconditional_value(self, worldstate, start_worldstate, effect_value):
-        return effect_value - self._increment
 
 
 

goap/src/goap/introspection.py

@@ -35,13 +35,11 @@
 from smach_msgs.msg import SmachContainerStatus, SmachContainerStructure
 from smach_ros.introspection import STATUS_TOPIC, STRUCTURE_TOPIC
 
-from planning import Node
-
-class GOAPIntrospection(object):
-    '''
-    classdocs
-    '''
 
+class Introspector(object):
+    """Gives insight to a planner's plan and GOAP net by publishing prepared
+    information to a smach_viewer.
+    """
     def __init__(self):
         self._pathprefix = '/GOAP_PLAN'
         self._pathprefix_net = '/GOAP_NET'
@@ -51,57 +49,35 @@ def __init__(self):
         self._publisher_status_net = rospy.Publisher(self._pathprefix_net + STATUS_TOPIC, SmachContainerStatus, latch=True)
 
 
-    def _add_nodes_recursively(self, node, structure):
-        """node: beginning with the start node"""
-        structure.children.append(self._nodeid(node))
-
-        if len(node.parent_nodes_path_list) > 0:
-            next_node = node.parent_nodes_path_list[-1]
-
-            structure.internal_outcomes.append(str(node.action._effects))
-            structure.outcomes_from.append(self._nodeid(node))
-            structure.outcomes_to.append(self._nodeid(next_node))
-#
-#            structure.internal_outcomes.append('aborted')
-#            structure.outcomes_from.append(self._nodeid(node))
-#            structure.outcomes_to.append('None')
-
-            self._add_nodes_recursively(next_node, structure)
-#        else: # goal node
-#            structure.internal_outcomes.append('succeeded')
-#            structure.outcomes_from.append(self._nodeid(node))
-#            structure.outcomes_to.append('None')
-
-
-    def _add_nodes_recursively_net(self, node, structure):
-        """node: beginning with the goal node"""
-        structure.children.append(self._nodeid(node))
-
-        if len(node.possible_prev_nodes) > 0: # goal or inner node
-            for prev_node in node.possible_prev_nodes:
-                structure.internal_outcomes.append(str(prev_node.action._effects))
-                structure.outcomes_from.append(self._nodeid(prev_node))
-                structure.outcomes_to.append(self._nodeid(node))
-                self._add_nodes_recursively_net(prev_node, structure)
-        else: # start or dead-end node
-            pass
-
-#        if len(node.parent_nodes_path_list) == 0: # goal node
-#            structure.internal_outcomes.append('succeeded')
-#            structure.outcomes_from.append(self._nodeid(node))
-#            structure.outcomes_to.append('None')
-#        else:
-#            structure.internal_outcomes.append('aborted')
-#            structure.outcomes_from.append(self._nodeid(node))
-#            structure.outcomes_to.append('None')
-
-
     def publish_net(self, start_node, goal_node):
+        """Publishes a GOAP planning net"""
+        def _add_nodes_recursively(node, structure):
+            """node: beginning with the goal node"""
+            structure.children.append(self._nodeid(node))
+
+            if len(node.possible_prev_nodes) > 0: # goal or inner node
+                for prev_node in node.possible_prev_nodes:
+                    structure.internal_outcomes.append(str(prev_node.action._effects))
+                    structure.outcomes_from.append(self._nodeid(prev_node))
+                    structure.outcomes_to.append(self._nodeid(node))
+                    _add_nodes_recursively(prev_node, structure)
+            else: # start or dead-end node
+                pass
+
+#            if len(node.parent_nodes_path_list) == 0: # goal node
+#                structure.internal_outcomes.append('succeeded')
+#                structure.outcomes_from.append(self._nodeid(node))
+#                structure.outcomes_to.append('None')
+#            else:
+#                structure.internal_outcomes.append('aborted')
+#                structure.outcomes_from.append(self._nodeid(node))
+#                structure.outcomes_to.append('None')
+
         structure = SmachContainerStructure()
         structure.header.stamp = rospy.Time.now()
         structure.path = self._pathprefix_net
 #        structure.container_outcomes = ['succeeded', 'aborted']
-        self._add_nodes_recursively_net(goal_node, structure)
+        _add_nodes_recursively(goal_node, structure)
         self._publisher_structure_net.publish(structure)
         print 'published net has ~%s nodes' % len(structure.children)
 
@@ -111,18 +87,42 @@ def publish_net(self, start_node, goal_node):
         status.initial_states = [self._nodeid(start_node) if start_node is not None else 'No plan found']
         #status.active_states = ['None']
         status.active_states = [self._nodeid(goal_node)]
-        status.local_data = pickle.dumps(goal_node.worldstate.get_state_name_dict(), 2)
-        status.info = 'initial state'
+        goal_states_dict = goal_node.worldstate.get_state_name_dict()
+        goal_states_dict['WORLDSTATE'] = 'GOAL'
+        status.local_data = pickle.dumps(goal_states_dict, 2)
+        status.info = 'goal state'
         self._publisher_status_net.publish(status)
 
         rospy.sleep(5)
 
     def publish(self, start_node):
+        """Publishes a planned GOAP plan"""
+        def _add_nodes_recursively(node, structure):
+            """node: beginning with the start node"""
+            structure.children.append(self._nodeid(node))
+
+            if len(node.parent_nodes_path_list) > 0:
+                next_node = node.parent_nodes_path_list[-1]
+
+                structure.internal_outcomes.append(str(node.action._effects))
+                structure.outcomes_from.append(self._nodeid(node))
+                structure.outcomes_to.append(self._nodeid(next_node))
+#
+#                structure.internal_outcomes.append('aborted')
+#                structure.outcomes_from.append(self._nodeid(node))
+#                structure.outcomes_to.append('None')
+
+                _add_nodes_recursively(next_node, structure)
+#            else: # goal node
+#                structure.internal_outcomes.append('succeeded')
+#                structure.outcomes_from.append(self._nodeid(node))
+#                structure.outcomes_to.append('None')
+
         structure = SmachContainerStructure()
         structure.header.stamp = rospy.Time.now()
         structure.path = self._pathprefix
 #        structure.container_outcomes = ['succeeded', 'aborted']
-        self._add_nodes_recursively(start_node, structure)
+        _add_nodes_recursively(start_node, structure)
         self._publisher_structure.publish(structure)
         print 'published plan has ~%s nodes' % len(structure.children)
 
@@ -131,7 +131,9 @@ def publish(self, start_node):
         status.path = self._pathprefix
         status.initial_states = [self._nodeid(start_node)]
         status.active_states = ['None']
-        status.local_data = pickle.dumps(start_node.worldstate.get_state_name_dict(), 2)
+        start_states_dict = start_node.worldstate.get_state_name_dict()
+        start_states_dict['WORLDSTATE'] = 'START'
+        status.local_data = pickle.dumps(start_states_dict, 2)
         status.info = 'initial state'
         self._publisher_status.publish(status)
 
@@ -146,8 +148,9 @@ def publish_update(self, node):
         status.path = self._pathprefix
         status.initial_states = []
         status.active_states = [nodeid]
-#        status.local_data = pickle.dumps(self._worldstate_to_userdata(start_node.worldstate)._data, 2)
-        status.local_data = pickle.dumps(node.worldstate.get_state_name_dict(), 2)
+        start_states_dict = node.worldstate.get_state_name_dict()
+        start_states_dict['WORLDSTATE'] = str(node)
+        status.local_data = pickle.dumps(start_states_dict, 2)
         status.info = 'node state'
         self._publisher_status.publish(status)
 

goap/src/goap/planning.py

@@ -98,19 +98,20 @@ def __init__(self, actionbag, worldstate, goal):
 
         self.last_goal_node = None
 
-    def plan(self, goal=None):
+    def plan(self, start_worldstate=None, goal=None):
         """Plan ...
         Return the node that matches the given start WorldState and
         is the start node for a plan reaching the given Goal.
 
         If any parameter is not given the data given at initialisation is used.
         """
 
+        if start_worldstate is not None:
+            self._start_worldstate = start_worldstate
         if goal is not None:
             self._goal = goal
 
         print 'start_worldstate: ', self._start_worldstate
-
         print 'goal: ', self._goal
 
         goal_worldstate = WorldState()
@@ -122,15 +123,12 @@ def plan(self, goal=None):
         print 'goal_node: ', goal_node
         self.last_goal_node = goal_node
 
-#         worldstate = self._start_worldstate
-
         child_nodes = deque([goal_node])
 
-#         while not self._goal.is_valid(worldstate):
         loopcount = 0
         while len(child_nodes) != 0:
             loopcount += 1
-            if loopcount > 2000: # loop limit
+            if loopcount > 500: # loop limit
                 print "Warning: planner stops because loop limit (%s) hit!" % (loopcount - 1)
                 break
 
@@ -149,7 +147,6 @@ def plan(self, goal=None):
                 print 'plan actions: ', current_node.parent_actions_path_list
                 return current_node
 
-            #current_node.check_and_add_actions(self._actionbag)
             print "Current node: ", current_node
 
             new_child_nodes = current_node.get_child_nodes_for_valid_actions(