diff --git a/goap/src/goap/runner.py b/goap/src/goap/runner.py index 092f451..87b6594 100644 --- a/goap/src/goap/runner.py +++ b/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) diff --git a/goap/test/GOAPTestBumpered.py b/goap/test/GOAPTestBumpered.py index 41b57ef..7125158 100644 --- a/goap/test/GOAPTestBumpered.py +++ b/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) diff --git a/goap/test/GOAPTestMemory.py b/goap/test/GOAPTestMemory.py index 9be85d5..d523607 100644 --- a/goap/test/GOAPTestMemory.py +++ b/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')