Fix MoveBaseAction and GOAPTestBumpered

goap/src/goap/common_ros.py

@@ -76,9 +76,10 @@ class ResetBumperAction(Action):
     def __init__(self):
         Action.__init__(self, [Precondition(Condition.get('robot.bumpered'), True)],  # TODO: Precondition fails if message wasn't received yet
                             [Effect(Condition.get('robot.bumpered'), False)])
+        self._publisher = rospy.Publisher('/bumper_reset', Empty)
 
     def run(self, next_worldstate):
-        rospy.Publisher('/bumper_reset', Empty).publish()
+        self._publisher.publish(Empty())
 
 # TODO: implement denial of trivial actions (not changing conditions)
 
@@ -99,7 +100,7 @@ def __init__(self):
 
     def check_freeform_context(self):
         # TODO: cache freeform context?
-        return not self._client.wait_for_server(rospy.Duration(0.1))
+        return self._client.wait_for_server(rospy.Duration(0.1))
 
     def apply_preconditions(self, worldstate): # TODO: move this to new class VariableAction?
         Action.apply_preconditions(self, worldstate) # apply fix preconditions

goap/test/GOAPTestBumpered.py

@@ -64,14 +64,14 @@ def calc_Pose(x, y, yaw):
 #    unittest.main()
 
 
-    rospy.init_node('goap_bumper_test')
+    rospy.init_node('goap_bumper_test', log_level=rospy.INFO)
 
     memory = Memory()
 
     Condition.add(ROSTopicCondition(
                     'robot.pose', '/odom', Odometry, '/pose/pose'))
     Condition.add(ROSTopicCondition(
-                    'robot.bumpered', '/bumper_state', ScitosG5Bumper, '/bumper_pressed'))
+                    'robot.bumpered', '/bumper', ScitosG5Bumper, '/motor_stop'))
     Condition.add(MemoryCondition(memory, 'reminded_myself'))
 
     worldstate = WorldState()
@@ -95,11 +95,10 @@ def calc_Pose(x, y, yaw):
 
     print 'start_node: ', start_node
 
-    PlanExecutor().execute(start_node)
-
-
-    ## init / spin to let conditions know reality
+    rospy.sleep(10)
 
+    PlanExecutor().execute(start_node)
 
 
+    rospy.sleep(20)