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

[knorth55]

I add exit-state and register-signal-exit-state to execute exit-state when Ctrl-C pressed. You can try with this launch + diff

<launch>
  <node pkg="roseus_smach" type="state-machine-ros-sample.l" name="state_machine_ros_sample"
        args="&quot;(exec-smach-simple)&quot;" output="screen" />
</launch>

diff --git a/roseus_smach/sample/state-machine-ros-sample.l b/roseus_smach/sample/state-machine-ros-sample.l
index f6b24e3..95da645 100755
--- a/roseus_smach/sample/state-machine-ros-sample.l
+++ b/roseus_smach/sample/state-machine-ros-sample.l
@@ -43,7 +43,7 @@

     sm ))

-(defun exec-smach-simple () (setq count 0) (exec-state-machine (smach-simple)))
+(defun exec-smach-simple () (setq count 0) (exec-state-machine (smach-simple) nil :exit-state :bar))
 (defun exec-smach-nested () (setq count 0) (exec-state-machine (smach-nested)))
 (defun exec-smach-userdata () (exec-state-machine (smach-userdata) '((count . 1))))

$ roslaunch ./spam.launch
... logging to /home/knorth55/.ros/log/961a5da6-097b-11ed-b140-507b9d9efafc/roslaunch-melodic-p50-10801.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/knorth55/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://melodic-p50:42833/

SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.12

NODES
  /
    state_machine_ros_sample (roseus_smach/state-machine-ros-sample.l)

auto-starting new master
process[master]: started with pid [10818]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 961a5da6-097b-11ed-b140-507b9d9efafc
process[rosout-1]: started with pid [10836]
started core service [/rosout]
process[state_machine_ros_sample-2]: started with pid [10843]
configuring by "/home/knorth55/install/jskeus/eus/lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph gnuplotlib ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb
;; extending gcstack 0x559bc0875690[16374] --> 0x559bc0cfdb20[32748] top=3d49
irtgl irtglc irtviewer
EusLisp 9.29(0033f2e 1d16d77c) for Linux64 created on melodic-p50(Thu Jul 14 17:35:16 JST 2022)
roseus ;; loading roseus("1.7.5-13-gdd8d2c5") on euslisp((9.29 melodic-p50 Thu Jul 14 17:35:16 JST 2022 0033f2e 1d16d77c))
[ INFO] [1658466194.244351543]: install ros::roseus-sigint-handler
eustf roseus_c_util ;;(smach-simple)
;;(smach-nested)
;;(smach-userdata)
;;(exec-smach-simple)
;;(exec-smach-nested)
;;(exec-smach-userdata)
Executing state :foo
Execute state FOO
State machine transitioning '(:foo)' :'(:outcome1)' --> '(:bar)'
Executing state :bar
Execute state BAR
State machine transitioning '(:bar)' :'(:outcome2)' --> '(:foo)'
^C[state_machine_ros_sample-2] killing on exit
[ERROR] [1658466197.011942277]: exit-signal-hook called.

Executing state :bar
Execute state BAR
State machine transitioning '(:bar)' :'(:outcome2)' --> '(:foo)'
[ERROR] [1658466197.015568140]: exit-signal-hook finished.

[ INFO] [1658466197.015636078]: cell* ROSEUS_EXIT(context*, int, cell**)
[ INFO] [1658466197.015674319]: exiting roseus 2
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

cc. @k-okada @Affonso-Gui @kochigami

[Affonso-Gui]

Do we need the state machine slot? In a quick look :register-exit-signal-hook and :exit-signal-hook seem to be auto sufficient.

[knorth55]

@Affonso-Gui I modify this PR and keep state-machine.l unchanged, and add exit-state slot in inspector. I also add sample_smach_exit_code.launch to try this PR.

$ roslaunch roseus_smach sample_smach_exit_state.launch gui:=false
... logging to /home/knorth55/.ros/log/beff4a9a-097d-11ed-b140-507b9d9efafc/roslaunch-melodic-p50-18162.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/knorth55/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://melodic-p50:42359/

SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.12

NODES
  /
    exec_smach_exit_state (roseus_smach/state-machine-ros-sample.l)

auto-starting new master
process[master]: started with pid [18179]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to beff4a9a-097d-11ed-b140-507b9d9efafc
process[rosout-1]: started with pid [18197]
started core service [/rosout]
process[exec_smach_exit_state-2]: started with pid [18204]
configuring by "/home/knorth55/install/jskeus/eus/lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph gnuplotlib ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb
;; extending gcstack 0x5593c817a690[16374] --> 0x5593c8602b20[32748] top=3d49
irtgl irtglc irtviewer
EusLisp 9.29(0033f2e 1d16d77c) for Linux64 created on melodic-p50(Thu Jul 14 17:35:16 JST 2022)
roseus ;; loading roseus("1.7.5-13-gdd8d2c5") on euslisp((9.29 melodic-p50 Thu Jul 14 17:35:16 JST 2022 0033f2e 1d16d77c))
[ INFO] [1658467122.065117526]: install ros::roseus-sigint-handler
eustf roseus_c_util ;;(smach-simple)
;;(smach-nested)
;;(smach-userdata)
;;(exec-smach-simple)
;;(exec-smach-nested)
;;(exec-smach-userdata)
;;(exec-smach-exit-state)
Executing state :foo
Execute state FOO
State machine transitioning '(:foo)' :'(:outcome1)' --> '(:bar)'
Executing state :bar
Execute state BAR
State machine transitioning '(:bar)' :'(:outcome2)' --> '(:foo)'
Executing state :foo
Execute state FOO
State machine transitioning '(:foo)' :'(:outcome1)' --> '(:bar)'
^C[exec_smach_exit_state-2] killing on exit
[ERROR] [1658467125.852376043]: exit-signal-hook called.

Executing state :bar
Execute state BAR
State machine transitioning '(:bar)' :'(:outcome2)' --> '(:foo)'
[ERROR] [1658467125.856063517]: exit-signal-hook finished.

[ INFO] [1658467125.856129872]: cell* ROSEUS_EXIT(context*, int, cell**)
[ INFO] [1658467125.856169068]: exiting roseus 2
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

[k-okada]

I think current implementation fails when we have userdata. Please check (exec-state-machine (smach-userdata) '((count . 1)) :exit-state :foo) and see https://github.com/knorth55/jsk_roseus/pull/1/commits/79cc0cf0c6b4643bd35fd3a8ac788bd487e007f3 : for fix

https://github.com/knorth55/jsk_roseus/pull/1/commits/549968ed3644bf13821f75c6ac09015fc3e0deea : is warning-message improvement

please check my comment on https://github.com/knorth55/jsk_roseus/pull/1 @knorth55, I think just to transit to specified state is not sufficient, we want to transit from that state to original goal, see https://github.com/knorth55/jsk_roseus/pull/1/commits/20319c5e5e2a92b0a2c1908b8a6517a786a8dcd1,

[k-okada]

this does not work exit-signal-hook-func does not work if we subscribe topics. More precisely it works if we subscribe container_status, but not working with image topic... I haven't tested with other topics. Any idea ? @knorth55 @Affonso-Gui

;; this is ok...                                                                                        
(ros::roseus-add-msgs "smach_msgs")
(ros::subscribe "/my_smach_introspection_server/smach/container_status"
                smach_msgs::SmachContainerStatus
                #'(lambda (msg) (print msg)))

;; this is ng                                                                                           
(ros::roseus-add-msgs "sensor_msgs")
(ros::subscribe "/smach_viewer/image"
                sensor_msgs::Image
                #'(lambda (msg) (print msg)))

(defun exec-smach-userdata-exit-state () (setq count 0) (exec-state-machine (smach-userdata) '((count . 1)) :exit-state :BAR :exit-signal-hook-func '(lambda-closure nil 0 0 (userdata))))

[Affonso-Gui]

exit-signal-hook-func does not work if we subscribe topics. More precisely it works if we subscribe container_status, but not working with image topic... I haven't tested with other topics. Any idea ? @knorth55 @Affonso-Gui

@k-okada Does it work with this? https://github.com/Affonso-Gui/EusLisp/commit/f62b5ce4789b000115ed2ba5d7536beee45aa184

[knorth55]

@k-okada the error may be caused by GC, probably. I add exit-singal-hook slot and (setq exit-signal-hook ~~~) to avoid the GC error.

1e482f9 (#717)