Merge pull request #717 from knorth55/smach-register-exit-signal

[roseus_smach] add exit-state in state-machine and execute exit-state when Ctrl-C

roseus_smach/sample/sample_smach_exit_state.launch

@@ -0,0 +1,14 @@
+<launch>
+  <arg name="gui" default="true" />
+  <arg name="userdata" default="true" />
+
+  <node name="exec_smach_exit_state" pkg="roseus_smach" type="state-machine-ros-sample.l"
+        args="&quot;(exec-smach-exit-state)&quot;" unless="$(arg userdata)" output="screen" />
+
+  <node name="exec_smach_userdata_exit_state" pkg="roseus_smach" type="state-machine-ros-sample.l"
+        args="&quot;(exec-smach-userdata-exit-state)&quot;" if="$(arg userdata)" output="screen" />
+
+  <node name="$(anon smach_viewer)" pkg="smach_viewer" type="smach_viewer.py"
+        if="$(arg gui)" />
+
+</launch>

roseus_smach/sample/state-machine-ros-sample.l

@@ -46,5 +46,8 @@
 (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))))
+(defun exec-smach-exit-state () (setq count 0) (exec-state-machine (smach-simple) nil :exit-state :BAR))
+(defun exec-smach-exit-state () (setq count 0) (exec-state-machine (smach-simple) nil :exit-state :BAR))
+(defun exec-smach-userdata-exit-state () (setq count 0) (exec-state-machine (smach-userdata) '((count . 1)) :exit-state :BAR))
 
-(warn ";;(exec-smach-simple)~%;;(exec-smach-nested)~%;;(exec-smach-userdata)~%")
+(warn ";;(exec-smach-simple)~%;;(exec-smach-nested)~%;;(exec-smach-userdata)~%;;(exec-smach-exit-state)~%;;(exec-smach-userdata-exit-state)~%")

roseus_smach/src/state-machine-ros.l

@@ -14,7 +14,7 @@
 (defclass state-machine-inspector
   :super propertied-object
   :slots (sm root-name srv-name init-tm state-counter structure-counter
-             groupname))
+             groupname exit-state exit-signal-hook))
 (defmethod state-machine-inspector
   (:init
    (sm-obj &key ((:root-name rn) "SM_ROOT") ((:srv-name sn) "/server_name") ((:groupname gp)))
@@ -149,6 +149,33 @@
      ;; TODO apply userdata
      (setq init-tm (ros::time-now))
      ))
+  (:exit-state () exit-state)
+  (:exit-signal-hook () exit-signal-hook)
+  (:register-exit-signal-hook
+    (es userdata exit-signal-hook-func)
+    (if (null (derivedp es state))
+      (setq es (send self :state-machine :node es)))
+    ;; update exit-state slot
+    (setq exit-state es)
+    (send self :put 'userdata userdata)
+    ;; we need to set exit-signal-hook in slot
+    ;; to avoid memory error, GC causes this error?
+    (setq exit-signal-hook
+          `(lambda-closure nil 0 0 (sig code)
+             (ros::ros-warn " Signal ~A called." unix::sigint)
+             (ros::ros-warn "Calling exit-signal-hook.")
+             (funcall ',exit-signal-hook-func (send ,self :get 'userdata))
+             (ros::ros-warn " Forcing transition to ~A." ,es)
+             (send ,self :exit-signal-hook sig code ,es (send ,self :get 'userdata))
+             ))
+    (unix:signal unix::sigint exit-signal-hook)
+    )
+  (:exit-signal-hook (sig code &optional es userdata)
+    (if (null es) (return-from :exit-signal-hook (exit sig)))
+    (send sm :active-state es)
+    (send self :publish-status userdata)
+    (unix:signal unix::sigint 'ros::roseus-sigint-handler)  ;; if C-c twice, kill roseus
+    )
   ;;
   ;; utility for users
   ;;

roseus_smach/src/state-machine-utils.l

@@ -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 exit-state
+                                (exit-signal-hook-func '(lambda-closure nil 0 0 (userdata))))
   "Execute state machine
 
 Args:
@@ -10,6 +12,8 @@ Args:
   :spin (bool, default: t): call (send insp :spin-once) every time before executing state if T, call nothing otherwise.
   :hz (integer, default: 1): rate of execution loop
   :iterate (t or function): Enable asking [Y/N] on each action execution if given t, if function is given, then it is called before execution. If function returns nil, next action is not executed. Otherwise the action is executed
+  :exit-state (state or symbol): register smach's exit state when catching sigint signal
+  :exit-signal-hook-func (function): custom function when calling exit-state. Due to https://github.com/jsk-ros-pkg/jsk_roseus/issues/666#issuecomment-843640159 issue, you need to use '(lambda-closure nil 0 0 (userdata) ... , instead of #'(lambda (userdata) ...
 
 Returns:
   the last active state
@@ -19,6 +23,8 @@ Returns:
     (send sm :reset-state)
     (send insp :publish-structure) ;; publish once and latch
     (apply #'send sm :arg-keys (union (send sm :arg-keys) (mapcar #'car mydata)))
+    (if exit-state
+      (send insp :register-exit-signal-hook exit-state mydata exit-signal-hook-func))
 
     (ros::rate hz)
     (while (ros::ok)

roseus_smach/src/state-machine.l

@@ -180,9 +180,10 @@
          (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'~%"
+     (warning-message 3 "State machine ~A '~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))
+                      (send-all active-state :name) (send-all (flatten trans-list) :name) (send-all next-active-state :name)
+                      (if userdata (format nil " with userdata '~A'" userdata) ""))
      (setq active-state (unique next-active-state))
      ret))