diff --git a/README.md b/README.md index b56a0e3..4a65d6b 100644 --- a/README.md +++ b/README.md @@ -769,7 +769,11 @@ int main(int argc, char *argv[]) { std::initializer_list{"outcome4"}); // cancel state machine on ROS 2 shutdown - rclcpp::on_shutdown([sm]() { sm->cancel_state(); }); + rclcpp::on_shutdown([sm]() { + if (sm->is_running()) { + sm->cancel_state(); + } + }); // add states sm->add_state("FOO", std::make_shared(), @@ -895,7 +899,11 @@ int main(int argc, char *argv[]) { std::initializer_list{"outcome4"}); // cancel state machine on ROS 2 shutdown - rclcpp::on_shutdown([sm]() { sm->cancel_state(); }); + rclcpp::on_shutdown([sm]() { + if (sm->is_running()) { + sm->cancel_state(); + } + }); // add states sm->add_state("SETTING_INTS", @@ -1053,7 +1061,11 @@ int main(int argc, char *argv[]) { std::initializer_list{"outcome4"}); // cancel state machine on ROS 2 shutdown - rclcpp::on_shutdown([sm]() { sm->cancel_state(); }); + rclcpp::on_shutdown([sm]() { + if (sm->is_running()) { + sm->cancel_state(); + } + }); // add states sm->add_state("CALLING_FIBONACCI", std::make_shared(), @@ -1177,7 +1189,11 @@ int main(int argc, char *argv[]) { std::initializer_list{"outcome4"}); // cancel state machine on ROS 2 shutdown - rclcpp::on_shutdown([sm]() { sm->cancel_state(); }); + rclcpp::on_shutdown([sm]() { + if (sm->is_running()) { + sm->cancel_state(); + } + }); // add states sm->add_state("PRINTING_ODOM", std::make_shared(5), diff --git a/yasmin/include/yasmin/state.hpp b/yasmin/include/yasmin/state.hpp index 61ea0ba..bf261c8 100644 --- a/yasmin/include/yasmin/state.hpp +++ b/yasmin/include/yasmin/state.hpp @@ -36,6 +36,7 @@ class State { private: std::atomic_bool canceled{false}; + std::atomic_bool running{false}; public: State(std::set outcomes); @@ -53,6 +54,7 @@ class State { this->canceled.store(true); }; bool is_canceled() const; + bool is_running() const; std::set const &get_outcomes(); diff --git a/yasmin/src/yasmin/state.cpp b/yasmin/src/yasmin/state.cpp index 89772c7..b29cb73 100644 --- a/yasmin/src/yasmin/state.cpp +++ b/yasmin/src/yasmin/state.cpp @@ -32,6 +32,7 @@ State::operator()(std::shared_ptr blackboard) { YASMIN_LOG_DEBUG("Executing state '%s'", this->to_string().c_str()); this->canceled.store(false); + this->running.store(true); std::string outcome = this->execute(blackboard); if (std::find(this->outcomes.begin(), this->outcomes.end(), outcome) == @@ -59,9 +60,12 @@ State::operator()(std::shared_ptr blackboard) { "'. The possible outcomes are: " + outcomes_string.c_str()); } + this->running.store(false); return outcome; } bool State::is_canceled() const { return this->canceled.load(); } +bool State::is_running() const { return this->running.load(); } + std::set const &State::get_outcomes() { return this->outcomes; } diff --git a/yasmin/tests/python/test_state.py b/yasmin/tests/python/test_state.py index 1e4b0e1..3344d58 100644 --- a/yasmin/tests/python/test_state.py +++ b/yasmin/tests/python/test_state.py @@ -37,7 +37,6 @@ def execute(self, blackboard): class TestState(unittest.TestCase): def setUp(self): - self.state = FooState() def test_state_call(self): diff --git a/yasmin/yasmin/state.py b/yasmin/yasmin/state.py index 17d2289..e1889c4 100644 --- a/yasmin/yasmin/state.py +++ b/yasmin/yasmin/state.py @@ -25,6 +25,7 @@ class State(ABC): def __init__(self, outcomes: Set[str]) -> None: self._outcomes = set() + self._running = False self._canceled = False if outcomes: @@ -38,6 +39,7 @@ def __call__(self, blackboard: Blackboard = None) -> str: yasmin.YASMIN_LOG_DEBUG(f"Executing state '{self}'") self._canceled = False + self._running = True if blackboard is None: blackboard = Blackboard() @@ -49,6 +51,7 @@ def __call__(self, blackboard: Blackboard = None) -> str: f"Outcome '{outcome}' does not belong to the outcomes of the state '{self}'. The possible outcomes are: {self._outcomes}" ) + self._running = False return outcome @abstractmethod @@ -65,5 +68,8 @@ def cancel_state(self) -> None: def is_canceled(self) -> bool: return self._canceled + def is_running(self) -> bool: + return self._running + def get_outcomes(self) -> Set[str]: return self._outcomes diff --git a/yasmin_demos/src/action_client_demo.cpp b/yasmin_demos/src/action_client_demo.cpp index 1af99a0..53952ca 100644 --- a/yasmin_demos/src/action_client_demo.cpp +++ b/yasmin_demos/src/action_client_demo.cpp @@ -112,7 +112,11 @@ int main(int argc, char *argv[]) { std::initializer_list{"outcome4"}); // cancel state machine on ROS 2 shutdown - rclcpp::on_shutdown([sm]() { sm->cancel_state(); }); + rclcpp::on_shutdown([sm]() { + if (sm->is_running()) { + sm->cancel_state(); + } + }); // add states sm->add_state("CALLING_FIBONACCI", std::make_shared(), diff --git a/yasmin_demos/src/monitor_demo.cpp b/yasmin_demos/src/monitor_demo.cpp index d4dabb2..9cd402c 100644 --- a/yasmin_demos/src/monitor_demo.cpp +++ b/yasmin_demos/src/monitor_demo.cpp @@ -85,7 +85,11 @@ int main(int argc, char *argv[]) { std::initializer_list{"outcome4"}); // cancel state machine on ROS 2 shutdown - rclcpp::on_shutdown([sm]() { sm->cancel_state(); }); + rclcpp::on_shutdown([sm]() { + if (sm->is_running()) { + sm->cancel_state(); + } + }); // add states sm->add_state("PRINTING_ODOM", std::make_shared(5), diff --git a/yasmin_demos/src/service_client_demo.cpp b/yasmin_demos/src/service_client_demo.cpp index 6e4e75a..7cf70fb 100644 --- a/yasmin_demos/src/service_client_demo.cpp +++ b/yasmin_demos/src/service_client_demo.cpp @@ -93,7 +93,11 @@ int main(int argc, char *argv[]) { std::initializer_list{"outcome4"}); // cancel state machine on ROS 2 shutdown - rclcpp::on_shutdown([sm]() { sm->cancel_state(); }); + rclcpp::on_shutdown([sm]() { + if (sm->is_running()) { + sm->cancel_state(); + } + }); // add states sm->add_state("SETTING_INTS", diff --git a/yasmin_demos/src/yasmin_demo.cpp b/yasmin_demos/src/yasmin_demo.cpp index dca766b..2e0b285 100644 --- a/yasmin_demos/src/yasmin_demo.cpp +++ b/yasmin_demos/src/yasmin_demo.cpp @@ -81,7 +81,11 @@ int main(int argc, char *argv[]) { std::initializer_list{"outcome4"}); // cancel state machine on ROS 2 shutdown - rclcpp::on_shutdown([sm]() { sm->cancel_state(); }); + rclcpp::on_shutdown([sm]() { + if (sm->is_running()) { + sm->cancel_state(); + } + }); // add states sm->add_state("FOO", std::make_shared(), diff --git a/yasmin_demos/yasmin_demos/action_client_demo.py b/yasmin_demos/yasmin_demos/action_client_demo.py index 8819d94..6b8cbf6 100755 --- a/yasmin_demos/yasmin_demos/action_client_demo.py +++ b/yasmin_demos/yasmin_demos/action_client_demo.py @@ -108,7 +108,8 @@ def main(): outcome = sm(blackboard) yasmin.YASMIN_LOG_INFO(outcome) except KeyboardInterrupt: - sm.cancel_state() + if sm.is_running(): + sm.cancel_state() # shutdown ROS 2 if rclpy.ok(): diff --git a/yasmin_demos/yasmin_demos/monitor_demo.py b/yasmin_demos/yasmin_demos/monitor_demo.py index 7708ac4..dd5d18d 100755 --- a/yasmin_demos/yasmin_demos/monitor_demo.py +++ b/yasmin_demos/yasmin_demos/monitor_demo.py @@ -87,7 +87,8 @@ def main(): outcome = sm() yasmin.YASMIN_LOG_INFO(outcome) except KeyboardInterrupt: - sm.cancel_state() + if sm.is_running(): + sm.cancel_state() # shutdown ROS 2 if rclpy.ok(): diff --git a/yasmin_demos/yasmin_demos/nav_demo.py b/yasmin_demos/yasmin_demos/nav_demo.py index d8a577e..87618e8 100755 --- a/yasmin_demos/yasmin_demos/nav_demo.py +++ b/yasmin_demos/yasmin_demos/nav_demo.py @@ -166,7 +166,8 @@ def main(): # shutdown ROS 2 if rclpy.ok(): - rclpy.shutdown() + if sm.is_running(): + rclpy.shutdown() if __name__ == "__main__": diff --git a/yasmin_demos/yasmin_demos/service_client_demo.py b/yasmin_demos/yasmin_demos/service_client_demo.py index f4cdfba..2f4c367 100755 --- a/yasmin_demos/yasmin_demos/service_client_demo.py +++ b/yasmin_demos/yasmin_demos/service_client_demo.py @@ -110,7 +110,8 @@ def main(): outcome = sm() yasmin.YASMIN_LOG_INFO(outcome) except KeyboardInterrupt: - sm.cancel_state() + if sm.is_running(): + sm.cancel_state() # shutdown ROS 2 if rclpy.ok(): diff --git a/yasmin_demos/yasmin_demos/yasmin_demo.py b/yasmin_demos/yasmin_demos/yasmin_demo.py index b12d41b..2ed4869 100755 --- a/yasmin_demos/yasmin_demos/yasmin_demo.py +++ b/yasmin_demos/yasmin_demos/yasmin_demo.py @@ -97,7 +97,8 @@ def main(): outcome = sm() yasmin.YASMIN_LOG_INFO(outcome) except KeyboardInterrupt: - sm.cancel_state() + if sm.is_running(): + sm.cancel_state() # shutdown ROS 2 if rclpy.ok():