def setUp(self): rospy.init_node(self.__class__.__name__) Condition._conditions_dict.clear() # start every test without previous conditions memory = Memory() self.rtc = common_ros.ROSTopicCondition('topic.testbool', Test.topic, Bool, '/data') Condition.add(self.rtc) Condition.add(MemoryCondition(memory, 'memory.anyvar')) print self.rtc self.rta_true = common_ros.ROSTopicAction(Test.topic, Bool, [Precondition(Condition.get('memory.anyvar'), 42)], [Effect('topic.testbool', True)], msg_args=[True]) self.rta_false = common_ros.ROSTopicAction(Test.topic, Bool, [Precondition(Condition.get('memory.anyvar'), 42)], [Effect('topic.testbool', False)], msg_args=[False]) def msg_cb(message, next_worldstate, value): message.data = value return message self.rta_cb_true = common_ros.ROSTopicAction(Test.topic, Bool, [Precondition(Condition.get('memory.anyvar'), 42)], [Effect('topic.testbool', True)], msg_cb=lambda msg, ws: msg_cb(msg, ws, True)) self.rta_cb_false = common_ros.ROSTopicAction(Test.topic, Bool, [Precondition(Condition.get('memory.anyvar'), 42)], [Effect('topic.testbool', False)], msg_cb=lambda msg, ws: msg_cb(msg, ws, False))
def testRunner(self): memory = self.runner.memory Condition.add(MemoryCondition(memory, 'awareness', 0)) Condition.add(MemoryCondition(memory, 'arm_can_move', True)) self.runner.actions.add(LookAroundAction()) print self.runner.actions goal = Goal([Precondition(Condition.get('awareness'), 2)]) self.runner.update_and_plan_and_execute(goal)
def setUp(self): rospy.init_node(self.__class__.__name__) Condition._conditions_dict.clear( ) # start every test without previous conditions memory = Memory() self.rtc = common_ros.ROSTopicCondition('topic.testbool', Test.topic, Bool, '/data') Condition.add(self.rtc) Condition.add(MemoryCondition(memory, 'memory.anyvar')) print self.rtc self.rta_true = common_ros.ROSTopicAction( Test.topic, Bool, [Precondition(Condition.get('memory.anyvar'), 42)], [Effect('topic.testbool', True)], msg_args=[True]) self.rta_false = common_ros.ROSTopicAction( Test.topic, Bool, [Precondition(Condition.get('memory.anyvar'), 42)], [Effect('topic.testbool', False)], msg_args=[False]) def msg_cb(message, next_worldstate, value): message.data = value return message self.rta_cb_true = common_ros.ROSTopicAction( Test.topic, Bool, [Precondition(Condition.get('memory.anyvar'), 42)], [Effect('topic.testbool', True)], msg_cb=lambda msg, ws: msg_cb(msg, ws, True)) self.rta_cb_false = common_ros.ROSTopicAction( Test.topic, Bool, [Precondition(Condition.get('memory.anyvar'), 42)], [Effect('topic.testbool', False)], msg_cb=lambda msg, ws: msg_cb(msg, ws, False))
from rgoap import Condition, MemoryCondition, Precondition, Goal from rgoap.memory import MemoryChangeVarAction from rgoap_ros import SMACHRunner import uashh_smach.rgoap_subclasses as rgoap_subclasses from uashh_smach.platform.move_base import position_tuple_to_pose if __name__ == "__main__": rospy.init_node('rgoap_bumper_test', log_level=rospy.INFO) runner = SMACHRunner(rgoap_subclasses) Condition.add(MemoryCondition(runner.memory, 'memory.reminded_myself')) runner.memory.set_value('awareness', 0) runner.memory.set_value('arm_can_move', True) runner.memory.set_value('memory.reminded_myself', 333) rospy.loginfo("Waiting to let conditions represent reality...") rospy.loginfo( "Remember to start topic publishers so conditions make sense instead of None!" ) rospy.sleep(2) Condition.initialize_worldstate(runner.worldstate) rospy.loginfo("worldstate now is: %s", runner.worldstate) runner.actions.add( MemoryChangeVarAction(runner.memory, 'memory.reminded_myself', 333,
class SymmetricAction(Action): def __init__(self): Action.__init__(self, [Precondition(Condition.get('robot.bumpered'), True)], [Effect(Condition.get('robot.bumpered'), False)]) def run(self, next_worldstate): print '%s: resetting bumper..' % self.__class__ if __name__ == "__main__": runner = Runner() Condition.add(MemoryCondition(runner.memory, 'robot.bumpered', True)) runner.actions.add(SymmetricAction()) Condition.initialize_worldstate(runner.worldstate) print 'worldstate now is: ', runner.worldstate goal = Goal([Precondition(Condition.get('robot.bumpered'), False)]) start_node = runner.update_and_plan(goal, introspection=True) print 'start_node: ', start_node if start_node is None: print 'No plan found! Check your ROS graph!' else:
from rgoap.memory import MemoryChangeVarAction from rgoap_ros import SMACHRunner import uashh_smach.rgoap_subclasses as rgoap_subclasses from uashh_smach.platform.move_base import position_tuple_to_pose if __name__ == "__main__": rospy.init_node("rgoap_bumper_test", log_level=rospy.INFO) runner = SMACHRunner(rgoap_subclasses) Condition.add(MemoryCondition(runner.memory, "memory.reminded_myself")) runner.memory.set_value("awareness", 0) runner.memory.set_value("arm_can_move", True) runner.memory.set_value("memory.reminded_myself", 333) rospy.loginfo("Waiting to let conditions represent reality...") rospy.loginfo("Remember to start topic publishers so conditions make sense instead of None!") rospy.sleep(2) Condition.initialize_worldstate(runner.worldstate) rospy.loginfo("worldstate now is: %s", runner.worldstate) runner.actions.add(MemoryChangeVarAction(runner.memory, "memory.reminded_myself", 333, 555)) goal = Goal( [
def testStateWrapperAction(self): # The in-value is translated into the SMACHStateWrapperAction # and checked inside the wrapped state to check data translation # to the wrapped state. NUMBER_IN = 123 # The out-value is translated into the SMACHStateWrapperAction # and out of it again to check data translation from the wrapped # state and also the next_worldstate parameter. NUMBER_OUT = 456 Condition.add( MemoryCondition(self.runner.memory, 'memory.in', NUMBER_IN)) Condition.add(MemoryCondition(self.runner.memory, 'memory.out', -1)) class ValueLooperState(State): def __init__(self): State.__init__(self, ['succeeded', 'aborted'], input_keys=['i', 'to_loop'], output_keys=['o']) def execute(self, userdata): print "%s found 'i' in userdata to be %s.." % ( self.__class__.__name__, userdata.i) if userdata.i == NUMBER_IN: print "..which is correct" userdata.o = userdata.to_loop #return 'succeeded' # will only work when using rgoap_ros.SMACHRunner else: print "..which is not correct!" #return 'aborted' # see above class TranslateAction(SMACHStateWrapperAction): def __init__(self): SMACHStateWrapperAction.__init__(self, ValueLooperState(), [ Precondition(Condition.get('memory.in'), NUMBER_IN), Precondition(Condition.get('memory.out'), -1) ], [Effect(Condition.get('memory.out'), NUMBER_OUT)]) def translate_worldstate_to_userdata(self, next_worldstate, userdata): userdata.i = next_worldstate.get_condition_value( Condition.get('memory.in')) userdata.to_loop = next_worldstate.get_condition_value( Condition.get('memory.out')) def translate_userdata_to_worldstate(self, userdata, next_worldstate): print "FIXME: translation from userdata does not work" # FIXME: translation from userdata does not work # # memory.set_value('memory.out', userdata.o) # next_worldstate.set_condition_value(Condition.get('memory.out'), userdata.o) self.runner.actions.add(TranslateAction()) goal = Goal([ Precondition(Condition.get('memory.out'), NUMBER_OUT), # memory.in is added to goal to be available in goal/next_worldstate Precondition(Condition.get('memory.in'), NUMBER_IN) ]) plan = self.runner.update_and_plan(goal) print "plan:", plan self.assertIsNotNone(plan, "plan should not be None") success = self.runner.execute(plan) print "success: ", success self.assertTrue(success, "executing plan was not successful") # self.assertEqual(outcome, 'succeeded', "executing plan did not return succeeded") mem_out = self.runner.memory.get_value('memory.out') print "memory.out = %s" % mem_out self.assertEqual(mem_out, NUMBER_OUT, "out variable in memory is not changed successfully")
def __init__(self): Action.__init__(self, [Precondition(Condition.get('robot.bumpered'), True)], [Effect(Condition.get('robot.bumpered'), False)]) def run(self, next_worldstate): print '%s: resetting bumper..' % self.__class__ if __name__ == "__main__": runner = Runner() Condition.add(MemoryCondition(runner.memory, 'robot.bumpered', True)) runner.actions.add(SymmetricAction()) Condition.initialize_worldstate(runner.worldstate) print 'worldstate now is: ', runner.worldstate goal = Goal([Precondition(Condition.get('robot.bumpered'), False)]) start_node = runner.update_and_plan(goal, introspection=True) print 'start_node: ', start_node