is_running added to state

uleroboticsgroup · Nov 1, 2024 · 91a50d6 · 91a50d6
1 parent e40d254
commit 91a50d6
Show file tree

Hide file tree

Showing 14 changed files with 62 additions and 14 deletions.
diff --git a/README.md b/README.md
@@ -769,7 +769,11 @@ int main(int argc, char *argv[]) {
       std::initializer_list<std::string>{"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<FooState>(),
@@ -895,7 +899,11 @@ int main(int argc, char *argv[]) {
       std::initializer_list<std::string>{"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<std::string>{"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<FibonacciState>(),
@@ -1177,7 +1189,11 @@ int main(int argc, char *argv[]) {
       std::initializer_list<std::string>{"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<PrintOdometryState>(5),

diff --git 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<std::string> outcomes);
@@ -53,6 +54,7 @@ class State {
     this->canceled.store(true);
   };
   bool is_canceled() const;
+  bool is_running() const;
 
   std::set<std::string> const &get_outcomes();
 

diff --git a/yasmin/src/yasmin/state.cpp b/yasmin/src/yasmin/state.cpp
@@ -32,6 +32,7 @@ State::operator()(std::shared_ptr<blackboard::Blackboard> 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::Blackboard> 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<std::string> const &State::get_outcomes() { return this->outcomes; }
diff --git 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
@@ -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
@@ -112,7 +112,11 @@ int main(int argc, char *argv[]) {
       std::initializer_list<std::string>{"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<FibonacciState>(),

diff --git 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<std::string>{"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<PrintOdometryState>(5),

diff --git 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<std::string>{"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
@@ -81,7 +81,11 @@ int main(int argc, char *argv[]) {
       std::initializer_list<std::string>{"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<FooState>(),

diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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():