Add pathprefix override parameter to Introspector.publish()

goap/src/goap/introspection.py

@@ -97,7 +97,7 @@ def _add_nodes_recursively(node, structure):
 
         rospy.sleep(5)
 
-    def publish(self, start_node):
+    def publish(self, start_node, pathprefix=None):
         """Publishes a planned GOAP plan
 
         The start node will be set as the active state, as its userdata is displayed.
@@ -125,7 +125,7 @@ def _add_nodes_recursively(node, structure):
 
         structure = SmachContainerStructure()
         structure.header.stamp = rospy.Time.now()
-        structure.path = self._pathprefix
+        structure.path = self._pathprefix if pathprefix is None else pathprefix
 #        structure.container_outcomes = ['succeeded', 'aborted']
         _add_nodes_recursively(start_node, structure)
         self._publisher_structure.publish(structure)

goap/src/goap/runner.py

@@ -174,6 +174,12 @@ def plan_and_execute_goals_NOT_USED(self, goals):
                                                           key=lambda t: t[1],
                                                           reverse=True))
 
+        # introspection
+        for goal, start_node in goal_dict.iteritems():
+            if start_node is not None:
+                self._introspector.publish(
+                       start_node, '/MULTIPLE_GOALS/' + goal.__class__.__name__)
+
         # execution
         if len(goals_with_plan) > 0:
             goal = goals_with_plan[0]