diff --git a/roseus_smach/sample/state-machine-ros-sample.l b/roseus_smach/sample/state-machine-ros-sample.l
index f6b24e39e..92beb98da 100755
--- a/roseus_smach/sample/state-machine-ros-sample.l
+++ b/roseus_smach/sample/state-machine-ros-sample.l
@@ -46,5 +46,13 @@
 (defun exec-smach-simple () (setq count 0) (exec-state-machine (smach-simple)))
 (defun exec-smach-nested () (setq count 0) (exec-state-machine (smach-nested)))
 (defun exec-smach-userdata () (exec-state-machine (smach-userdata) '((count . 1))))
-
-(warn ";;(exec-smach-simple)~%;;(exec-smach-nested)~%;;(exec-smach-userdata)~%")
+(defun exec-smach-userdata-hook ()
+  (exec-state-machine
+    (smach-userdata) '((count . 1))
+    :before-hook-func
+    #'(lambda (userdata) (ros::ros-warn (format nil "userdata (before): ~A" userdata)))
+    :after-hook-func
+    #'(lambda (userdata) (ros::ros-warn (format nil "userdata (after): ~A" userdata)))
+    ))
+
+(warn ";;(exec-smach-simple)~%;;(exec-smach-nested)~%;;(exec-smach-userdata)~%;;(exec-smach-userdata-hook)~%")
diff --git a/roseus_smach/src/state-machine-utils.l b/roseus_smach/src/state-machine-utils.l
index 033129604..8272d4691 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:
@@ -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 mydata))
+        (send sm :execute mydata :step -1)
+        (if after-hook-func (funcall after-hook-func mydata))
+        (ros::sleep))
+      (send sm :active-state))))
 
 (defun smach-exec (sm)
   "Deprecated function"
diff --git a/roseus_smach/test/test-roseus-smach-utils.l b/roseus_smach/test/test-roseus-smach-utils.l
index 1f2e835a6..afbdec0e5 100644
--- a/roseus_smach/test/test-roseus-smach-utils.l
+++ b/roseus_smach/test/test-roseus-smach-utils.l
@@ -74,7 +74,12 @@
   (assert (eq (send *sm* :active-state) (send *sm* :node :end))))
 
 (deftest test-exec-state-machine
-  (exec-state-machine *sm* '((count . 0)) :spin t :hz 0.5 :root-name "SM_ROOT"))
+  (exec-state-machine *sm* '((count . 0)) :spin t :hz 0.5 :root-name "SM_ROOT"
+                      :before-hook-func
+                      #'(lambda (userdata) (ros::ros-warn (format nil "userdata (before): ~A" userdata)))
+                      :after-hook-func
+                      #'(lambda (userdata) (ros::ros-warn (format nil "userdata (after): ~A" userdata)))
+                      ))
 
 (run-all-tests)
 (exit)
diff --git a/roseus_smach/test/test-samples.l b/roseus_smach/test/test-samples.l
index d1c51a0a4..acfd307f9 100755
--- a/roseus_smach/test/test-samples.l
+++ b/roseus_smach/test/test-samples.l
@@ -35,20 +35,39 @@
 (defun exec-smach-simple2 () (setq count 0) (exec-state-machine (smach-simple2)))
 (defun exec-smach-simple3 () (setq count 0) (exec-state-machine (smach-simple3)))
 (defun exec-smach-simple-nested () (setq count 0) (exec-state-machine (smach-simple-nested)))
-(run-test-smach exec-smach-simple :outcome4 '((FOO) (BAR) (FOO) (BAR) (FOO) (BAR) (FOO)))
-(run-test-smach exec-smach-simple2 :outcome4 '((FOO) (BAR) (FOO) (BAR) (FOO) (BAR) (FOO)))
-(run-test-smach exec-smach-simple3 :outcome4 '((FOO) (BAR) (FOO) (BAR) (FOO) (BAR) (FOO)))
+; (run-test-smach exec-smach-simple :outcome4 '((FOO) (BAR) (FOO) (BAR) (FOO) (BAR) (FOO)))
+; (run-test-smach exec-smach-simple2 :outcome4 '((FOO) (BAR) (FOO) (BAR) (FOO) (BAR) (FOO)))
+; (run-test-smach exec-smach-simple3 :outcome4 '((FOO) (BAR) (FOO) (BAR) (FOO) (BAR) (FOO)))
+;
+; (run-test-smach exec-smach-nested :outcome5 '(nil (FOO) (BAR) (FOO) (BAR) (FOO) (BAR) nil (BAS) nil))
+; (run-test-smach exec-smach-simple-nested :outcome5 '(nil (FOO) (BAR) (FOO) (BAR) (FOO) (BAR) nil (BAS) nil))
+;
+; (run-test-smach exec-smach-userdata :outcome4 '((FOO) (BAR) (FOO) (BAR) (FOO)))
+; (run-test-smach exec-smach-userdata-hook :outcome4 '((FOO) (BAR) (FOO) (BAR) (FOO)))
+;
+; (deftest test-smach-sample-userdata ()
+; ;  (assert (eq (send (exec-smach-userdata) :name) :outcome4)
+; ;	  "sample of smach with userdata")
+;   (assert (eq (send (exec-state-machine (smach-userdata)) :name) :outcome4)
+;           "exec (smach-userdata) without initial userdata"))
 
-(run-test-smach exec-smach-nested :outcome5 '(nil (FOO) (BAR) (FOO) (BAR) (FOO) (BAR) nil (BAS) nil))
-(run-test-smach exec-smach-simple-nested :outcome5 '(nil (FOO) (BAR) (FOO) (BAR) (FOO) (BAR) nil (BAS) nil))
+(deftest test-smach-sample-userdata-no-hook ()
+  (let ((mydata '((count . 1) (test-count . 0))))
+    (exec-state-machine (smach-userdata) mydata)
+    (assert (eq (cdr (assoc 'test-count mydata)) 0)
+            "exec (smach-userdata) without hook failed")))
 
-(run-test-smach exec-smach-userdata :outcome4 '((FOO) (BAR) (FOO) (BAR) (FOO)))
-
-(deftest test-smach-sample-userdata ()
-;  (assert (eq (send (exec-smach-userdata) :name) :outcome4)
-;	  "sample of smach with userdata")
-  (assert (eq (send (exec-state-machine (smach-userdata)) :name) :outcome4)
-          "exec (smach-userdata) without initial userdata"))
+(deftest test-smach-sample-userdata-hook ()
+  (let ((mydata '((count . 1) (test-count . 0))))
+    (exec-state-machine
+      (smach-userdata) mydata
+      :before-hook-func
+      #'(lambda (userdata) (incf (cdr (assoc 'test-count userdata))))
+      :after-hook-func
+      #'(lambda (userdata) (incf (cdr (assoc 'test-count userdata))))
+      )
+    (assert (eq (cdr (assoc 'test-count mydata)) 10)
+            "exec (smach-userdata) with hook failed")))
 
 (run-test-smach exec-sample-parallel-state-machine :success '((PRESS-BUTTON) (CLOSE-DOOR) (PUT-SOAP PUT-CLOTH) (OPEN-DOOR)))