def test_history(self):
sm = self.make_sm()
history = ContainerHistory(sm)
parent_ud = UserData()
parent_ud.key = 0
outcome = sm.execute(parent_ud)
self.assertEqual(outcome, 'ok')
self.assertEqual(parent_ud.key, 5)
labels = [
'.'.join(s.label for s in c.save_states)
for c in history._checkpoints
]
self.assertEqual(labels, ['A', 'A.A', 'A.B', 'B', 'B.A', 'B.B', 'C'])
uds = [
tuple(s.userdata.key for s in c.save_states)
for c in history._checkpoints
]
self.assertEqual(uds, [
(0, ),
(0, 0),
(0, 1),
(2, ),
(2, 2),
(2, 3),
(4, ),
])
def userdata_from_set(self, data, state=None, grammar=grammar):
""" UserData random generation.
Keywords arguments:
data -- A set of inputs_key that need the userdata.
state -- The name of state to be fuzzed. Used for get information from configuration.
grammar -- A specific grammar if the default is not enogh.
"""
userdata = UserData()
log = {}
for k in data:
value = "<value>"
if state != None and self.config != None and state in self.config:
if k in self.config[state]["data"]["keys"]:
index = self.config[state]["data"]["keys"].index(k)
value = self.config[state]["data"]["types"][index]
if "grammar" in self.config[state]["data"]:
grammar = self.config[state]["data"]["grammar"]
if "list" in k:
value = "<array>"
elif "text" in k:
value = "<string>"
userdata[k] = self.specific_grammar_fuzzer(grammar, value)
log[k] = userdata[k]
if state != None and self.config != None and state in self.config and "keys" in self.config[
state]["params"]:
for k in self.config[state]["params"]["keys"]:
if "grammar" in self.config[state]["params"]:
grammar = self.config[state]["params"]["grammar"]
index = self.config[state]["params"]["keys"].index(k)
self.fuzz_param(
k,
self.specific_grammar_fuzzer(
grammar, self.config[state]["params"]["types"][index]))
return userdata, log
def test_resume_from_history(self):
sm = self.make_sm()
history = ContainerHistory(sm)
parent_ud = UserData()
parent_ud.key = 0
outcome = sm.execute(parent_ud)
self.assertEqual(outcome, 'ok')
self.assertEqual(parent_ud.key, 5)
checkpoints = list(history.checkpoints)
for checkpoint in checkpoints:
parent_ud.key = 0
outcome = sm.execute_from(checkpoint, parent_ud)
self.assertEqual(outcome, 'ok')
self.assertEqual(parent_ud.key, 5)
def test_resume_to_end(self):
"""
Can resume to final state
"""
sm = self.simple_sm()
outcome = sm.execute_from(
Checkpoint(uuid4(), datetime.now(),
[ContainerSaveState(label='C', userdata=UserData())]))
self.assertEqual(outcome, 'ok')
self.assertEqual(self.trace(), ['C'])
def test_nested_sm(self):
"""
Can resume to nested state-machines
"""
sm = self.nested_sm()
outcome = sm.execute_from(
Checkpoint(uuid4(), datetime.now(), [
ContainerSaveState(label='A', userdata=UserData()),
ContainerSaveState(label='B', userdata=UserData()),
]))
self.assertEqual(outcome, 'ok')
self.assertEqual(self.trace(), ['A.B', 'B.A', 'B.B', 'C'])
# Cannot resume to non-resumable sm
self.assertRaises(
ValueError, lambda: sm.execute_from(
Checkpoint(uuid4(), datetime.now(), [
ContainerSaveState(label='B', userdata=UserData()),
ContainerSaveState(label='B', userdata=UserData()),
])))
# Cannot resume to nonexistant state
self.assertRaises(
ValueError, lambda: sm.execute_from(
Checkpoint(uuid4(), datetime.now(), [
ContainerSaveState(label='A', userdata=UserData()),
ContainerSaveState(label='X', userdata=UserData()),
])))
# Cannot resume with spec nested deeper than state-machine is
self.assertRaises(
ValueError, lambda: sm.execute_from(
Checkpoint(uuid4(), datetime.now(), [
ContainerSaveState(label='A', userdata=UserData()),
ContainerSaveState(label='B', userdata=UserData()),
ContainerSaveState(label='C', userdata=UserData()),
])))
#!/usr/bin/env python
import rospy
from smach.user_data import UserData
from thorp_toolkit.geometry import TF2, create_2d_pose
from thorp_smach.states.navigation import LookToPose
from thorp_smach.utils import wait_for_mbf
if __name__ == '__main__':
rospy.init_node('run_look_to_pose_state')
# MBF is the last component to start, so wait for it before running the sm
wait_for_mbf()
ud = UserData()
ud['robot_pose'] = TF2().transform_pose(None, 'base_footprint', 'map')
ud['target_pose'] = create_2d_pose(0.0, 0.0, 0.0, 'map')
state = LookToPose()
t0 = rospy.get_time()
outcome = state.execute(ud)
rospy.loginfo("LookToPose completed in %.2fs with outcome '%s'",
rospy.get_time() - t0, outcome)
# Wait for ctrl-c to stop the application
rospy.spin()
rospy.signal_shutdown('All done.')
def make_ud(**kwargs):
ud = UserData()
ud._data.update(kwargs)
return ud
def data_to_userdata(data):
""" Create a UserData from dictionary. """
userdata = UserData()
for k in data.keys():
userdata[k] = data[k]
return userdata