From cca3d5f41eca6e1d5326cd7ae3e0a08dd37b28c6 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 24 Feb 2021 18:10:31 +0900 Subject: [PATCH 1/7] add more test, check :active-tates, duration time --- roseus_smach/test/test-samples.l | 35 ++++++++++++++++++++++++-------- 1 file changed, 27 insertions(+), 8 deletions(-) diff --git a/roseus_smach/test/test-samples.l b/roseus_smach/test/test-samples.l index 6c0767b61..4a8e40781 100755 --- a/roseus_smach/test/test-samples.l +++ b/roseus_smach/test/test-samples.l @@ -4,19 +4,38 @@ (ros::roseus "test_roseus_smach_samples") + +(setq *container-active-status* nil) +(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) + (let ((test-func-name (read-from-string (format nil "test-~A" func)))) + `(deftest ,test-func-name + (let (start-tm result) + (setq *container-active-status* nil) + (setq start-tm (ros::time-now)) + (setq result (funcall ',func)) + (format *error-output* "executed ~A and returns ~A~%" ',func result) + (assert (eq (send result :name) ,outcome) "expected result is ~A, but returns ~A" ,outcome result) + (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))) + )) + )) + (init-unit-test) -(deftest test-smach-sample-simple () - (assert (eq (send (exec-smach-simple) :name) :outcome4) - "simple smach sample")) +(run-test-smach exec-smach-simple :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)) -(deftest test-smach-sample-nested () - (assert (eq (send (exec-smach-nested) :name) :outcome5) - "nested smach sample")) +(run-test-smach exec-smach-userdata :outcome4 '((FOO) (BAR) (FOO) (BAR) (FOO) nil)) (deftest test-smach-sample-userdata () - (assert (eq (send (exec-smach-userdata) :name) :outcome4) - "sample of smach with 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")) From dd36fa27b8eb42bdb978036be2b85b51ff76a8f6 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 24 Feb 2021 18:18:04 +0900 Subject: [PATCH 2/7] add make-sample-parallel-state-machine tests --- roseus_smach/test/test-samples.l | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/roseus_smach/test/test-samples.l b/roseus_smach/test/test-samples.l index 4a8e40781..c51dbf886 100755 --- a/roseus_smach/test/test-samples.l +++ b/roseus_smach/test/test-samples.l @@ -1,6 +1,7 @@ (require :unittest "lib/llib/unittest.l") (load "package://roseus_smach/sample/state-machine-ros-sample.l") +(load "package://roseus_smach/sample/parallel-state-machine-sample.l") (ros::roseus "test_roseus_smach_samples") @@ -27,11 +28,15 @@ (init-unit-test) +(defun exec-sample-parallel-state-machine () + (make-sample-parallel-state-machine) + (exec-state-machine *sm*)) + (run-test-smach exec-smach-simple :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-userdata :outcome4 '((FOO) (BAR) (FOO) (BAR) (FOO) nil)) +(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) @@ -39,6 +44,8 @@ (assert (eq (send (exec-state-machine (smach-userdata)) :name) :outcome4) "exec (smach-userdata) without initial userdata")) +(run-test-smach exec-sample-parallel-state-machine :success '((PRESS-BUTTON) (CLOSE-DOOR) (PUT-SOAP PUT-CLOTH) (OPEN-DOOR))) + (deftest test-smach-action-client-state () (setq userdata '(nil)) (assert (eq (send (exec-state-machine (smach-action-client-state) userdata) :name) :SUCCEED-STATE) From fc76bfb10aa57a2f56f89b5529ae59e6a1dffcd8 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 24 Feb 2021 15:37:24 +0900 Subject: [PATCH 3/7] fix typo of nestate state machine example path --- roseus_smach/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus_smach/README.md b/roseus_smach/README.md index 1b8830355..f17e0a682 100644 --- a/roseus_smach/README.md +++ b/roseus_smach/README.md @@ -27,7 +27,7 @@ Sample codes are available on `sample` directory. (exec-smach-simple) ``` - nested state machine - ![](http://gist.github.com/furushchev/raw/9b1ed0aa57b47537cd2d/smach-nested.gif) + ![](http://gist.github.com/furushchev/9b1ed0aa57b47537cd2d/raw/smach-nested.gif) ``` rosrun smach_viewer smach_viewer.py ``` From d5641ec02711a61d0b52e2242188a8a40930e4e3 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 24 Feb 2021 15:41:10 +0900 Subject: [PATCH 4/7] add more info(URL/python code) on sample --- roseus_smach/sample/state-machine-sample.l | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/roseus_smach/sample/state-machine-sample.l b/roseus_smach/sample/state-machine-sample.l index 6204bdcdb..567dbfc2b 100755 --- a/roseus_smach/sample/state-machine-sample.l +++ b/roseus_smach/sample/state-machine-sample.l @@ -7,6 +7,8 @@ ;; ;; sample 1: simple state machine +;; Euslisp version of http://wiki.ros.org/smach/Tutorials/Getting%20Started#Example +;; https://raw.githubusercontent.com/rhaschke/executive_smach_tutorials/indigo-devel/examples/state_machine_simple.py ;; (setq count 0) (defun func-foo (&rest args) @@ -34,6 +36,8 @@ ;; ;; sample 2: nodes can contain other state machine +;; Euslisp version of http://wiki.ros.org/smach/Tutorials/Create%20a%20hierarchical%20state%20machine +;; https://raw.githubusercontent.com/rhaschke/executive_smach_tutorials/indigo-devel/examples/state_machine_nesting2.py ;; (defun func-bas (&rest args) (format t "Execute state BAS~%") @@ -66,6 +70,8 @@ ;; ;; sample 3: A State machine reperesents only transitions between conditions. ;; There is no local variable in state machine. +;; Euslisp version of http://wiki.ros.org/smach/Tutorials/User%20Data +;; https://raw.githubusercontent.com/rhaschke/executive_smach_tutorials/indigo-devel/examples/user_data2.py ;; (defun func-foo-data (args) (format t "Execute state FOO~%") From 83356fe4facef8578626f65d287e9367a2917b26 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 25 Feb 2021 12:53:46 +0900 Subject: [PATCH 5/7] write more info similar to rospy implementation --- roseus_smach/src/state-machine.l | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/roseus_smach/src/state-machine.l b/roseus_smach/src/state-machine.l index 0722a2a7e..78e427f56 100644 --- a/roseus_smach/src/state-machine.l +++ b/roseus_smach/src/state-machine.l @@ -120,13 +120,16 @@ (send active-state :execute userdata :step (1- step)))) |# ;; execute once on this machine - (let (ret next-active-state) + (let (ret next-active-state trans-list) + ;;(warning-message 3 "Executing state ~A~%" (send-all active-state :name)) (dolist (astate active-state) + (warning-message 3 "Executing state ~A~%" (send astate :name)) (let* ((last-state astate) (trans (send self :next-arc-list astate)) (exec-result (send last-state :execute userdata))) (ros::ros-debug "trans: ~A" trans) (setq trans (remove-if-not #'(lambda(tr)(send tr :check exec-result)) trans)) + (setq trans-list (append trans-list (list trans))) (case (length trans) (0 (error "undefined transition ~A from ~A~%" exec-result last-state)) (1 t) ;; OK @@ -142,6 +145,15 @@ (if (send astate :submachine) (send (send astate :submachine) :reset-state)) (push exec-result ret))) + ;; spew some info + (when (> (length active-state) 1) + (dotimes (i (length active-state)) + (warning-message 3 "Concurent state '~A' retuned outcome '~A' on termination~%" + (send (elt active-state i) :name) (elt ret i))) + (warning-message 3 "Concurrent Outcomes ~A~%" (mapcar #'(lambda (s r) (cons (send s :name) r)) active-state ret))) + (warning-message 3 "State machine ~A '~A' :'~A' --> '~A'~%" + (if (send self :goal-test next-active-state) "terminating" "transitioning") + (send-all active-state :name) (send-all (flatten trans-list) :name) (send-all next-active-state :name)) (setq active-state (unique next-active-state)) ret)) From 69065a45290ec3253d050bc62acce579031f2cb5 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 25 Feb 2021 13:09:36 +0900 Subject: [PATCH 6/7] add example/test for smach-simple with make-state-machine function --- roseus_smach/README.md | 61 ++++++++++++++++++++++ roseus_smach/sample/state-machine-sample.l | 29 ++++++++++ roseus_smach/test/test-samples.l | 4 ++ 3 files changed, 94 insertions(+) diff --git a/roseus_smach/README.md b/roseus_smach/README.md index f17e0a682..ff112e1ec 100644 --- a/roseus_smach/README.md +++ b/roseus_smach/README.md @@ -216,3 +216,64 @@ These lines define the behavior of `sm-sub` in detail just like the previous sim (send (smach-nested) :execute nil) ``` Finally, the `sm-top` is executed here. + +## Writing Simple Smach with `(make-state-machine)` + +`make-state-machine` function provides easy-way to define simple state machine. It requires `graph-list`, `func-map`, `initial-state`, `goal-states` as arguments. + +For example, [simple state machine](http://wiki.ros.org/smach/Tutorials/Getting%20Started#Example) can be written as + +```lisp +(defun smach-simple2 () + (let (sm) + (setq sm + (make-state-machine + ;; define graph, list of ( ) + ;; if is ->, it corresponds when node returns t and !-> for nil. + '((:foo :outcome2 :outcome4) + (:foo :outcome1 :bar) + (:bar :outcome2 :foo)) + ;; define function map + '((:foo 'func-foo) ;; foo returns :outcome1 3 times and then returns :outcome2 + (:bar 'func-bar)) ;; bar always returns :outcome2 + ;; initial state + '(:foo) + ;; goal state + '(:outcome4))))) +``` + +This example have two node `:foo` and `:bar` and `:outcome4` as terminate node. +Each node corresponds to `'func-foo` and `'func-bar` functions. +The function `'func-foo` returns `:outcome1` 3 times and then returns `:outcome2`. +The function `'func-bar` always returns `:outcome2`. + +`(:foo :outcome2 :outcome4)` means when `:foo` returns `:outcome2`, it transit to `:outcome4`. +`(:foo :outcome1 :bar)` means when `:foo` returns `:outcome1`, it transit to `:bar`. +`(:bar :outcome2 :foo)` means when `:bar` returns `:outcome2`, it transit to `:foo`. + + +To simplify the state machine definition, we recommend users to use `t`/`nil` for return value of each node, so that users is able to use `(:foo -> :outcome4)` for graph definition. + +```lisp +(defun smach-simple3 () + (let (sm) + (setq sm + (make-state-machine + '((:foo -> :outcome4) + (:foo !-> :bar) + (:bar -> :foo)) + '((:foo '(lambda (&rest args) (cond ((< count 3) (incf count) nil) (t t)))) ;; foo returns nil 3 times and then returns t + (:bar '(lambda (&rest args) t))) ;; bar always returns t + '(:foo) + '(:outcome4))))) +``` + + +Both example can be tested with +``` +$ roscd roseus_smach/sample +$ roseus state-machine-ros-sample.l "(progn (setq count 0)(exec-state-machine (smach-simple2)))" +or +$ roseus state-machine-ros-sample.l "(progn (setq count 0)(exec-state-machine (smach-simple3)))" +``` +and you can check the state machine behavior with ` rosrun smach_viewer smach_viewer.py` diff --git a/roseus_smach/sample/state-machine-sample.l b/roseus_smach/sample/state-machine-sample.l index 567dbfc2b..35b8be6ab 100755 --- a/roseus_smach/sample/state-machine-sample.l +++ b/roseus_smach/sample/state-machine-sample.l @@ -34,6 +34,35 @@ (send sm :add-transition :BAR :FOO :outcome2) sm )) +(defun smach-simple2 () + (let (sm) + (setq sm + (make-state-machine + ;; define graph, list of ( ) + ;; if is ->, it corresponds when node returns t and !-> for nil. + '((:foo :outcome2 :outcome4) + (:foo :outcome1 :bar) + (:bar :outcome2 :foo)) + ;; define function map + '((:foo 'func-foo) ;; foo returns :outcome1 3 times and then returns :outcome2 + (:bar 'func-bar)) ;; bar always returns :outcome2 + ;; initial state + '(:foo) + ;; goal state + '(:outcome4))))) + +(defun smach-simple3 () + (let (sm) + (setq sm + (make-state-machine + '((:foo -> :outcome4) + (:foo !-> :bar) + (:bar -> :foo)) + '((:foo '(lambda (&rest args) (cond ((< count 3) (incf count) nil) (t t)))) ;; foo returns nil 3 times and then returns t + (:bar '(lambda (&rest args) t))) ;; bar always returns t + '(:foo) + '(:outcome4))))) + ;; ;; sample 2: nodes can contain other state machine ;; Euslisp version of http://wiki.ros.org/smach/Tutorials/Create%20a%20hierarchical%20state%20machine diff --git a/roseus_smach/test/test-samples.l b/roseus_smach/test/test-samples.l index c51dbf886..17ae4b870 100755 --- a/roseus_smach/test/test-samples.l +++ b/roseus_smach/test/test-samples.l @@ -32,7 +32,11 @@ (make-sample-parallel-state-machine) (exec-state-machine *sm*)) +(defun exec-smach-simple2 () (setq count 0) (exec-state-machine (smach-simple2))) +(defun exec-smach-simple3 () (setq count 0) (exec-state-machine (smach-simple3))) (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)) From 4f4bf276a4b3e5a8aaccd4472f0c00d07b4751b8 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 25 Feb 2021 15:16:23 +0900 Subject: [PATCH 7/7] extend test time limit to 120 sec for test-samples.l --- roseus_smach/test/test_samples.test | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/roseus_smach/test/test_samples.test b/roseus_smach/test/test_samples.test index 2ea947900..f6e0da50d 100644 --- a/roseus_smach/test/test_samples.test +++ b/roseus_smach/test/test_samples.test @@ -1,5 +1,6 @@ + args="$(find roseus_smach)/test/test-samples.l" + time-limit="120" />