def __init__(self):
ROSTopicAction.__init__(
self,
'/bumper_reset',
Empty, [Precondition(Condition.get('robot.bumpered'), True)],
[Effect(Condition.get('robot.bumpered'), False)],
msg_args=[])
def __init__(self):
self._condition = Condition.get('robot.pose')
self._check_path_vareffect = MoveBaseAction.CheckForPathVarEffect(
self._condition)
SMACHStateWrapperAction.__init__(self, MoveBaseState(), [
Precondition(Condition.get('robot.bumpered'), False),
Precondition(Condition.get('robot.arm_pose_floor'), True)
], [self._check_path_vareffect])
def __init__(self):
self._condition = Condition.get('robot.pose')
self._check_path_vareffect = MoveBaseAction.CheckForPathVarEffect(self._condition)
SMACHStateWrapperAction.__init__(self, MoveBaseState(),
[Precondition(Condition.get('robot.bumpered'), False),
Precondition(Condition.get('robot.arm_pose_floor'), True)
],
[self._check_path_vareffect])
def __init__(self):
SMACHStateWrapperAction.__init__(
self, get_move_arm_to_joints_positions_state(ARM_POSE_FOLDED),
[Precondition(Condition.get('arm_can_move'), True),
# TODO: maybe remove necessary anti-effect-preconditions
# the currently available alternative would be to use a
# variable effect that can reach any value
Precondition(Condition.get('robot.arm_folded'), False)],
[Effect(Condition.get('robot.arm_folded'), True)])
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 __init__(self, state_name, topic, topic_class, field=None, msgeval=None):
Condition.__init__(self, state_name)
self._topic = topic
self._field = field
self._subscriber = rospy.Subscriber(topic, topic_class, self._callback)
if msgeval is None:
assert field is not None
msgeval = rostopic.msgevalgen(field)
self._msgeval = msgeval
self._value = None
def __init__(self):
SMACHStateWrapperAction.__init__(
self,
get_move_arm_to_joints_positions_state(ARM_POSE_FOLDED),
[
Precondition(Condition.get('arm_can_move'), True),
# TODO: maybe remove necessary anti-effect-preconditions
# the currently available alternative would be to use a
# variable effect that can reach any value
Precondition(Condition.get('robot.arm_folded'), False)
],
[Effect(Condition.get('robot.arm_folded'), True)])
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 translate_worldstate_to_userdata(self, next_worldstate, userdata):
goal_pose = next_worldstate.get_condition_value(Condition.get('robot.pose'))
(_roll, _pitch, yaw) = tf.transformations.euler_from_quaternion(
pose_orientation_to_quaternion(goal_pose.orientation))
userdata.x = goal_pose.position.x
userdata.y = goal_pose.position.y
userdata.yaw = yaw
def _generate_variable_preconditions(self, var_effects, worldstate,
start_worldstate):
effect = var_effects.pop() # this action has one variable effect
assert effect._condition is self._condition
precond_value = start_worldstate.get_condition_value(
Condition.get('robot.pose'))
return [Precondition(effect._condition, precond_value, None)]
def translate_worldstate_to_userdata(self, next_worldstate, userdata):
goal_pose = next_worldstate.get_condition_value(
Condition.get('robot.pose'))
(_roll, _pitch, yaw) = tf.transformations.euler_from_quaternion(
pose_orientation_to_quaternion(goal_pose.orientation))
userdata.x = goal_pose.position.x
userdata.y = goal_pose.position.y
userdata.yaw = yaw
def __init__(self, pose, frame, usability):
if frame != '/map' and frame != 'map':
if MoveToPoseGoal.tl is None:
MoveToPoseGoal.tl = tf.TransformListener()
rospy.sleep(1)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = frame
pose_stamped.pose = pose
#tf.Transformer().waitForTransform('/map', frame)
pose_stmpd_in_map = MoveToPoseGoal.tl.transformPose('/map', pose_stamped)
pose = pose_stmpd_in_map.pose
Goal.__init__(self, [Precondition(Condition.get('robot.pose'), pose)], usability)
def __init__(self, pose, frame, usability):
if frame != '/map' and frame != 'map':
if MoveToPoseGoal.tl is None:
MoveToPoseGoal.tl = tf.TransformListener()
rospy.sleep(1)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = frame
pose_stamped.pose = pose
#tf.Transformer().waitForTransform('/map', frame)
pose_stmpd_in_map = MoveToPoseGoal.tl.transformPose(
'/map', pose_stamped)
pose = pose_stmpd_in_map.pose
Goal.__init__(self, [Precondition(Condition.get('robot.pose'), pose)],
usability)
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 __init__(self):
Action.__init__(self,
[Precondition(Condition.get('robot.bumpered'), True)],
[Effect(Condition.get('robot.bumpered'), False)])
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):
ROSTopicAction.__init__(
self, '/bumper_reset', Empty,
[Precondition(Condition.get('robot.bumpered'), True)],
[Effect(Condition.get('robot.bumpered'), False)],
msg_args=[])
def __init__(self):
SMACHStateWrapperAction.__init__(
self, get_lookaround_smach_mock(),
[Precondition(Condition.get('arm_can_move'), True)],
[VariableEffect(Condition.get('awareness'))])
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 __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 _generate_variable_preconditions(self, var_effects, worldstate, start_worldstate):
effect = var_effects.pop() # this action has one variable effect
assert effect._condition is self._condition
precond_value = start_worldstate.get_condition_value(Condition.get('robot.pose'))
return [Precondition(effect._condition, precond_value, None)]
def __init__(self):
Goal.__init__(self, [Precondition(Condition.get('awareness'), 1)], 0.6)
def __init__(self):
Action.__init__(self,
[Precondition(Condition.get('robot.bumpered'), True)],
[Effect(Condition.get('robot.bumpered'), False)])
def __init__(self):
Goal.__init__(self, [Precondition(Condition.get('awareness'), 1)], 0.6)
def __init__(self):
SMACHStateWrapperAction.__init__(
self, get_lookaround_smach(glimpse=True),
[Precondition(Condition.get('arm_can_move'), True)],
[VariableEffect(Condition.get('awareness'))])
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,
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