Merge pull request #659 from k-okada/add_more_test_readme

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
   ```
@@ -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 (<from-node> <transition> <to-node>)
+           ;; if <transition> 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`

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)
@@ -32,8 +34,39 @@
     (send sm :add-transition :BAR :FOO :outcome2)
     sm ))
 
+(defun smach-simple2 ()
+  (let (sm)
+    (setq sm
+          (make-state-machine
+           ;; define graph,  list of (<from-node> <transition> <to-node>)
+           ;; if <transition> 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
+;;      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 +99,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~%")

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))
 

roseus_smach/test/test-samples.l

@@ -1,25 +1,55 @@
 (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")
 
+
+(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"))
+(defun exec-sample-parallel-state-machine ()
+  (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)))
 
-(deftest test-smach-sample-nested ()
-  (assert (eq (send (exec-smach-nested) :name) :outcome5)
-	  "nested smach sample"))
+(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)))
 
 (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"))
 
+(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)

roseus_smach/test/test_samples.test

@@ -1,5 +1,6 @@
 <launch>
   <node name="fibonacci_server" pkg="actionlib_tutorials" type="fibonacci_server"/>
   <test test-name="test_roseus_smach_samples" pkg="roseus" type="roseus"
-        args="$(find roseus_smach)/test/test-samples.l" />
+        args="$(find roseus_smach)/test/test-samples.l"
+        time-limit="120" />
 </launch>