def plan(self, goal, introspection=False):
"""plan for given goal and return start_node of plan or None
introspection: introspect RGOAP planning via smach.introspection
"""
if introspection:
self._setup_introspection()
start_node = Runner.plan(self, goal, introspection)
if introspection:
if start_node is not None:
self._introspector.publish(start_node)
self._introspector.publish_net(self.planner.last_goal_node, start_node)
return start_node
def plan(self, goal, introspection=False):
"""plan for given goal and return start_node of plan or None
introspection: introspect RGOAP planning via smach.introspection
"""
if introspection:
self._setup_introspection()
start_node = Runner.plan(self, goal, introspection)
if introspection:
if start_node is not None:
self._introspector.publish(start_node)
self._introspector.publish_net(self.planner.last_goal_node,
start_node)
return start_node
def test_runner():
rospy.init_node('runner_test')
runner = Runner()
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded')
wfg = WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state
with sq:
Sequence.add('SLEEP', SleepState(5))
Sequence.add('WAIT_FOR_GOAL', wfg,
transitions={'aborted':'SLEEP'})
Sequence.add('MOVE_BASE_RGOAP', MoveBaseRGOAPState(runner),
transitions={'succeeded':'SLEEP'})
execute_smach_container(sq, enable_introspection=True)
def plan_and_execute_goals(self, goals):
"""Sort goals by usability and try to plan and execute one by one until
one goal is achieved"""
self._setup_introspection()
return Runner.plan_and_execute_goals(self, goals)
def service_preempt(self):
Runner.service_preempt(self)
if self._current_smach is not None:
self._current_smach.service_preempt()
def preempt_requested(self):
return Runner.preempt_requested(self) or (self._current_smach.preempt_requested()
if self._current_smach is not None
else False)
from rgoap import Runner
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:
def setUp(self):
Condition._conditions_dict.clear(
) # start every test without previous conditions
self.runner = Runner()
def request_preempt(self):
Runner.request_preempt(self)
if self._current_smach is not None:
self._current_smach.request_preempt()
def __init__(self, *args, **kwargs):
Runner.__init__(self, *args, **kwargs)
self._introspector = None
self._current_smach = None # used to propagate preemption into generated smach
def plan_and_execute_goals(self, goals):
"""Sort goals by usability and try to plan and execute one by one until
one goal is achieved"""
self._setup_introspection()
return Runner.plan_and_execute_goals(self, goals)
def service_preempt(self):
Runner.service_preempt(self)
if self._current_smach is not None:
self._current_smach.service_preempt()
def preempt_requested(self):
return Runner.preempt_requested(self) or (
self._current_smach.preempt_requested()
if self._current_smach is not None else False)
def __init__(self, *args, **kwargs):
Runner.__init__(self, *args, **kwargs)
self._introspector = None
self._current_smach = None # used to propagate preemption into generated smach
class Test(unittest.TestCase):
@classmethod
def setUpClass(cls):
# rospy.init_node('smach_rgoap_test')
pass
def setUp(self):
Condition._conditions_dict.clear(
) # start every test without previous conditions
self.runner = Runner()
def tearDown(self):
pass
@classmethod
def tearDownClass(cls):
pass
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 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 request_preempt(self):
Runner.request_preempt(self)
if self._current_smach is not None:
self._current_smach.request_preempt()
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