class TestTransitionErrorStateMachine(unittest.TestCase):
def setUp(self):
self.sm = StateMachine("test_state_machine", ["exit", "__preempted__"])
self.state1 = TestStateLoop("test1", ["s2", "s3"])
self.state2 = TestStateLoop("test2", ["s1", "s1"])
self.state3 = TestStateLoop("test3", ["s1", "exit"])
self.sm.add_state(self.state1, {"s2": "test2", "s3": "test3"}, initial=True)
self.sm.add_state(self.state2, {"s1": "test1"})
self.sm.add_state(self.state3, {"s1": "test1"})
def test_no_states(self):
error = None
try:
StateMachine("test_state_machine", [])._run()
except TransitionError as e:
error = e
assert error is not None
def test_transition_doesnt_exist(self):
error = None
sm = StateMachine("test_state_machine", ["exit", "__preempted__"])
state1 = TestState("test1", ["s2"])
sm.add_state(self.state1, {}, initial=True)
try:
sm._run()
except TransitionError as e:
error = e
print(error)
assert error is not None
def test_transition_doesnt_exist(self):
error = None
sm = StateMachine("test_state_machine", ["exit", "__preempted__"])
state1 = TestState("test1", ["s2"])
sm.add_state(self.state1, {}, initial=True)
try:
sm._run()
except TransitionError as e:
error = e
print(error)
assert error is not None
def test_abort(self):
print("_____abort______")
sm = StateMachine("test_state_machine", ["exit", "__preempted__"])
state = AbortingState("test1", self.mock, ["exit"])
sm.add_state(state, {}, initial=True)
execution = Thread(target=sm._run)
execution.start()
execution.join()
self.mock.assert_any_call("on_begin")
self.mock.assert_any_call("on_execute")
self.mock.assert_called_with("on_abort")
assert self.mock.call_count == 3
def setUp(self):
self.base_sm = StateMachine("test_state_machine", ["exit", "__preempted__"])
self.state1 = TestState("test1", ["s2"])
self.state2 = TestState("test2", ["s3"])
self.state3 = TestState("test3", ["exit"])
self.base_sm.add_state(self.state1, {"s2": "test2"}, initial=True)
self.base_sm.add_state(self.state2, {"s3": "test3"})
self.base_sm.add_state(self.state3, {})
self.sms = []
self.sms.append(self.base_sm)
for i in range(100):
sm = StateMachine("test_state_machine" + str(i), ["exit"])
sm.add_state(self.sms[i], {}, initial=True)
self.sms.append(sm)
self.top_sm = self.sms[-1]
def initialize_state_machine():
# create machine object of class StateMachine and add states
machine = StateMachine(cubesat)
machine.cubesat = cubesat
# Initialize Transitions
low_power_transition = LowPowerTransition("lowpower", MIN_VOLTAGE)
high_power_transition = HighPowerTransition("idle", MIN_VOLTAGE)
# Add States
machine.add_state(IdleState([low_power_transition]))
machine.add_state(LowPowerState([high_power_transition]))
# start off the StateMachine object in idle
machine.go_to_state("idle")
return machine
class TestLoopStateMachine(unittest.TestCase):
def setUp(self):
self.sm = StateMachine("test_state_machine", ["exit", "__preempted__"])
self.state1 = TestStateLoop("test1", ["s2", "s3"])
self.state2 = TestStateLoop("test2", ["s1", "s1"])
self.state3 = TestStateLoop("test3", ["s1", "exit"])
self.sm.add_state(self.state1, {"s2": "test2", "s3": "test3"}, initial=True)
self.sm.add_state(self.state2, {"s1": "test1"})
self.sm.add_state(self.state3, {"s1": "test1"})
def test_basic(self):
start_time = time.time()
self.sm._run()
execution_time = time.time() - start_time
print("execution_time ", execution_time)
print(self.state1, "\n")
print(self.state2, "\n")
print(self.state3, "\n")
def parse_elements(rulename, elements):
machine = StateMachine(rulename)
machine_start = State(START_STATE)
machine_end = State(FINAL_STATE)
elements = elements_splitter.split_into_tokens(elements)
start_edges, end_states, states = recursive_parse_elements(elements)
for state in states:
machine.add_state(state)
for start_edge in start_edges:
machine_start.add_edge(start_edge)
for end_state in end_states:
end_state.add_edge(Edge('', FINAL_STATE))
machine.add_state(machine_start)
machine.add_state(machine_end)
return machine
class TestMonitoredStateMachine(unittest.TestCase):
def setUp(self):
self.mock = Mock()
self.iterations_num = 3
self.sm = StateMachine("test_state_machine", ["exit"])
self.state1 = TestMonitoredState(
"test1", outcomes=["s2"], event_cb=self.mock, execute_iterations=self.iterations_num
)
self.state2 = TestMonitoredState(
"test2", outcomes=["s3"], event_cb=self.mock, execute_iterations=self.iterations_num
)
self.state3 = TestMonitoredState(
"test3", outcomes=["exit"], event_cb=self.mock, execute_iterations=self.iterations_num
)
self.sm.add_state(self.state1, {"s2": "test2"}, initial=True)
self.sm.add_state(self.state2, {"s3": "test3"})
self.sm.add_state(self.state3, {})
self.mock.reset_mock()
def test_basic(self):
print("_____run______")
self.sm._run()
time.sleep(0.04)
assert self.mock.call_count == (self.iterations_num + 2) * 3
self.mock.assert_any_call("on_begin")
self.mock.assert_any_call("on_execute")
self.mock.assert_any_call("on_end")
def test_pause(self):
print("_____pause______")
execution = Thread(target=self.sm._run)
execution.start()
time.sleep(0.02)
PauseThread(self.sm, 0.03).start()
execution.join()
time.sleep(0.02)
assert self.mock.call_count == (self.iterations_num + 2) * 3 + 2
# check cb called with pause_in and pause_out once and only once
self.mock.assert_any_call("on_pause_in")
self.mock.assert_any_call("on_pause_out")
self.mock.assert_any_call("on_begin")
self.mock.assert_any_call("on_execute")
self.mock.assert_any_call("on_end")
def test_preempt(self):
print("_____preempt______")
execution = Thread(target=self.sm._run)
execution.start()
PreemptThread(self.sm).start()
execution.join()
time.sleep(0.04)
self.mock.assert_any_call("on_begin")
self.mock.assert_called_with("on_preempt")
def test_abort(self):
print("_____abort______")
sm = StateMachine("test_state_machine", ["exit", "__preempted__"])
state = AbortingState("test1", self.mock, ["exit"])
sm.add_state(state, {}, initial=True)
execution = Thread(target=sm._run)
execution.start()
execution.join()
self.mock.assert_any_call("on_begin")
self.mock.assert_any_call("on_execute")
self.mock.assert_called_with("on_abort")
assert self.mock.call_count == 3
class TestStringPattern(unittest.TestCase):
def __init__(self, *args, **kwargs):
super(TestStringPattern, self).__init__(*args, **kwargs)
self.m = StateMachine()
self.m.add_state("start_state", state_zero)
self.m.add_state("first_state", state_one)
self.m.add_state("second_state", state_two)
self.m.add_state("third_state", state_three)
self.m.add_state("last_state", None, end_state=1)
self.m.add_state("error_state", None, end_state=1)
self.m.set_start("start_state")
def test_right_pattern(self):
self.assertEqual(self.m.run("12@123#"),
"reached last_state which is an end state")
self.assertEqual(self.m.run("@3445#"),
"reached last_state which is an end state")
self.assertEqual(self.m.run("123@5363@968495#"),
"reached last_state which is an end state")
self.assertEqual(self.m.run("123@@968495#"),
"reached last_state which is an end state")
def test_wrong_pattern(self):
self.assertEqual(self.m.run("12@2345"),
"reached error_state which is an end state")
self.assertEqual(self.m.run("122345#"),
"reached error_state which is an end state")
self.assertEqual(self.m.run("12@23f45#"),
"reached error_state which is an end state")
return "green", next_input
if timer_input == 0:
return "red", next_input
def state_green(timer_input, next_input=randint(0, 1)):
if timer_input == 1:
return "yellow", next_input
if timer_input == 0:
return "green", next_input
def state_yellow(timer_input, next_input=randint(0, 1)):
if timer_input == 1:
return "red", next_input
if timer_input == 0:
return "yellow", next_input
if __name__ == "__main__":
m = StateMachine()
m.add_state("red", state_red)
m.add_state("green", state_green)
m.add_state("yellow", state_yellow)
m.add_state("last_state", None, end_state=1)
m.add_state("error_state", None, end_state=1)
m.set_start("red")
while True:
m.run(randint(0, 1))
def not_state_transitions(txt):
splitted_txt = txt.split(None, 1)
word, txt = splitted_txt if len(splitted_txt) > 1 else (txt, "")
if word in positive_adjectives:
new_state = "neg_state"
elif word in negative_adjectives:
new_state = "pos_state"
else:
new_state = "error_state"
return (new_state, txt)
def neg_state(txt):
print("Hallo")
return ("neg_state", "")
if __name__ == "__main__":
m = StateMachine()
m.add_state("Start", start_transitions)
m.add_state("Python_state", python_state_transition)
m.add_state("is_state", is_state_transitions)
m.add_state("not_state", not_state_transitions)
m.add_state("neg_state", None, end_state=1)
m.add_state("pos_state", None, end_state=1)
m.add_state("error_state", None, end_state=1)
m.set_start("Start")
m.run("Python is great")
m.run("Python is difficult")
m.run("Perl is ugly")
NAO_IP, # parent broker IP
port) # parent broker port
textToSpeech = ALProxy("ALTextToSpeech", NAO_IP, port)
textToSpeech.setLanguage(naoLanguage)
#textToSpeech.setVolume(1.0);
if(naoWriting):
nao.setpose("StandInit");
[temp,joints_standInit] = nao.execute([naoqi_request("motion","getAngles",["RArm",True])]);
nao.execute([naoqi_request("motion","wbEnableEffectorControl",[effector,True])])
#initialise word manager (passes feedback to shape learners and keeps history of words learnt)
wordManager = ShapeLearnerManager(generateSettings);
stateMachine = StateMachine();
stateMachine.add_state("STARTING_INTERACTION", startInteraction);
stateMachine.add_state("WAITING_FOR_ROBOT_TO_CONNECT", waitForRobotToConnect);
stateMachine.add_state("WAITING_FOR_WORD", waitForWord);
stateMachine.add_state("RESPONDING_TO_NEW_WORD", respondToNewWord);
stateMachine.add_state("PUBLISHING_WORD", publishWord);
stateMachine.add_state("PUBLISHING_LETTER", publishShape);
stateMachine.add_state("WAITING_FOR_LETTER_TO_FINISH", waitForShapeToFinish);
stateMachine.add_state("ASKING_FOR_FEEDBACK", askForFeedback);
stateMachine.add_state("WAITING_FOR_FEEDBACK", waitForFeedback);
stateMachine.add_state("RESPONDING_TO_FEEDBACK", respondToFeedback);
stateMachine.add_state("RESPONDING_TO_DEMONSTRATION", respondToDemonstration);
stateMachine.add_state("RESPONDING_TO_TEST_CARD", respondToTestCard);
#stateMachine.add_state("RESPONDING_TO_TABLET_DISCONNECT", respondToTabletDisconnect);
stateMachine.add_state("WAITING_FOR_TABLET_TO_CONNECT", waitForTabletToConnect);
stateMachine.add_state("STOPPING", stopInteraction);
stateMachine.add_state("EXIT", None, end_state=True);
class Testblock:
def __init__(self, testblock_name, metrics):
self.testblock_name = testblock_name
rospy.Subscriber("/atf/" + self.testblock_name + "/Trigger", Trigger, self.trigger_callback)
self.transition = None
self.metrics = metrics
self.m = StateMachine(self.testblock_name)
self.m.add_state(Status.PURGED, self.purged_state)
self.m.add_state(Status.ACTIVE, self.active_state)
self.m.add_state(Status.PAUSED, self.paused_state)
self.m.add_state(Status.FINISHED, self.finished_state, end_state=True)
self.m.add_state(Status.ERROR, self.error_state, end_state=True)
self.m.set_start(Status.PURGED)
self.m.run()
def trigger_callback(self, msg):
self.transition = msg.trigger
def purge(self):
for metric in self.metrics:
metric.purge()
def activate(self):
for metric in self.metrics:
metric.start()
def pause(self):
self.stop()
def finish(self):
self.stop()
def exit(self):
self.transition = Trigger.ERROR
def stop(self):
for metric in self.metrics:
metric.stop()
def get_state(self):
return self.m.get_current_state()
def purged_state(self):
while not rospy.is_shutdown() and self.transition is None:
continue
if self.transition == Trigger.PURGE:
# is already purged
new_state = Status.PURGED
elif self.transition == Trigger.ACTIVATE:
self.activate()
new_state = Status.ACTIVE
elif self.transition == Trigger.PAUSE:
new_state = Status.ERROR
elif self.transition == Trigger.FINISH:
new_state = Status.ERROR
elif self.transition == Trigger.ERROR:
new_state = Status.ERROR
else:
new_state = Status.ERROR
self.transition = None
return new_state
def active_state(self):
while not rospy.is_shutdown() and self.transition is None:
continue
if self.transition == Trigger.PURGE:
self.purge()
new_state = Status.PURGED
elif self.transition == Trigger.ACTIVATE:
# is already active
new_state = Status.ACTIVE
elif self.transition == Trigger.PAUSE:
self.pause()
new_state = Status.PAUSED
elif self.transition == Trigger.FINISH:
self.finish()
new_state = Status.FINISHED
elif self.transition == Trigger.ERROR:
new_state = Status.ERROR
else:
new_state = Status.ERROR
self.transition = None
return new_state
def paused_state(self):
while not rospy.is_shutdown() and self.transition is None:
continue
if self.transition == Trigger.PURGE:
self.purge()
new_state = Status.PURGED
elif self.transition == Trigger.ACTIVATE:
self.activate()
new_state = Status.ACTIVE
elif self.transition == Trigger.PAUSE:
# is already paused
new_state = Status.PAUSED
elif self.transition == Trigger.FINISH:
self.finish()
new_state = Status.FINISHED
elif self.transition == Trigger.ERROR:
new_state = Status.ERROR
else:
new_state = Status.ERROR
self.transition = None
return new_state
def finished_state(self):
pass
def error_state(self):
pass
else:
nextState = requestedState
requestedState = None
infoForNextState = {'state_cameFrom': "IDLE"}
return nextState, infoForNextState
if __name__ == "__main__":
rospy.init_node("activity_nao")
# add all state to state machine
stateMachine = StateMachine()
stateMachine.add_state("STARTING_INTERACTION", startInteraction)
stateMachine.add_state("WAITING_FOR_PATH", waitForPath)
stateMachine.add_state("RESPONDING_TO_NEW_PATH", respondToNewPath)
stateMachine.add_state("WAITING_FOR_PATH_TO_FINISH", waitForPathToFinish)
#stateMachine.add_state("ASKING_PLAY_GAME", askToPlayGame)
stateMachine.add_state("WAITING_FOR_GAME_TO_FINISH", waitForGameToFinish)
stateMachine.add_state("RESPONDING_TO_CHILD_SCORE", respondToChildScore)
stateMachine.add_state("IDLE", onIdle)
stateMachine.add_state("EXIT", None, end_state=True)
stateMachine.add_state("FAIL", onRobotFailed)
stateMachine.set_start("STARTING_INTERACTION")
infoForStartState = {'state_cameFrom': None}
#listen for path to draw
path_subscriber = rospy.Subscriber(PATH_TOPIC, Path, onPathReceived)
#listen for child score
if char == "@":
return "first_state", text
if char == "#":
return "third_state", text
return "start_state", text
def state_three(text: str):
try:
char, text = text[0], text[1:]
except IndexError:
return "last_state", text
if char == "@":
return "first_state", text
return "start_state", text
if __name__ == "__main__":
m = StateMachine()
m.add_state("start_state", state_zero)
m.add_state("first_state", state_one)
m.add_state("second_state", state_two)
m.add_state("third_state", state_three)
m.add_state("last_state", None, end_state=1)
m.add_state("error_state", None, end_state=1)
m.set_start("start_state")
m.run("@234#")
m.run("1@638#")
m.run("@139#1")
def not_state_transitions(txt):
splitted_txt = txt.split(None, 1)
word, txt = splitted_txt if len(splitted_txt) > 1 else (txt, "")
if word in positive_adjectives:
newState = "neg_state"
elif word in negative_adjectives:
newState = "pos_state"
else:
newState = "error_state"
return (newState, txt)
if __name__ == "__main__":
m = StateMachine()
m.add_state("Start", start_transitions) # 添加初始状态
m.add_state("Python_state", python_state_transitions)
m.add_state("is_state", is_state_transitions)
m.add_state("not_state", not_state_transitions)
m.add_state("neg_state", None, end_state=1) # 添加最终状态
m.add_state("pos_state", None, end_state=1)
m.add_state("error_state", None, end_state=1)
m.set_start("Start") # 设置开始状态
m.run("Python is great")
m.run("Python is not fun")
m.run("Perl is ugly")
m.run("Python is")
m.run("Pythoniseasy")
class TestBasicStateMachine(unittest.TestCase):
def setUp(self):
self.iterations_num = 3
self.sm = StateMachine("test_state_machine", ["exit", "__preempted__"])
self.state1 = TestState("test1", ["s2", "s3"], execute_iterations=self.iterations_num)
self.state2 = TestState("test2", ["s3", "s1"], execute_iterations=self.iterations_num)
self.state3 = TestState("test3", ["exit", "s1"], execute_iterations=self.iterations_num)
self.sm.add_state(self.state1, {"s2": "test2", "s3": "test3"}, initial=True)
self.sm.add_state(self.state2, {"s1": "test1", "s3": "test3"})
self.sm.add_state(self.state3, {"s2": "test2", "s1": "test1"})
def test_basic(self):
start_time = time.time()
self.sm._run()
execution_time = time.time() - start_time
print("execution_time ", execution_time)
self.state1.mock.begin.assert_called()
assert self.state1.mock.begin.call_count == 1
self.state1.mock.execute.assert_called()
assert self.state1.mock.execute.call_count == self.iterations_num
self.state1.mock.end.assert_called()
assert self.state1.mock.end.call_count == 1
self.state2.mock.begin.assert_called()
assert self.state2.mock.begin.call_count == 1
self.state2.mock.execute.assert_called()
assert self.state2.mock.execute.call_count == self.iterations_num
self.state2.mock.end.assert_called()
assert self.state2.mock.end.call_count == 1
self.state3.mock.begin.assert_called()
assert self.state3.mock.begin.call_count == 1
self.state3.mock.execute.assert_called()
assert self.state3.mock.execute.call_count == self.iterations_num
self.state3.mock.end.assert_called()
assert self.state3.mock.end.call_count == 1
def test_pause(self):
execution = Thread(target=self.sm._run)
execution.start()
time.sleep(0.02)
PauseThread(self.sm, 0.03).start()
execution.join()
self.state1.mock.begin.assert_called()
assert self.state1.mock.begin.call_count == 1
self.state1.mock.pause_in.assert_called()
assert self.state1.mock.pause_in.call_count == 1
self.state1.mock.pause_out.assert_called()
assert self.state1.mock.pause_out.call_count == 1
self.state1.mock.execute.assert_called()
assert self.state1.mock.execute.call_count == self.iterations_num
self.state1.mock.end.assert_called()
assert self.state1.mock.end.call_count == 1
self.state2.mock.begin.assert_called()
assert self.state2.mock.begin.call_count == 1
self.state2.mock.execute.assert_called()
assert self.state2.mock.execute.call_count == self.iterations_num
self.state2.mock.end.assert_called()
assert self.state2.mock.end.call_count == 1
self.state3.mock.begin.assert_called()
assert self.state3.mock.begin.call_count == 1
self.state3.mock.execute.assert_called()
assert self.state3.mock.execute.call_count == self.iterations_num
self.state3.mock.end.assert_called()
assert self.state3.mock.end.call_count == 1
def test_pause_twice(self):
execution = Thread(target=self.sm._run)
execution.start()
time.sleep(0.03)
PauseThread(self.sm, 0.03).start()
time.sleep(0.03)
PauseThread(self.sm, 0.03).start()
execution.join()
self.state1.mock.begin.assert_called()
assert self.state1.mock.begin.call_count == 1
self.state1.mock.pause_in.assert_called()
assert self.state1.mock.pause_in.call_count == 2
self.state1.mock.pause_out.assert_called()
assert self.state1.mock.pause_out.call_count == 2
self.state1.mock.execute.assert_called()
assert self.state1.mock.execute.call_count == self.iterations_num
self.state1.mock.end.assert_called()
assert self.state1.mock.end.call_count == 1
self.state2.mock.begin.assert_called()
assert self.state2.mock.begin.call_count == 1
self.state2.mock.execute.assert_called()
assert self.state2.mock.execute.call_count == self.iterations_num
self.state2.mock.end.assert_called()
assert self.state2.mock.end.call_count == 1
self.state3.mock.begin.assert_called()
assert self.state3.mock.begin.call_count == 1
self.state3.mock.execute.assert_called()
assert self.state3.mock.execute.call_count == self.iterations_num
self.state3.mock.end.assert_called()
assert self.state3.mock.end.call_count == 1
def test_preempt(self):
execution = Thread(target=self.sm._run, args=())
execution.start()
time.sleep(0.03)
PreemptThread(self.sm).start()
execution.join()
self.state1.mock.begin.assert_called()
assert self.state1.mock.begin.call_count == 1
self.state1.mock.execute.assert_called()
assert self.state1.mock.execute.call_count <= self.iterations_num
self.state1.mock.end.assert_not_called()
self.state2.mock.begin.assert_not_called()
self.state2.mock.execute.assert_not_called()
self.state2.mock.end.assert_not_called()
self.state3.mock.begin.assert_not_called()
self.state3.mock.execute.assert_not_called()
self.state3.mock.end.assert_not_called()
def test_restart_after_preempt(self):
execution = Thread(target=self.sm._run, args=())
execution.start()
time.sleep(0.04)
PreemptThread(self.sm).start()
execution.join()
time.sleep(0.01)
execution2 = Thread(target=self.sm._run, args=())
execution2.start()
execution2.join()
self.state1.mock.begin.assert_called()
assert self.state1.mock.begin.call_count == 2
self.state1.mock.execute.assert_called()
assert self.state1.mock.execute.call_count >= self.iterations_num
self.state1.mock.end.assert_called()
assert self.state1.mock.end.call_count == 1
self.state2.mock.begin.assert_called()
assert self.state2.mock.begin.call_count == 1
self.state2.mock.execute.assert_called()
assert self.state2.mock.execute.call_count == self.iterations_num
self.state2.mock.end.assert_called()
assert self.state2.mock.end.call_count == 1
self.state3.mock.begin.assert_called()
assert self.state3.mock.begin.call_count == 1
self.state3.mock.execute.assert_called()
assert self.state3.mock.execute.call_count == self.iterations_num
self.state3.mock.end.assert_called()
assert self.state3.mock.end.call_count == 1
def test_preempt_while_paused(self):
print("test_preempt_while_paused")
execution = Thread(target=self.sm._run)
execution.start()
time.sleep(0.02)
PauseThread(self.sm, 0.03).start()
PreemptThread(self.sm).start()
time.sleep(0.03)
Thread(target=self.sm._pause_out, args=()).start()
execution.join()
assert self.state1.mock.pause_in.call_count == 1
self.state1.mock.end.assert_not_called()
self.state2.mock.begin.assert_not_called()
self.state2.mock.execute.assert_not_called()
self.state2.mock.end.assert_not_called()
self.state3.mock.begin.assert_not_called()
self.state3.mock.execute.assert_not_called()
self.state3.mock.end.assert_not_called()
class Testblock:
def __init__(self, testblock_name, metrics):
self.testblock_name = testblock_name
rospy.Subscriber("/atf/" + self.testblock_name + "/Trigger", Trigger,
self.trigger_callback)
self.transition = None
self.metrics = metrics
self.m = StateMachine(self.testblock_name)
self.m.add_state(Status.PURGED, self.purged_state)
self.m.add_state(Status.ACTIVE, self.active_state)
self.m.add_state(Status.PAUSED, self.paused_state)
self.m.add_state(Status.FINISHED, self.finished_state, end_state=True)
self.m.add_state(Status.ERROR, self.error_state, end_state=True)
self.m.set_start(Status.PURGED)
self.m.run()
def trigger_callback(self, msg):
self.transition = msg.trigger
def purge(self):
for metric in self.metrics:
metric.purge()
def activate(self):
for metric in self.metrics:
metric.start()
def pause(self):
self.stop()
def finish(self):
self.stop()
def exit(self):
self.transition = Trigger.ERROR
def stop(self):
for metric in self.metrics:
metric.stop()
def get_state(self):
return self.m.get_current_state()
def purged_state(self):
while not rospy.is_shutdown() and self.transition is None:
continue
if self.transition == Trigger.PURGE:
# is already purged
new_state = Status.PURGED
elif self.transition == Trigger.ACTIVATE:
self.activate()
new_state = Status.ACTIVE
elif self.transition == Trigger.PAUSE:
new_state = Status.ERROR
elif self.transition == Trigger.FINISH:
new_state = Status.ERROR
elif self.transition == Trigger.ERROR:
new_state = Status.ERROR
else:
new_state = Status.ERROR
self.transition = None
return new_state
def active_state(self):
while not rospy.is_shutdown() and self.transition is None:
continue
if self.transition == Trigger.PURGE:
self.purge()
new_state = Status.PURGED
elif self.transition == Trigger.ACTIVATE:
# is already active
new_state = Status.ACTIVE
elif self.transition == Trigger.PAUSE:
self.pause()
new_state = Status.PAUSED
elif self.transition == Trigger.FINISH:
self.finish()
new_state = Status.FINISHED
elif self.transition == Trigger.ERROR:
new_state = Status.ERROR
else:
new_state = Status.ERROR
self.transition = None
return new_state
def paused_state(self):
while not rospy.is_shutdown() and self.transition is None:
continue
if self.transition == Trigger.PURGE:
self.purge()
new_state = Status.PURGED
elif self.transition == Trigger.ACTIVATE:
self.activate()
new_state = Status.ACTIVE
elif self.transition == Trigger.PAUSE:
# is already paused
new_state = Status.PAUSED
elif self.transition == Trigger.FINISH:
self.finish()
new_state = Status.FINISHED
elif self.transition == Trigger.ERROR:
new_state = Status.ERROR
else:
new_state = Status.ERROR
self.transition = None
return new_state
def finished_state(self):
pass
def error_state(self):
pass
class TestVerticalStackStateMachines(unittest.TestCase): # TODO : add asserts
def setUp(self):
self.base_sm = StateMachine("test_state_machine", ["exit", "__preempted__"])
self.state1 = TestState("test1", ["s2"])
self.state2 = TestState("test2", ["s3"])
self.state3 = TestState("test3", ["exit"])
self.base_sm.add_state(self.state1, {"s2": "test2"}, initial=True)
self.base_sm.add_state(self.state2, {"s3": "test3"})
self.base_sm.add_state(self.state3, {})
self.sms = []
self.sms.append(self.base_sm)
for i in range(100):
sm = StateMachine("test_state_machine" + str(i), ["exit"])
sm.add_state(self.sms[i], {}, initial=True)
self.sms.append(sm)
self.top_sm = self.sms[-1]
def test_basic(self):
start_time = time.time()
print(self.top_sm._run())
execution_time = time.time() - start_time
print("execution_time ", execution_time)
print(self.state1, "\n")
print(self.state2, "\n")
print(self.state3, "\n")
def test_pause(self):
execution = Thread(target=self.top_sm._run)
execution.start()
time.sleep(0.02)
PauseThread(self.top_sm, 0.03).start()
execution.join()
print(self.state1, "\n")
print(self.state2, "\n")
print(self.state3, "\n")
def test_preempt(self):
execution = Thread(target=self.top_sm._run, args=())
execution.start()
time.sleep(0.03)
start_preempt_time = time.time()
PreemptThread(self.top_sm).start()
execution.join()
preempt_time = time.time() - start_preempt_time
print("preempt_time ", preempt_time)
print(self.state1, "\n")
print(self.state2, "\n")
print(self.state3, "\n")
def test_restart_after_preempt(self):
execution = Thread(target=self.top_sm._run, args=())
execution.start()
time.sleep(0.1)
PreemptThread(self.top_sm).start()
execution.join()
time.sleep(0.1)
execution2 = Thread(target=self.top_sm._run, args=())
execution2.start()
execution2.join()
print(self.state1, "\n")
print(self.state2, "\n")
print(self.state3, "\n")
currentWord = []
settings_shapeLearners = []
if __name__ == "__main__":
datasetDirectory = rospy.get_param('~dataset_directory', 'default')
if (datasetDirectory.lower() == 'default'): #use default
import inspect
fileName = inspect.getsourcefile(ShapeModeler)
installDirectory = fileName.split('/lib')[0]
datasetDirectory = installDirectory + '/share/allograph/letter_model_datasets/alexis_set_for_children'
robotDirectory = installDirectory + '/share/allograph/robot_tries/start'
stateMachine = StateMachine()
stateMachine.add_state("STARTING_INTERACTION", startInteraction)
stateMachine.add_state("WAITING_FOR_ROBOT_TO_CONNECT",
waitForRobotToConnect)
stateMachine.add_state("WAITING_FOR_WORD", waitForWord)
stateMachine.add_state("RESPONDING_TO_NEW_WORD", respondToNewWord)
stateMachine.add_state("PUBLISHING_WORD", publishWord)
stateMachine.add_state("WAITING_FOR_LETTER_TO_FINISH",
waitForShapeToFinish)
stateMachine.add_state("ASKING_FOR_FEEDBACK", askForFeedback)
stateMachine.add_state("WAITING_FOR_FEEDBACK", waitForFeedback)
stateMachine.add_state("RESPONDING_TO_DEMONSTRATION_FULL_WORD",
respondToDemonstrationWithFullWord)
stateMachine.add_state("EXIT", None, end_state=True)
stateMachine.set_start("WAITING_FOR_ROBOT_TO_CONNECT")
infoForStartState = {
'state_goTo': ["STARTING_INTERACTION"],
if __name__ == '__main__':
rospy.init_node('state_machine', log_level=rospy.INFO)
sm = StateMachine('TOP_LEVEL_SM')
setup_state = SetupState()
nav_state = NavState()
hover_state = HoverState(newPoseStamped(2, 2, 0.4, 0, 0, 0, 'map'))
setup_state.transitions['success'] = 'NAV_STATE'
nav_state.transitions['success'] = 'HOVER_STATE'
sm.add_state(setup_state)
sm.add_state(nav_state)
sm.add_state(hover_state)
rospy.loginfo(rospy.get_name() + ': Starting statemachine...')
try:
sm.setup()
sm.execute()
except ROSInterruptException:
pass
rospy.spin()
