From cc7fcc4698a921ddea99e1e6b44438f60d1e9070 Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Wed, 20 Jul 2022 09:09:44 +0900
Subject: [PATCH] enable to publish info, currently we publish node's function
 document, or node name

---
 roseus_smach/sample/state-machine-sample.l |  2 +-
 roseus_smach/src/state-machine-ros.l       |  3 ++-
 roseus_smach/src/state-machine-utils.l     | 17 +++++++++++++++--
 roseus_smach/src/state-machine.l           |  2 ++
 4 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/roseus_smach/sample/state-machine-sample.l b/roseus_smach/sample/state-machine-sample.l
index c86bda399..9a11e6916 100755
--- a/roseus_smach/sample/state-machine-sample.l
+++ b/roseus_smach/sample/state-machine-sample.l
@@ -11,7 +11,7 @@
 ;;      https://raw.githubusercontent.com/rhaschke/executive_smach_tutorials/indigo-devel/examples/state_machine_simple.py
 ;;
 (setq count 0)
-(defun func-foo (&rest args)
+(defun func-foo (&rest args) "document foo"
   (format t "Execute state FOO~%")
   (cond ((< count 3) (incf count) :outcome1)
 	(t :outcome2)))
diff --git a/roseus_smach/src/state-machine-ros.l b/roseus_smach/src/state-machine-ros.l
index 983d6d223..d6eed8c78 100644
--- a/roseus_smach/src/state-machine-ros.l
+++ b/roseus_smach/src/state-machine-ros.l
@@ -43,7 +43,7 @@
   (:state-machine (&rest args) (forward-message-to sm args))
   ;;
   (:publish-status
-   (&optional (userdata nil) (machine sm) (path (format nil "/~A" root-name)))
+   (&optional (userdata nil) (machine sm) (path (format nil "/~A" root-name)) (info))
    (let ((msg (instance smach_msgs::SmachContainerStatus :init))
          (initial (send-all (flatten (list (send machine :start-state))) :name))
          (active (send-all (flatten (list (send machine :active-state))) :name)))
@@ -54,6 +54,7 @@
      (send msg :initial_states (mapcar #'string initial))
      (send msg :active_states (mapcar #'string active))
      (send msg :local_data (pickle::dump userdata))
+     (send msg :info info)
      (ros::publish (concatenate string srv-name "/smach/container_status") msg)
      ;; recursive call for publish active sub-machines
      (dolist (active-state (flatten (list (send machine :active-state))))
diff --git a/roseus_smach/src/state-machine-utils.l b/roseus_smach/src/state-machine-utils.l
index b76e268f8..e59a5c934 100644
--- a/roseus_smach/src/state-machine-utils.l
+++ b/roseus_smach/src/state-machine-utils.l
@@ -18,7 +18,7 @@ Args:
 Returns:
   the last active state
 "
-  (let ((insp (instance state-machine-inspector :init sm :root-name root-name :srv-name srv-name)))
+  (let ((insp (instance state-machine-inspector :init sm :root-name root-name :srv-name srv-name)) acs act doc)
     (unix::usleep (round (* 0.5 1e6)))
     (send sm :reset-state)
     (send insp :publish-structure) ;; publish once and latch
@@ -31,7 +31,20 @@ Returns:
       (when spin
         (send insp :spin-once)
         (if (and (boundp '*ri*) *ri*) (send *ri* :spin-once)))
-      (send insp :publish-status mydata)
+      (setq acs (send sm :active-state))
+      (if (atom acs) (setq acs (list acs)))
+      (setq doc "")
+      (dolist (a acs)
+        (setq act (send a :action))
+        (setq d
+                (cond ((and (functionp act) (not (listp act)))
+                       (documentation act))
+                      ((and (listp act) (eq (car act) 'lambda))
+                       (format nil "~A ~A..." (butlast act) (if (listp (car (last act))) (subseq (car (last act)) 0 3) act)))
+                      (t
+                       (format nil "~A" act))))
+        (push (cond ((stringp d) d) (t (string (send (send sm :active-state) :name)))) doc))
+      (send insp :publish-status mydata sm (format nil "/~A" root-name) (apply #'concatenate string doc))
       (when (send sm :goal-reached)
         (return))
       (when iterate
diff --git a/roseus_smach/src/state-machine.l b/roseus_smach/src/state-machine.l
index 318e528b8..bbcdd2bd6 100644
--- a/roseus_smach/src/state-machine.l
+++ b/roseus_smach/src/state-machine.l
@@ -221,6 +221,8 @@
    (send-super :init name))
   ;; check if this state is state-machine, then return it
   (:submachine () (if (derivedp action state-machine) action nil))
+  ;; accessor to action
+  (:action (&rest args) (forward-message-to action args))
   ;; remap userdata names
   (:remap-list
    (&rest args)