roseus_smach

This package includes euslisp implementation of state machine and smach.

requirements

• roseus
• smach
• smach_viewer
  • Optional for visualization.
  • You have to install this package manually:
    sudo apt install ros-$ROS_DISTRO-smach-viewer

sample

Sample codes are available on sample directory.

• rosrun roseus_smach state-machine-ros-sample.l
  • simple state machine

  rosrun smach_viewer smach_viewer.py
  

  rosrun roseus_smach state-machine-ros-sample.l
  (exec-smach-simple)
  

• nested state machine

  rosrun smach_viewer smach_viewer.py
  

  rosrun roseus_smach state-machine-ros-sample.l
  (exec-smach-nested)
  

• state machine with userdata

  rosrun smach_viewer smach_viewer.py
  

  rosrun roseus_smach state-machine-ros-sample.l
  (exec-smach-userdata)
  

• sample/parallel-state-machine-sample.l

  • state machine with parallel action execution

  rosrun smach_viewer smach_viewer.py
  

  rosrun roseus_smach parallel-state-machine-sample.l
  (demo)
  

Writing Simple Smach(state-machine)

Example codes are here.

#!/usr/bin/env roseus
(load "package://roseus_smach/src/state-machine.l")

(setq count 0)
(defun func-foo (userdata-alist)
  (format t "Execute state FOO~%")
  (cond ((< count 3) (incf count) :outcome1)
	(t :outcome2)))
(defun func-bar (userdata-alist)
  (format t "Execute state BAR~%")
  :outcome2)

