From 3ae96ee728bec48855fa20d8594bfb14bc204b83 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Tue, 18 Oct 2022 22:21:37 +0900
Subject: [PATCH 1/2] add before and after hook for state machine execution

---
 roseus_smach/src/state-machine-utils.l | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/roseus_smach/src/state-machine-utils.l b/roseus_smach/src/state-machine-utils.l
index 033129604..c32ac5d43 100644
--- a/roseus_smach/src/state-machine-utils.l
+++ b/roseus_smach/src/state-machine-utils.l
@@ -1,7 +1,9 @@
 ;; state-machine-utils.l
 
 (defun exec-state-machine (sm &optional (mydata '(nil))
-                           &key (spin t) (hz 1) (root-name "SM_ROOT") (srv-name "/server_name") iterate)
+                           &key (spin t) (hz 1) (root-name "SM_ROOT") (srv-name "/server_name") iterate
+                           (before-hook-func) (after-hook-func)
+                           )
   "Execute state machine
 
 Args:
@@ -40,7 +42,9 @@ Returns:
              (ros::ros-warn "aborting...")
              (return))
           (t (error "value of key :iterate must be t or function"))))))
+      (if before-hook-func (funcall before-hook-func))
       (send sm :execute mydata :step -1)
+      (if after-hook-func (funcall after-hook-func))
       (ros::sleep))
     (send sm :active-state)))
 

From c424c4474784e463be1c63bc5647c134a54a5637 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Tue, 18 Oct 2022 22:21:52 +0900
Subject: [PATCH 2/2] add catch loop for global exit

---
 roseus_smach/src/state-machine-utils.l | 49 +++++++++++++-------------
 1 file changed, 25 insertions(+), 24 deletions(-)

diff --git a/roseus_smach/src/state-machine-utils.l b/roseus_smach/src/state-machine-utils.l
index c32ac5d43..e2ee985f7 100644
--- a/roseus_smach/src/state-machine-utils.l
+++ b/roseus_smach/src/state-machine-utils.l
@@ -23,30 +23,31 @@ Returns:
     (apply #'send sm :arg-keys (union (send sm :arg-keys) (mapcar #'car mydata)))
 
     (ros::rate hz)
-    (while (ros::ok)
-      (when spin
-        (send insp :spin-once)
-        (if (and (boundp '*ri*) *ri*) (send *ri* :spin-once)))
-      (send insp :publish-status mydata)
-      (when (send sm :goal-reached)
-        (return))
-      (when iterate
-        (cond
-          ((functionp iterate)
-           (unless (funcall iterate (send sm :active-state))
-             (ros::ros-warn "set abort in iteration")
-             (return))
-          (iterate
-           (unless (y-or-n-p (format nil "Execute ~A ? "
-                                     (send (send sm :active-state) :name)))
-             (ros::ros-warn "aborting...")
-             (return))
-          (t (error "value of key :iterate must be t or function"))))))
-      (if before-hook-func (funcall before-hook-func))
-      (send sm :execute mydata :step -1)
-      (if after-hook-func (funcall after-hook-func))
-      (ros::sleep))
-    (send sm :active-state)))
+    (catch :exec-state-machine-loop
+      (while (ros::ok)
+        (when spin
+          (send insp :spin-once)
+          (if (and (boundp '*ri*) *ri*) (send *ri* :spin-once)))
+        (send insp :publish-status mydata)
+        (when (send sm :goal-reached)
+          (return))
+        (when iterate
+          (cond
+            ((functionp iterate)
+             (unless (funcall iterate (send sm :active-state))
+               (ros::ros-warn "set abort in iteration")
+               (return))
+            (iterate
+             (unless (y-or-n-p (format nil "Execute ~A ? "
+                                       (send (send sm :active-state) :name)))
+               (ros::ros-warn "aborting...")
+               (return))
+            (t (error "value of key :iterate must be t or function"))))))
+        (if before-hook-func (funcall before-hook-func))
+        (send sm :execute mydata :step -1)
+        (if after-hook-func (funcall after-hook-func))
+        (ros::sleep))
+      (send sm :active-state))))
 
 (defun smach-exec (sm)
   "Deprecated function"