catching KeyboardInterrupt in python demos

uleroboticsgroup · Oct 31, 2024 · 92df08c · 92df08c
1 parent abe8aaf
commit 92df08c
Show file tree

Hide file tree

Showing 6 changed files with 72 additions and 30 deletions.
diff --git a/README.md b/README.md
@@ -179,11 +179,15 @@ def main():
     YasminViewerPub("yasmin_demo", sm)
 
     # execute FSM
-    outcome = sm()
-    yasmin.YASMIN_LOG_INFO(outcome)
+    try:
+        outcome = sm()
+        yasmin.YASMIN_LOG_INFO(outcome)
+    except KeyboardInterrupt:
+        sm.cancel_state()
 
     # shutdown ROS 2
-    rclpy.shutdown()
+    if rclpy.ok():
+        rclpy.shutdown()
 
 
 if __name__ == "__main__":
@@ -296,11 +300,15 @@ def main():
     YasminViewerPub("YASMIN_SERVICE_CLIENT_DEMO", sm)
 
     # execute FSM
-    outcome = sm()
-    yasmin.YASMIN_LOG_INFO(outcome)
+    try:
+        outcome = sm()
+        yasmin.YASMIN_LOG_INFO(outcome)
+    except KeyboardInterrupt:
+        sm.cancel_state()
 
     # shutdown ROS 2
-    rclpy.shutdown()
+    if rclpy.ok():
+        rclpy.shutdown()
 
 
 if __name__ == "__main__":
@@ -411,11 +419,15 @@ def main():
     blackboard["n"] = 10
 
     # execute FSM
-    outcome = sm(blackboard)
-    yasmin.YASMIN_LOG_INFO(outcome)
+    try:
+        outcome = sm(blackboard)
+        yasmin.YASMIN_LOG_INFO(outcome)
+    except KeyboardInterrupt:
+        sm.cancel_state()
 
     # shutdown ROS 2
-    rclpy.shutdown()
+    if rclpy.ok():
+        rclpy.shutdown()
 
 
 if __name__ == "__main__":
@@ -501,11 +513,15 @@ def main():
     YasminViewerPub("YASMIN_MONITOR_DEMO", sm)
 
     # execute FSM
-    outcome = sm()
-    yasmin.YASMIN_LOG_INFO(outcome)
+    try:
+        outcome = sm()
+        yasmin.YASMIN_LOG_INFO(outcome)
+    except KeyboardInterrupt:
+        sm.cancel_state()
 
     # shutdown ROS 2
-    rclpy.shutdown()
+    if rclpy.ok():
+        rclpy.shutdown()
 
 
 if __name__ == "__main__":
@@ -655,11 +671,16 @@ def main():
     # execute FSM
     blackboard = Blackboard()
     blackboard["waypoints_num"] = 2
-    outcome = sm(blackboard)
-    yasmin.YASMIN_LOG_INFO(outcome)
+
+    try:
+        outcome = sm(blackboard)
+        yasmin.YASMIN_LOG_INFO(outcome)
+    except KeyboardInterrupt:
+        sm.cancel_state()
 
     # shutdown ROS 2
-    rclpy.shutdown()
+    if rclpy.ok():
+        rclpy.shutdown()
 
 
 if __name__ == "__main__":

diff --git a/yasmin_demos/yasmin_demos/action_client_demo.py b/yasmin_demos/yasmin_demos/action_client_demo.py
@@ -104,11 +104,15 @@ def main():
     blackboard["n"] = 10
 
     # execute FSM
-    outcome = sm(blackboard)
-    yasmin.YASMIN_LOG_INFO(outcome)
+    try:
+        outcome = sm(blackboard)
+        yasmin.YASMIN_LOG_INFO(outcome)
+    except KeyboardInterrupt:
+        sm.cancel_state()
 
     # shutdown ROS 2
-    rclpy.shutdown()
+    if rclpy.ok():
+        rclpy.shutdown()
 
 
 if __name__ == "__main__":

diff --git a/yasmin_demos/yasmin_demos/monitor_demo.py b/yasmin_demos/yasmin_demos/monitor_demo.py
@@ -83,11 +83,15 @@ def main():
     YasminViewerPub("YASMIN_MONITOR_DEMO", sm)
 
     # execute FSM
-    outcome = sm()
-    yasmin.YASMIN_LOG_INFO(outcome)
+    try:
+        outcome = sm()
+        yasmin.YASMIN_LOG_INFO(outcome)
+    except KeyboardInterrupt:
+        sm.cancel_state()
 
     # shutdown ROS 2
-    rclpy.shutdown()
+    if rclpy.ok():
+        rclpy.shutdown()
 
 
 if __name__ == "__main__":

diff --git a/yasmin_demos/yasmin_demos/nav_demo.py b/yasmin_demos/yasmin_demos/nav_demo.py
@@ -157,11 +157,16 @@ def main():
     # execute FSM
     blackboard = Blackboard()
     blackboard["waypoints_num"] = 2
-    outcome = sm(blackboard)
-    yasmin.YASMIN_LOG_INFO(outcome)
+
+    try:
+        outcome = sm(blackboard)
+        yasmin.YASMIN_LOG_INFO(outcome)
+    except KeyboardInterrupt:
+        sm.cancel_state()
 
     # shutdown ROS 2
-    rclpy.shutdown()
+    if rclpy.ok():
+        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
@@ -106,11 +106,15 @@ def main():
     YasminViewerPub("YASMIN_SERVICE_CLIENT_DEMO", sm)
 
     # execute FSM
-    outcome = sm()
-    yasmin.YASMIN_LOG_INFO(outcome)
+    try:
+        outcome = sm()
+        yasmin.YASMIN_LOG_INFO(outcome)
+    except KeyboardInterrupt:
+        sm.cancel_state()
 
     # shutdown ROS 2
-    rclpy.shutdown()
+    if rclpy.ok():
+        rclpy.shutdown()
 
 
 if __name__ == "__main__":

diff --git a/yasmin_demos/yasmin_demos/yasmin_demo.py b/yasmin_demos/yasmin_demos/yasmin_demo.py
@@ -93,11 +93,15 @@ def main():
     YasminViewerPub("yasmin_demo", sm)
 
     # execute FSM
-    outcome = sm()
-    yasmin.YASMIN_LOG_INFO(outcome)
+    try:
+        outcome = sm()
+        yasmin.YASMIN_LOG_INFO(outcome)
+    except KeyboardInterrupt:
+        sm.cancel_state()
 
     # shutdown ROS 2
-    rclpy.shutdown()
+    if rclpy.ok():
+        rclpy.shutdown()
 
 
 if __name__ == "__main__":