(defun smach-simple ()
  (let ((sm (instance state-machine :init)))
    (send sm :add-node (instance state :init :FOO 'func-foo))
    (send sm :add-node (instance state :init :BAR 'func-bar))
    ;; goal-states are generated in this method
    (send sm :goal-state (list :outcome4 :outcome5))

    ;; select a node as start-node
    (send sm :start-state :FOO)
    ;; from and to nodes are selected by name or symbol
    (send sm :add-transition :FOO :BAR :outcome1)
    (send sm :add-transition :FOO :outcome4 :outcome2)
    (send sm :add-transition :BAR :FOO :outcome2)
    sm ))

(send (smach-simple) :execute nil)

The Code Explained

(load "package://roseus_smach/src/state-machine.l")

This line imports state-machine class, state class, and transition class.

(defun func-foo (userdata-alist)
  (format t "Execute state FOO~%")
  (cond ((< count 3) (incf count) :outcome1)
	(t :outcome2)))
(defun func-bar (userdata-alist)
  (format t "Execute state BAR~%")
  :outcome2)

These lines define some functions that will be hooked to state. Note that the functions are called with one argument, an alist of the declared user-data arguments, and that their return value is equal to the transition (or the action) of the state.

(sm (instance state-machine :init))

This line creates the state-machine instance and binds it to sm.

(send sm :add-node (instance state :init :FOO 'func-foo))

This line creates a new state and add the node to the state machine. When you create new state instance, you can name the state and hook the state to a function. Any lisp object can be used for state names. In this case, a new state has name :FOO and hooked with func-foo.

(send sm :goal-state (list :outcome4 :outcome5))
(send sm :start-state :FOO)

These lines define goal state(s) and start state(s) to the state machine. You can create multiple start states as a list of state, also you can define multiple goal state. Goal state(s) do nothing and returns its name. Note that a state machine is also a state. So the goal state can be considered as the outcome of the state machine.

(send sm :add-transition :FOO :BAR :outcome1)
 

This line defines transition. The arguments of :add-transition method is TO, FROM, OUTCOME. So this line means that “adding transition from FOO to BAR, when FOO node returns outcome1”.

(send (smach-simple) :execute nil)

This line sets up and execute the state machine. In order to view and check the progress with smach_viewer, use the function exec-state-machine instead.

Writing Nested Smach

Example codes are here. You can also add child state-machine as a node to state-machine.

#!/usr/bin/env roseus
(load "package://roseus_smach/src/state-machine.l")
(defun smach-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))
          (bar-node (instance state :init "BAR" 'func-bar)))
      (send sm-sub :add-node foo-node)
      (send sm-sub :add-node bar-node)
      (send sm-sub :goal-state :outcome4)
      (send sm-sub :start-state foo-node)
      (send sm-sub :add-transition foo-node bar-node :outcome1)
      (send sm-sub :add-transition foo-node :outcome4 :outcome2)
      (send sm-sub :add-transition bar-node foo-node :outcome2))

    sm-top ))

(send (smach-nested) :execute nil)

The Code Explained

#!/usr/bin/env roseus
(load "package://roseus_smach/src/state-machine.l")

Same as in the previous example, this line imports state-machine class, state class, and transition class.

  (let ((sm-top (instance state-machine :init))
        (sm-sub (instance state-machine :init)))

This line creates two of state-machine instance. The sm-sub is a state machine but also acting like a state (or a node) in sm-top. This is easy to understand if you think that a state machine as a function: when the state machine is called, or executed, state machine does some processing and eventually returns the goal state as its return value just like a function.

    (send sm-top :add-node (instance state :init "SUB" sm-sub))

The sm-sub instance is hooked as a node in sm-top with name of "SUB" in this line. This line also indicates that you can add a state machine as a state just like a function.

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

These lines define another node, goal-state, start-state, and transition in the sm-top.

    (send sm-top :add-transition "SUB" :outcome5 :outcome4)

Remember that the goal state(s) of state-machine does nothing and returns its name. So the outcome (or the return value) of the sm-sub is its name of goal state(s). If you look further, the goal state of sm-sub is :outcome4, so the return value of sm-sub is :outcome4. Therefore this line adds transition of from "SUB" to :outcome5 when "SUB" node returns :outcome4.

  (let ((foo-node (instance state :init "FOO" 'func-foo))
        (bar-node (instance state :init "BAR" 'func-bar)))
      (send sm-sub :add-node foo-node)
      (send sm-sub :add-node bar-node)
      (send sm-sub :goal-state :outcome4)
      (send sm-sub :start-state foo-node)
      (send sm-sub :add-transition foo-node bar-node :outcome1)
      (send sm-sub :add-transition foo-node :outcome4 :outcome2)
      (send sm-sub :add-transition bar-node foo-node :outcome2))

These lines define the behavior of sm-sub in detail just like the previous simple state machine example. Note that (send sm-sub :goal-state :outcome4) not only defines the goal state, but also defines the return value of its state machine.

(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 can be written as

(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.

(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`

Writing Nested Smach with (make-state-machine)

You can also write nested state machine with make-state-machie function.

you can add sub state-machine to function maps, as you already added node to them.

(defun smach-simple-nested ()
  (let (sm-top sm-sub)
    (setq sm-sub
          (make-state-machine
           '((:foo :outcome2 :outcome4) ;; transitions
             (:foo :outcome1 :bar)
             (:bar :outcome2 :foo))
           '((:foo 'func-foo) ;; function maps
             (:bar 'func-bar))
           '(:foo)      ;; initial
           '(:outcome4) ;; goal
           ))
    (setq sm-top
          (make-state-machine
           '((:bas :outcome3 :sub) ;; transitions
             (:sub :outcome4 :outcome5))
           `((:bas 'func-bas)  ;; functon maps
             (:sub ,sm-sub))   ;; set "nestaed state machine"
           '(:bas)      ;; initial
           '(:outcome5) ;; goal
           ))
    sm-top))

From roseus >= 1.7.4, you may write

           '((:bas 'func-bas)  ;; functon maps
             (:sub sm-sub))   ;; set "nestaed state machine"

but as for now, please be very careful when you add sub machine. you need to use `(backquote) and , (comma).

This example can be tested with

$ roscd roseus_smach/sample
$ roseus state-machine-ros-sample.l "(progn (setq count 0)(exec-state-machine (smach-simple-nested)))"

Changelog for package roseus_smach

1.7.5 (2021-12-13)

• [roseus_smach] add :append-goal-state (#696)

• add nested example with make-state-machine function (#661)

  • add readme on smach-simple-nested

  * use defmacro instead of defun for make-simple-state without this, we need to write ` (setq sm-top (make-state-machine '((:bas :outcome3 :sub) ;; transitions (:sub :outcome4 :outcome5)) `((:bas 'func-bas) ;; functon maps (:sub ,sm-sub)) ;; set "nestaed state machine" '(:bas) ;; initial '(:outcome5) ;; goal ))[ to avoid ]{.title-ref}/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl unittest-error: unbound variable sm-sub in (eval (get-alist node func-map)), exitting...[ errors, this change enable us to write intuitive way ]{.title-ref}'((:bas 'func-bas) ;; functon maps (:sub sm-sub)) ;; set "nestaed state machine"[ you can test this behavior with ]{.title-ref}(defmacro test (l) `(dolist (e ,l) (print e) (print (eval (cadr e))))) (defmacro test (l) (dolist (e l) (print e) (print (eval (cadr e))))) (let (a) (setq a 10) (test '((:foo 1) (:bar a))))`

• [roseus_smach] make roseus_smach execution faster (#684)
  • sleep 0.5 s for publisher
  • use ros::sleep for smach execution
• [roseus_smach] add more ros-info in convert-smach (#683)
• [roseus_smach] fix typo in state-machine-utils.l (#693)
• [roseus_smach] add :start-state and :goal-state in convert-smach (#682)
• [roseus_smach] add groupname in state-machine-inspector (#691)
  • use send sm :spin-once in state-machine-utils.l
  • add groupname and spin-once with groupname
• Updates to README sample code and explanations (#659)
  • extend test time limit to 120 sec for test-samples.l
  • add example/test for smach-simple with make-state-machine function
  • write more info similar to rospy implementation
  • add more info(URL/python code) on sample
  • fix typo of nestate state machine example path
  • add make-sample-parallel-state-machine tests
  • add more test, check :active-tates, duration time
• [roseus_smach] use roseus for parallel-state-machine-sample (#651)
• [roseus_smach] add smach_viewer installation to README (#641)
• [roseus_smach] add code explanation of simple-state-machine in README.md. (#627)
• Contributors: Guilherme Affonso, Kei Okada, Naoki Hiraoka, Naoya Yamaguchi, Shingo Kitagawa, Yoichiro Kawamura

1.7.4 (2019-02-04)

1.7.3 (2019-02-01)

• check why test-smach-action-client-state failing on installed test (#570 )
• fix make-state-machine docstring (#589 )
• re-enable test-smach-action-client-state, which is removed in https://github.com/jsk-ros-pkg/jsk_roseus/pull/567#issuecomment-406841511
• Add issue link in make-state-machine docstring (#595 )
• roseus_smach: change the order of callee args for userdata in pddl2smach (#587)
• roseus_smach: pass userdata values unless :arg-keys is set (#586)
  • roseus_smach: fix error on exec state machine without ri
• Contributors: Guilherme Affonso, Yuki Furuta, Shingo Kitagawa

1.7.2 (2018-11-10)

1.7.1 (2018-07-22)

• add melodic test (#567)
  • split test_samples.test into test_samples_action_client_state.test, since it is always failling on only installed test https://api.travis-ci.org/v3/job/406576370/log.txt, not sure why...
  • roseus_smach/test/test_samples.test: extend time-limit to 300

  * add retry for testtest_roseus_smach_samples the test failing https://api.travis-ci.org/v3/job/406540328/log.txt is this related to memory leak? https://github.com/jsk-ros-pkg/jsk_roseus/pull/563 ` mstart testing [test-smach-sample-userdata] mfoo-count is not set. Setting 0 start testing [test-smach-action-client-state] m;p=pointer?(0x6690338) ;; Segmentation Fault. terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::lock_error> >' what(): boost: mutex lock failed in pthread_mutex_lock: Invalid argument [Testcase: testtest_roseus_smach_samples] ... FAILURE! FAILURE: test [test_roseus_smach_samples] did not generate test results File \"/usr/lib/python2.7/unittest/case.py\", line 329, in run testMethod() File \"/opt/ros/kinetic/lib/python2.7/dist-packages/rostest/runner.py\", line 164, in fn self.assert\_(os.path.isfile(test_file), \"test [%s] did not generate test results\"%test_name) File \"/usr/lib/python2.7/unittest/case.py\", line 422, in assertTrue raise self.failureException(msg)`

• Contributors: Kei Okada

1.7.0 (2018-07-11)

• Bugfixes and test codes for roseus_smach (#566)
  • roseus_smach: add log messages on state transition
  • roseus_smach: fix: pass :cancel state to action-client-state
  • roseus_smach: fix test
  • roseus_smach: add test code for smach-actionlib
  • • Fix: indentations
    • Fix: [bug] userdata is not kept if not given as arguments
    • Add: Test code for action-client-state class
    • Add: action-client-state sets action result/feedback to userdata for key :result/:feedback
• [roseus_smach] func: make-state-machine accepts various edges (#548)
  • correct doc of :add-transition :add-transition do not accept list as exec-result
  • update make-state-machine docstring
  • set testfunc for transition in make-state-machine
  • func: make-state-machine accepts various edges
• [roseus_smach] pass userdata keys to state-machine in execution (#549)
  • add exec-state-machine test
  • pass userdata keys to state-machine in execution
• Contributors: Shingo Kitagawa, Yuki Furuta

1.6.3 (2017-09-08)

1.6.2 (2017-06-21)

• replace ros-info by ros-debug in state-machine.l (#523)
• add root-name key in exec-state-machine (#523)
  • use exex-state-machine in sample program exec-smach-with-spin is deprecated.
  • add exec-state-machine with :root-name key test
  • add root-name key in exec-state-machine
• Contributors: Shingo Kitagawa

1.6.1 (2017-03-15)

• Merge smach exec (#507)
  • [roseus_smach] rename smach-exec-with-spin -> exec-state-machine
  • [roseus_smach/src/pddl2smach.l] use function namespace to call
• Contributors: Yuki Furuta

1.6.0 (2016-10-02)

• [roseus/src/state-machine-utils.l] add document string for exec-smach-with-spin
• [roseus_smach/src/state-machine-utils.l] support y-or-n-p when iterate mode
• Contributors: Yuki Furuta

1.5.3 (2016-05-28)

1.5.2 (2016-05-28)

• [roseus_smach/src/state-machine-utils.l] fix: return after goal reached on exec-smach-with-spin #460
• Remove no need euslisp from build_depend and find_package #456
• Contributors: Kentaro Wada, Yuki Furuta

1.5.1 (2016-04-22)

• [roseus_smach/README.md] update to use github official image link
• Contributors: Furushchev

1.5.0 (2016-03-20)

• {roseus_smach, roseus_mongo}/README.md: fix section/subsection
• [roseus_smach/src/state-machine-utils.l] fix: smach connection from/to nil state
• Contributors: Kei Okada, Yuki Furuta

1.4.1 (2015-11-25)

• [roseus_smach/src/state-machine.l] another impl for #383
• Contributors: Yuki Furuta

1.4.0 (2015-11-03)

• [roseus_smach] warning message for :goal-state
• [roseus_smach] add failure nodes addition feature for state-machine generation utils
• [roseus_smach/sample/parallel-state-machine-sample.l] fix: https://github.com/jsk-ros-pkg/jsk_roseus/issues/324
• [roseus_smach] fix test launch file extension .launch -> .test; test only required
• Contributors: Kamada Hitoshi, Yuki Furuta

1.3.9 (2015-09-14)

1.3.8 (2015-09-12)

1.3.7 (2015-08-18)

• [README.md] describe how to run smach viewer
• [sample/state-machine-ros-sample.l, sample/state-machine-sample.l] add shbang
• [roseus_smach/CMakeLists.txt] disable test/test_parallel_state_machine_sample.launch for now
• [package.xml] add actionlib_tutorials depends to roseus_smach
• Contributors: Kei Okada

1.3.6 (2015-06-11)

1.3.5 (2015-05-15)

1.3.4 (2015-05-03)

• [roseus_smach] add test for parallel state machine
• [roseus_smach] add parallel state machine sample test to CMakeLists
• [roseus_smach] add sample test to CMakeLists
• [roseus_smach] split sample test in order to inspect failure detail
• [roseus_smach] change order of roseus in find_package
• [roseus_smach] move smach-exec function from sample to utils
• [roseus_smach] fix wrong file/module name
• [roseus_smach] miscellaneous fixes
• [roseus_smach] add test launch
• [roseus_smach] add feature async join
• [roseus_smach] fix transition fail when parallel state
• [roseus_smach] add async join state to state-machine
• [roseus_smach] fix tmp -> next
• [roseus_smach] modify state-machine :execute-impl
• Contributors: Yuki Furuta, Kamada Hitoshi

1.3.3 (2015-04-29)

1.3.2 (2015-04-28)

1.3.1 (2015-04-26)

• [roseus_smach/src/state-machine-actionlib.l] support spin action client group, see #274
• [roseus_smach/src/state-machine-utils.l] modify make-state-machine
• [roseus_smach/src/state-machine-utils.l] add iterative execute state machine util
• Contributors: Yuki Furuta, Hitoshi Kamada, Kei Okada

1.3.0 (2015-04-24)

• [roseus_smach] add docstring for 'make-state-machine' function; add key option to custom exec-result to transit states
• [roseus_smach] fix sample parallel task transition; fix typo
• [roseus_smach] fix typo; change image link
• [roseus_smach] Create README.md add sample image
• [roseus_smach] add syntax suggar of creating state machine with parallel execution, and its sample code
• [roseus_smach] add feature: parallel executive state machine, and its visualization stuff
• [roseus_smach] use soft tab
• Contributors: Yuki Furuta

1.2.6 (2015-02-21)

1.2.5 (2015-02-13)

1.2.4 (2015-02-12)

1.2.3 (2015-02-02)

1.2.2 (2015-01-27)

1.2.1 (2015-01-27)

1.2.0 (2015-01-26)

1.1.33 (2015-01-26)

1.1.32 (2015-01-26)

1.1.31 (2015-01-23)

• remove old manifest.xml, fully catkinize
• Contributors: Kei Okada

1.1.30 (2015-01-14)

1.1.29 (2014-12-27)

1.1.28 (2014-12-26)

1.1.27 (2014-12-20)

• remove cmake file for rosbuild
• not use executive_smach as deps directly; remove manifest.xml

1.1.26 (2014-11-10)

1.1.25 (2014-10-10)

1.1.23 (2014-09-24)

1.1.22 (2014-09-04)

1.1.21 (2014-06-30)

1.1.20 (2014-06-29)

1.1.19 (2014-06-11)

1.1.18 (2014-05-16)

1.1.17 (2014-05-11)

1.1.16 (2014-05-11)

1.1.15 (2014-05-10)

1.1.14 (2014-05-09)

1.1.13 (2014-05-06)

1.1.12 (2014-05-06)

1.1.11 (2014-05-04)

• catkinize roseus_smach
• Contributors: Kei Okada

1.1.10 (2014-05-03 10:35)

1.1.9 (2014-05-03 09:30)

1.1.8 (2014-05-02)

1.1.7 (2014-04-28 14:29)

1.1.6 (2014-04-28 03:12)

1.1.5 (2014-04-27)

1.1.4 (2014-04-25)

1.1.3 (2014-04-14)

1.1.2 (2014-04-07 23:17)

1.1.1 (2014-04-07 09:02)

1.1.0 (2014-04-07 00:52)

1.0.4 (2014-03-31)

1.0.3 (2014-03-30)

1.0.2 (2014-03-28)

1.0.1 (2014-03-27)

• roseus_smach: disable packages for groovy
• publish-all-status to state-machine-inspector
• use-sub-machine to pddl2smach.l
• modify :reset-state for setting typical state
• add keywords to pddl-graph-to-smach
• add smach utility functions
• fix default option
• add utility methods to state-machine-inspector
• add keyword for using userdata in pddl2smach
• add keyword for changing return value
• add :readable keyword for pddl2smach
• fix for working sample
• remove load command for irtgraph.l
• update internal data structure for new graph.l
• publish smach structure once, and latch it
• add test for roseus_smach samples, fixed the initial state setter method
• update roseus_smach for set initial state callback
• add actionlib_tutorials for sample scripts
• changed to use unreviewed version of irtgraph.l
• fix smach_structure publish properly timing, add user input action to task_compiler
• remove old method in roseus_smach
• move convert script from pddl to smach
• chenge test function to compare execution results
• commit for current scripts for demonstration
• use package:// for loading graph.l
• change test function for transition, eq -> equal
• set initial-state = send :start-state
• add initial-state-cb to roseus_smach
• add message name to constant in msg definition
• add ** to msg constant type
• add function to create state-instance which execute action-client
• commit current source tree
• add code for smach_viewer
• change name smach_roseus -> roseus_smach
• Contributors: Kei Okada, youhei, Manabu Saito, Xiangyu Chen