From f687492219122eda8641b54ec2ee21eeec67f579 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 27 Dec 2022 18:50:32 +0900 Subject: [PATCH 1/2] set_initial_state: set current_state to initial state --- smach/src/smach/state_machine.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/smach/src/smach/state_machine.py b/smach/src/smach/state_machine.py index 4989614..98055a4 100644 --- a/smach/src/smach/state_machine.py +++ b/smach/src/smach/state_machine.py @@ -1,6 +1,7 @@ import threading import traceback +import time from contextlib import contextmanager import smach @@ -41,6 +42,7 @@ def __init__(self, outcomes, input_keys=[], output_keys=[]): # Properties self._state_transitioning_lock = threading.Lock() + self._state_executing_lock = threading.Lock() # Current state of the state machine self._is_running = False # True when a goal has been dispatched to and accepted by the state machine @@ -356,7 +358,10 @@ def execute(self, parent_ud = smach.UserData()): # Step through state machine while container_outcome is None and self._is_running and not smach.is_shutdown(): # Update the state machine - container_outcome = self._update_once() + with self._state_executing_lock: + container_outcome = self._update_once() + # let's set_initial_state to update status + time.sleep(0.01) # Copy output keys self._copy_output_keys(self.userdata, parent_ud) @@ -421,6 +426,12 @@ def set_initial_state(self, initial_states, userdata=smach.UserData()): # Set local userdata self.userdata.update(userdata) + with self._state_executing_lock: + # Set initial state + self._set_current_state(self._initial_state_label) + # Call start callbacks + self.call_start_cbs() + def get_active_states(self): return [str(self._current_label)] From 01b0f1dd7840a913033bcf2483c1b26b940da002 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 29 Dec 2022 03:15:55 +0900 Subject: [PATCH 2/2] set initial state only when it continas in slef._states, --- smach/src/smach/state_machine.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/smach/src/smach/state_machine.py b/smach/src/smach/state_machine.py index 98055a4..250d698 100644 --- a/smach/src/smach/state_machine.py +++ b/smach/src/smach/state_machine.py @@ -428,9 +428,13 @@ def set_initial_state(self, initial_states, userdata=smach.UserData()): with self._state_executing_lock: # Set initial state - self._set_current_state(self._initial_state_label) - # Call start callbacks - self.call_start_cbs() + if self._initial_state_label not in self._states: + smach.logwarn("Try to set inital state '%s', but it does not exist. Available states are: %s" % + (self._initial_state_label, list(self._states.keys()))) + else: + self._set_current_state(self._initial_state_label) + # Call start callbacks + self.call_start_cbs() def get_active_states(self): return [str(self._current_label)]