raise exception when state machine ends by canceling but with bad tra…

…nsition
uleroboticsgroup · Nov 1, 2024 · e40d254 · e40d254
1 parent f01df00
commit e40d254
Show file tree

Hide file tree

Showing 10 changed files with 66 additions and 24 deletions.
diff --git a/README.md b/README.md
@@ -786,8 +786,12 @@ int main(int argc, char *argv[]) {
   yasmin_viewer::YasminViewerPub yasmin_pub("yasmin_demo", sm);
 
   // execute
-  std::string outcome = (*sm.get())();
-  YASMIN_LOG_INFO(outcome.c_str());
+  try {
+    std::string outcome = (*sm.get())();
+    YASMIN_LOG_INFO(outcome.c_str());
+  } catch (const std::exception &e) {
+    YASMIN_LOG_WARN(e.what());
+  }
 
   rclcpp::shutdown();
 
@@ -921,8 +925,12 @@ int main(int argc, char *argv[]) {
   yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm);
 
   // execute
-  std::string outcome = (*sm.get())();
-  YASMIN_LOG_INFO(outcome.c_str());
+  try {
+    std::string outcome = (*sm.get())();
+    YASMIN_LOG_INFO(outcome.c_str());
+  } catch (const std::exception &e) {
+    YASMIN_LOG_WARN(e.what());
+  }
 
   rclcpp::shutdown();
 
@@ -1072,8 +1080,12 @@ int main(int argc, char *argv[]) {
   blackboard->set<int>("n", 10);
 
   // execute
-  std::string outcome = (*sm.get())(blackboard);
-  YASMIN_LOG_INFO(outcome.c_str());
+  try {
+    std::string outcome = (*sm.get())(blackboard);
+    YASMIN_LOG_INFO(outcome.c_str());
+  } catch (const std::exception &e) {
+    YASMIN_LOG_WARN(e.what());
+  }
 
   rclcpp::shutdown();
 
@@ -1179,8 +1191,12 @@ int main(int argc, char *argv[]) {
   yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_MONITOR_DEMO", sm);
 
   // execute
-  std::string outcome = (*sm.get())();
-  YASMIN_LOG_INFO(outcome.c_str());
+  try {
+    std::string outcome = (*sm.get())();
+    YASMIN_LOG_INFO(outcome.c_str());
+  } catch (const std::exception &e) {
+    YASMIN_LOG_WARN(e.what());
+  }
 
   rclcpp::shutdown();
 

diff --git a/yasmin/src/yasmin/state.cpp b/yasmin/src/yasmin/state.cpp
@@ -29,7 +29,7 @@ State::State(std::set<std::string> outcomes) : outcomes(outcomes) {}
 std::string
 State::operator()(std::shared_ptr<blackboard::Blackboard> blackboard) {
 
-  YASMIN_LOG_DEBUG("Executing state %s", this->to_string().c_str());
+  YASMIN_LOG_DEBUG("Executing state '%s'", this->to_string().c_str());
 
   this->canceled.store(false);
   std::string outcome = this->execute(blackboard);

diff --git a/yasmin/src/yasmin/state_machine.cpp b/yasmin/src/yasmin/state_machine.cpp
@@ -71,7 +71,7 @@ void StateMachine::add_state(std::string name, std::shared_ptr<State> state,
 
   std::string transition_string = "";
 
-  for (auto t : transitions) {
+  for (auto const &t : transitions) {
     transition_string += "\n\t" + t.first + " --> " + t.second;
   }
 
@@ -141,12 +141,14 @@ void StateMachine::add_end_cb(EndCallbackType cb,
 void StateMachine::call_start_cbs(
     std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
     const std::string &start_state) {
+
   try {
     for (const auto &callback_pair : this->start_cbs) {
       const auto &cb = callback_pair.first;
       const auto &args = callback_pair.second;
       cb(blackboard, start_state, args);
     }
+
   } catch (const std::exception &e) {
     YASMIN_LOG_ERROR("Could not execute start callback: %s"),
         std::string(e.what());
@@ -157,12 +159,14 @@ void StateMachine::call_transition_cbs(
     std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
     const std::string &from_state, const std::string &to_state,
     const std::string &outcome) {
+
   try {
     for (const auto &callback_pair : this->transition_cbs) {
       const auto &cb = callback_pair.first;
       const auto &args = callback_pair.second;
       cb(blackboard, from_state, to_state, outcome, args);
     }
+
   } catch (const std::exception &e) {
     YASMIN_LOG_ERROR("Could not execute transition callback: %s"),
         std::string(e.what());
@@ -172,13 +176,14 @@ void StateMachine::call_transition_cbs(
 void StateMachine::call_end_cbs(
     std::shared_ptr<yasmin::blackboard::Blackboard> blackboard,
     const std::string &outcome) {
-  try {
 
+  try {
     for (const auto &callback_pair : this->end_cbs) {
       const auto &cb = callback_pair.first;
       const auto &args = callback_pair.second;
       cb(blackboard, outcome, args);
     }
+
   } catch (const std::exception &e) {
     YASMIN_LOG_ERROR("Could not execute end callback: %s"),
         std::string(e.what());
@@ -187,7 +192,7 @@ void StateMachine::call_end_cbs(
 
 void StateMachine::validate() {
 
-  YASMIN_LOG_DEBUG("Validating state machine");
+  YASMIN_LOG_DEBUG("Validating state machine '%s'", this->to_string().c_str());
 
   // check initial state
   if (this->start_state.empty()) {
@@ -332,7 +337,8 @@ StateMachine::execute(std::shared_ptr<blackboard::Blackboard> blackboard) {
     }
   }
 
-  return "";
+  throw std::runtime_error("Ending canceled state machine '" +
+                           this->to_string() + "' with bad transition");
 }
 
 std::string StateMachine::execute() {

diff --git a/yasmin/tests/python/test_state.py b/yasmin/tests/python/test_state.py
@@ -49,7 +49,7 @@ def test_state_cancel(self):
         self.assertTrue(self.state.is_canceled())
 
     def test_state_get_outcomes(self):
-        self.assertEqual("outcome1", list(self.state.get_outcomes())[1])
+        self.assertEqual("outcome1", list(self.state.get_outcomes())[0])
 
     def test_state_str(self):
         self.assertEqual("FooState", str(self.state))

diff --git a/yasmin/yasmin/state.py b/yasmin/yasmin/state.py
@@ -35,7 +35,7 @@ def __init__(self, outcomes: Set[str]) -> None:
 
     def __call__(self, blackboard: Blackboard = None) -> str:
 
-        yasmin.YASMIN_LOG_DEBUG(f"Executing state {self}")
+        yasmin.YASMIN_LOG_DEBUG(f"Executing state '{self}'")
 
         self._canceled = False
 

diff --git a/yasmin/yasmin/state_machine.py b/yasmin/yasmin/state_machine.py
@@ -143,7 +143,7 @@ def _call_end_cbs(self, blackboard: Blackboard, outcome: str) -> None:
 
     def validate(self) -> None:
 
-        yasmin.YASMIN_LOG_DEBUG("Validating state machine")
+        yasmin.YASMIN_LOG_DEBUG(f"Validating state machine '{self}'")
 
         # check initial state
         if not self._start_state:
@@ -253,6 +253,10 @@ def execute(self, blackboard: Blackboard) -> str:
                     f"Outcome '{outcome}' is not a state neither a state machine outcome"
                 )
 
+        raise RuntimeError(
+            f"Ending canceled state machine '{self}' with bad transition"
+        )
+
     def cancel_state(self) -> None:
         super().cancel_state()
         with self.__current_state_lock:

diff --git a/yasmin_demos/src/action_client_demo.cpp b/yasmin_demos/src/action_client_demo.cpp
@@ -139,8 +139,12 @@ int main(int argc, char *argv[]) {
   blackboard->set<int>("n", 10);
 
   // execute
-  std::string outcome = (*sm.get())(blackboard);
-  YASMIN_LOG_INFO(outcome.c_str());
+  try {
+    std::string outcome = (*sm.get())(blackboard);
+    YASMIN_LOG_INFO(outcome.c_str());
+  } catch (const std::exception &e) {
+    YASMIN_LOG_WARN(e.what());
+  }
 
   rclcpp::shutdown();
 

diff --git a/yasmin_demos/src/monitor_demo.cpp b/yasmin_demos/src/monitor_demo.cpp
@@ -99,8 +99,12 @@ int main(int argc, char *argv[]) {
   yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_MONITOR_DEMO", sm);
 
   // execute
-  std::string outcome = (*sm.get())();
-  YASMIN_LOG_INFO(outcome.c_str());
+  try {
+    std::string outcome = (*sm.get())();
+    YASMIN_LOG_INFO(outcome.c_str());
+  } catch (const std::exception &e) {
+    YASMIN_LOG_WARN(e.what());
+  }
 
   rclcpp::shutdown();
 

diff --git a/yasmin_demos/src/service_client_demo.cpp b/yasmin_demos/src/service_client_demo.cpp
@@ -123,8 +123,12 @@ int main(int argc, char *argv[]) {
   yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm);
 
   // execute
-  std::string outcome = (*sm.get())();
-  YASMIN_LOG_INFO(outcome.c_str());
+  try {
+    std::string outcome = (*sm.get())();
+    YASMIN_LOG_INFO(outcome.c_str());
+  } catch (const std::exception &e) {
+    YASMIN_LOG_WARN(e.what());
+  }
 
   rclcpp::shutdown();
 

diff --git a/yasmin_demos/src/yasmin_demo.cpp b/yasmin_demos/src/yasmin_demo.cpp
@@ -98,8 +98,12 @@ int main(int argc, char *argv[]) {
   yasmin_viewer::YasminViewerPub yasmin_pub("yasmin_demo", sm);
 
   // execute
-  std::string outcome = (*sm.get())();
-  YASMIN_LOG_INFO(outcome.c_str());
+  try {
+    std::string outcome = (*sm.get())();
+    YASMIN_LOG_INFO(outcome.c_str());
+  } catch (const std::exception &e) {
+    YASMIN_LOG_WARN(e.what());
+  }
 
   rclcpp::shutdown();