enable to publish info, currently we publish node's function document, or node name

Author: k-okada

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

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

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

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)