Convert GOAPTestBumpered and second half of GOAPTestMemory to use a R…

…unner

goap/src/goap/runner.py

@@ -85,7 +85,7 @@ def __init__(self, config_module=None):
                 self.actionbag.add(action)
 
         print 'letting conditions receive reality...'
-        rospy.sleep(2)
+        rospy.sleep(2)  # TODO: avoid sleeping if no ROS reality is used
 
         Condition.initialize_worldstate(self.worldstate)
 
@@ -105,6 +105,7 @@ def update_and_plan(self, goal, tries=1):
             start_node = self.planner.plan(goal)
             if start_node is not None:
                 return start_node
+        return None
 
     def update_and_plan_and_execute(self, goal, tries=1):
         start_node = self.update_and_plan(goal, tries)

goap/test/GOAPTestBumpered.py

@@ -42,6 +42,7 @@
 from goap.inheriting import *
 from goap.common_ros import *
 from goap.planning import Planner, PlanExecutor
+from goap.runner import Runner
 
 
 class Test(unittest.TestCase):
@@ -66,38 +67,37 @@ def calc_Pose(x, y, yaw):
 
     rospy.init_node('goap_bumper_test', log_level=rospy.INFO)
 
-    memory = Memory()
+    runner = Runner()
 
     Condition.add(ROSTopicCondition(
                     'robot.pose', '/odom', Odometry, '/pose/pose'))
     Condition.add(ROSTopicCondition(
                     'robot.bumpered', '/bumper', ScitosG5Bumper, '/motor_stop'))
-    Condition.add(MemoryCondition(memory, 'reminded_myself'))
+    Condition.add(MemoryCondition(runner.memory, 'reminded_myself'))
 
-    worldstate = WorldState()
 
     print 'Waiting to let conditions represent reality...'
     print 'Remember to start topic publishers so conditions make sense instead of None!'
     rospy.sleep(2)
-    Condition.initialize_worldstate(worldstate)
-    print 'worldstate now is: ', worldstate
+    Condition.initialize_worldstate(runner.worldstate)
+    print 'worldstate now is: ', runner.worldstate
 
-    actionbag = ActionBag()
-    actionbag.add(ResetBumperAction())
-    actionbag.add(MoveBaseAction())
+    runner.actionbag.add(ResetBumperAction())
+    runner.actionbag.add(MoveBaseAction())
 
 
     goal = Goal([Precondition(Condition.get('robot.pose'), calc_Pose(1, 0, 0))])
 
-    planner = Planner(actionbag, worldstate, goal)
-
-    start_node = planner.plan()
+    start_node = runner.update_and_plan(goal)
 
     print 'start_node: ', start_node
 
     rospy.sleep(10)
 
-    PlanExecutor().execute(start_node)
+    if start_node is None:
+        print 'No plan found! Check you ROS graph!'
+    else:
+        PlanExecutor().execute(start_node)
 
 
     rospy.sleep(20)

goap/test/GOAPTestMemory.py

@@ -110,20 +110,22 @@ def tearDown(self):
 class TestIncrementer(unittest.TestCase):
 
     def setUp(self):
-        self.memory = Memory()
-        self.worldstate = WorldState()
+        self.runner = Runner()
+
+        self.memory = self.runner.memory
+        self.worldstate = self.runner.worldstate
 
         self.memory.set_value('memory.counter', 0)
 
         print self.memory
 
-        Condition._conditions_dict.clear()
+        Condition._conditions_dict.clear() # start every test without previous conditions
 
         Condition.add(MemoryCondition(self.memory, 'counter'))
 
         Condition.initialize_worldstate(self.worldstate)
 
-        self.actionbag = ActionBag()
+        self.actionbag = self.runner.actionbag
         self.actionbag.add(MemoryIncrementerAction(self.memory, 'counter'))
 
         print Condition.print_dict()
@@ -136,15 +138,16 @@ def setUp(self):
 
         print self.worldstate
 
+        print self.runner
 
     def testGoals(self):
         print '==', self.testGoals.__name__
+        Condition.initialize_worldstate(self.worldstate) # needed because worldstate was never initialized before
         self.assertFalse(self.goal.is_valid(self.worldstate), 'Goal should not be valid yet')
 
     def testPlannerPos(self):
         print '==', self.testPlannerPos.__name__
-        planner = Planner(self.actionbag, self.worldstate, self.goal)
-        start_node = planner.plan()
+        start_node = self.runner.update_and_plan(self.goal)
         print 'start_node found: ', start_node
         self.assertIsNotNone(start_node, 'There should be a plan')
         self.assertIsInstance(start_node, Node, 'Plan should be a Node')
@@ -160,8 +163,7 @@ def testPlannerPosUnneededCondition(self):
 
     def testPlannerNeg(self):
         print '==', self.testPlannerNeg.__name__
-        planner = Planner(self.actionbag, self.worldstate, self.goal_inaccessible)
-        start_node = planner.plan()
+        start_node = self.runner.update_and_plan(self.goal_inaccessible)
         print 'start_node found: ', start_node
         self.assertIsNone(start_node, 'There should be no plan')
 
@@ -171,8 +173,7 @@ def testPlannerNegPos(self):
         """
         print '==', self.testPlannerPos.__name__
         self.actionbag.add(MemoryIncrementerAction(self.memory, 'counter', -4))
-        planner = Planner(self.actionbag, self.worldstate, self.goal_inaccessible)
-        start_node = planner.plan()
+        start_node = self.runner.update_and_plan(self.goal_inaccessible)
         print 'start_node found: ', start_node
         self.assertIsNotNone(start_node, 'There should be a plan')
         self.assertIsInstance(start_node, Node, 'Plan should be a Node')