diff --git a/README.md b/README.md index 8601661..b56a0e3 100644 --- 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("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 index a92e455..89772c7 100644 --- a/yasmin/src/yasmin/state.cpp +++ b/yasmin/src/yasmin/state.cpp @@ -29,7 +29,7 @@ State::State(std::set outcomes) : outcomes(outcomes) {} std::string State::operator()(std::shared_ptr 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 index 5773954..007aca6 100644 --- 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, 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 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 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 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) { } } - 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 index d500368..1e4b0e1 100644 --- 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 index 7790bcd..17d2289 100644 --- 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 index 4097b55..24d02e6 100644 --- 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 index 5ecc715..1af99a0 100644 --- 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("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 index 4f2c782..d4dabb2 100644 --- 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 index 79de04f..6e4e75a 100644 --- 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 index c25e553..dca766b 100644 --- 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();