Merge remote-tracking branch 'knorth55/exec-state-machine-hook' into …

…fetch15
jsk-ros-pkg · Oct 19, 2022 · 5f1ec6e · 5f1ec6e
2 parents 01b3f20 + c424c44
commit 5f1ec6e
Showing 1 changed file with 28 additions and 23 deletions.
diff --git 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:
@@ -21,28 +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"))))))
-      (send sm :execute mydata :step -1)
-      (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"