more logs for the state machine execute

uleroboticsgroup · Oct 30, 2024 · cb9ebc7 · cb9ebc7
1 parent bd4c3a0
commit cb9ebc7
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 12 deletions.
diff --git a/yasmin/src/yasmin/state_machine.cpp b/yasmin/src/yasmin/state_machine.cpp
@@ -258,6 +258,8 @@ StateMachine::execute(std::shared_ptr<blackboard::Blackboard> blackboard) {
 
   this->validate();
 
+  YASMIN_LOG_INFO("Executing state machine with initial state '%s'",
+                  this->start_state.c_str());
   this->call_start_cbs(blackboard, this->get_start_state());
 
   this->current_state_mutex->lock();
@@ -289,10 +291,6 @@ StateMachine::execute(std::shared_ptr<blackboard::Blackboard> blackboard) {
 
     // translate outcome using transitions
     if (transitions.find(outcome) != transitions.end()) {
-
-      YASMIN_LOG_INFO("%s: %s --> %s", this->current_state.c_str(),
-                      outcome.c_str(), transitions.at(outcome).c_str());
-
       outcome = transitions.at(outcome);
     }
 
@@ -304,20 +302,23 @@ StateMachine::execute(std::shared_ptr<blackboard::Blackboard> blackboard) {
       this->current_state.clear();
       this->current_state_mutex->unlock();
 
+      YASMIN_LOG_INFO("State machine ends with outcome '%s'", outcome.c_str());
       this->call_end_cbs(blackboard, outcome);
 
       return outcome;
 
       // outcome is a state
     } else if (this->states.find(outcome) != this->states.end()) {
 
+      YASMIN_LOG_INFO("%s (%s) --> %s", this->current_state.c_str(),
+                      old_outcome.c_str(), outcome.c_str());
+      this->call_transition_cbs(blackboard, this->get_start_state(), outcome,
+                                old_outcome);
+
       this->current_state_mutex->lock();
       this->current_state = outcome;
       this->current_state_mutex->unlock();
 
-      this->call_transition_cbs(blackboard, this->get_start_state(), outcome,
-                                old_outcome);
-
       // outcome is not in the sm
     } else {
       throw std::logic_error(

diff --git a/yasmin/yasmin/state_machine.py b/yasmin/yasmin/state_machine.py
@@ -189,6 +189,9 @@ def execute(self, blackboard: Blackboard) -> str:
 
         self.validate()
 
+        yasmin.YASMIN_LOG_INFO(
+            f"Executing state machine with initial state '{self._start_state}'"
+        )
         self._call_start_cbs(blackboard, self._start_state)
 
         with self.__current_state_lock:
@@ -200,7 +203,7 @@ def execute(self, blackboard: Blackboard) -> str:
                 state = self._states[self.__current_state]
 
             outcome = state["state"](blackboard)
-            old_come = outcome
+            old_outcome = outcome
 
             # check outcome belongs to state
             if outcome not in state["state"].get_outcomes():
@@ -210,9 +213,6 @@ def execute(self, blackboard: Blackboard) -> str:
 
             # translate outcome using transitions
             if outcome in state["transitions"]:
-                yasmin.YASMIN_LOG_INFO(
-                    f"{self.__current_state}: {outcome} --> {state['transitions'][outcome]}"
-                )
                 outcome = state["transitions"][outcome]
 
             # outcome is an outcome of the sm
@@ -226,12 +226,15 @@ def execute(self, blackboard: Blackboard) -> str:
 
             # outcome is a state
             elif outcome in self._states:
+                yasmin.YASMIN_LOG_INFO(
+                    f"{self.__current_state} ({old_outcome}) --> {outcome}"
+                )
 
                 self._call_transition_cbs(
                     blackboard,
                     self.get_current_state(),
                     outcome,
-                    old_come,
+                    old_outcome,
                 )
 
                 with self.__current_state_lock: