Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[roseus_smach] add before and after hook for state machine execution #724

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open

[roseus_smach] add before and after hook for state machine execution #724

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
3ae96ee
add before and after hook for state machine execution
knorth55 Oct 18, 2022
c424c44
add catch loop for global exit
knorth55 Oct 18, 2022
9bfbb39
set userdata as same as mydata
knorth55 Oct 19, 2022
bcef5fa
pass userdata as arguments
knorth55 Oct 20, 2022
9a75aac
add exec-smach-userdata-hook in sample
knorth55 Oct 20, 2022
750c936
add before/after-hook-func in test
knorth55 Oct 20, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 10 additions & 2 deletions roseus_smach/sample/state-machine-ros-sample.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)~%")
51 changes: 28 additions & 23 deletions roseus_smach/src/state-machine-utils.l
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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:
Expand All @@ -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"
Expand Down
7 changes: 6 additions & 1 deletion roseus_smach/test/test-roseus-smach-utils.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
43 changes: 31 additions & 12 deletions roseus_smach/test/test-samples.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)))

Expand Down