From 00f5690c176851580cf7a70b85d1b28c392ebd2f Mon Sep 17 00:00:00 2001 From: Felix Kolbe Date: Thu, 1 Aug 2013 18:13:51 +0200 Subject: [PATCH] Fix MoveBaseAction and GOAPTestBumpered --- goap/src/goap/common_ros.py | 5 +++-- goap/test/GOAPTestBumpered.py | 11 +++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/goap/src/goap/common_ros.py b/goap/src/goap/common_ros.py index 7c28cb0..a7b3bcc 100644 --- a/goap/src/goap/common_ros.py +++ b/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 diff --git a/goap/test/GOAPTestBumpered.py b/goap/test/GOAPTestBumpered.py index a71b16f..41b57ef 100644 --- a/goap/test/GOAPTestBumpered.py +++ b/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)