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

[roseus_smach] add double nested example #719

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

[roseus_smach] add double nested example #719

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
b5aa30f
add double nested example
knorth55 Aug 30, 2022
4224673
add nested nested test
knorth55 Aug 30, 2022
96c6267
change name to double-nested
knorth55 Oct 20, 2022
fe7124e
Merge branch 'master' into nested-nested-example
knorth55 Oct 20, 2022
4d05c6f
Merge branch 'master' into nested-nested-example
knorth55 Jan 25, 2023
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
3 changes: 2 additions & 1 deletion 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 @@ -45,6 +45,7 @@

(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-double-nested () (setq count 0) (exec-state-machine (smach-double-nested)))
(defun exec-smach-userdata () (exec-state-machine (smach-userdata) '((count . 1))))

(warn ";;(exec-smach-simple)~%;;(exec-smach-nested)~%;;(exec-smach-userdata)~%")
(warn ";;(exec-smach-simple)~%;;(exec-smach-nested)~%;;(exec-smach-double-nested)~%;;(exec-smach-userdata)~%")
37 changes: 37 additions & 0 deletions roseus_smach/sample/state-machine-sample.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,12 @@
(defun func-bar (&rest args)
(format t "Execute state BAR~%")
:outcome2)
(defun func-foo-foo (&rest args)
(format t "Execute state FOOFOO~%")
:outcome6)
(defun func-bar-bar (&rest args)
(format t "Execute state BARBAR~%")
:outcome6)

(defun smach-simple ()
(let ((sm (instance state-machine :init)))
Expand Down Expand Up @@ -96,6 +102,37 @@
sm-top ))


(defun smach-double-nested ()
(let ((sm-top (instance state-machine :init))
(sm-sub (instance state-machine :init)))
;; state instance can include other state-machine like function
(send sm-top :add-node (instance state :init "SUB" sm-sub))
(send sm-top :add-node (instance state :init "BAS" 'func-bas))
(send sm-top :goal-state :outcome5)
(send sm-top :start-state "BAS")
(send sm-top :add-transition "BAS" "SUB" :outcome3)
(send sm-top :add-transition "SUB" :outcome5 :outcome4)
;; node instance can be args of :add-node, :start-state, :add-transition
(let ((foo-node (instance state :init "FOO" 'func-foo))
(sm-sub-sub (instance state-machine :init)))
(send sm-sub :add-node foo-node)
(send sm-sub :add-node (instance state :init "BAR" sm-sub-sub))
(send sm-sub :goal-state :outcome4)
(send sm-sub :start-state foo-node)
(send sm-sub :add-transition "FOO" "BAR" :outcome1)
(send sm-sub :add-transition "FOO" :outcome4 :outcome2)
(send sm-sub :add-transition "BAR" "FOO" :outcome7)
(let ((foo-foo-node (instance state :init "FOOFOO" 'func-foo-foo))
(bar-bar-node (instance state :init "BARBAR" 'func-bar-bar)))
(send sm-sub-sub :add-node foo-foo-node)
(send sm-sub-sub :add-node bar-bar-node)
(send sm-sub-sub :goal-state :outcome7)
(send sm-sub-sub :start-state foo-foo-node)
(send sm-sub-sub :add-transition "FOOFOO" "BARBAR" :outcome6)
(send sm-sub-sub :add-transition "BARBAR" :outcome7 :outcome6)))
sm-top ))


(defun smach-simple-nested ()
(let (sm-top sm-sub)
(setq sm-sub
Expand Down
5 changes: 3 additions & 2 deletions roseus_smach/test/test-samples.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
(ros::subscribe "/server_name/smach/container_status" smach_msgs::SmachContainerStatus
#'(lambda (msg) (push (send msg :active_states) *container-active-status*)))

(defmacro run-test-smach (func outcome active-states)
(defmacro run-test-smach (func outcome active-states &key (duration 5.0))
(let ((test-func-name (read-from-string (format nil "test-~A" func))))
`(deftest ,test-func-name
(let (start-tm result)
Expand All @@ -22,7 +22,7 @@
(setq elapsed-tm (ros::time- (ros::time-now) start-tm))
(format *error-output* "received container-active-status is ~A, and takes ~A sec~%" *container-active-status* (send elapsed-tm :to-sec))
(assert (= (length (remove nil *container-active-status*)) (length (remove nil ,active-states))) (format nil "length of container active status is ~A, but ~A~%" (length (remove nil ,active-states)) (length (remove nil *container-active-status*))))
(assert (eps= (send elapsed-tm :to-sec) (float (length ,active-states)) 5.0) (format nil "duration of container active status is equal to length of container active status (~A), but ~A~%" (length ,active-states) (send elapsed-tm :to-sec)))
(assert (eps= (send elapsed-tm :to-sec) (float (length ,active-states)) ,duration) (format nil "duration of container active status is equal to length of container active status (~A), but ~A~%" (length ,active-states) (send elapsed-tm :to-sec)))
))
))

Expand All @@ -40,6 +40,7 @@
(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-double-nested :outcome5 '(nil (FOO) nil (BARBAR) (FOOFOO) (FOO) nil (BARBAR) (FOOFOO) (FOO) nil (BARBAR) nil nil (BAS)) :duration 20.0)
(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)))
Expand Down