[WIP] [Fetch] Enable to publish info, currently we publish node's function document or node name

.gitmodules

@@ -1,3 +1,6 @@
 [submodule ".travis"]
 	path = .travis
 	url = https://github.com/jsk-ros-pkg/jsk_travis
+[submodule "roseus_bt/include/rosbridgecpp"]
+	path = roseus_bt/include/rosbridgecpp
+	url = https://github.com/ppianpak/rosbridgecpp

roseus/cmake/roseus.cmake

@@ -183,7 +183,7 @@ macro(generate_eusdoc _lispfile)
   get_filename_component(_name ${_lispfile} NAME_WE)
   set(_lisppath "${CMAKE_CURRENT_SOURCE_DIR}/${_lispfile}")
   set(_mdfile "${_name}.md")
-  set(_generate_eusdoc_command "\\\"(setq lisp::*error-handler* 'exit)\\\" \\\"(load \\\\\\\"${_lisppath}\\\\\\\")\\\" \\\"(make-document \\\\\\\"${_lisppath}\\\\\\\" \\\\\\\"${_mdfile}\\\\\\\" \\\\\\\"${_pkg_name}\\\\\\\")\\\" \\\"(exit)\\\" ")
+  set(_generate_eusdoc_command "\\\"(install-handler error #'(lambda (c) (lisp::print-error-message c) (exit 1)))\\\" \\\"(load \\\\\\\"${_lisppath}\\\\\\\")\\\" \\\"(make-document \\\\\\\"${_lisppath}\\\\\\\" \\\\\\\"${_mdfile}\\\\\\\" \\\\\\\"${_pkg_name}\\\\\\\")\\\" \\\"(exit)\\\" ")
   separate_arguments(_generate_eusdoc_command_list WINDOWS_COMMAND "${_generate_eusdoc_command}")
   #set(_roseus_exe roseus)
   find_program(_roseus_exe roseus)

roseus/euslisp/roseus-utils.l

@@ -6,40 +6,31 @@
 (ros::roseus-add-srvs "dynamic_reconfigure")
 (ros::roseus-add-msgs "dynamic_reconfigure")
 
-(defun print-ros-msg (msg &optional (rp (find-package "ROS"))
-                          (ro ros::object) (nest 0) (padding "  "))
+(defun print-ros-msg (msg &optional (strm *standard-output*) (tab 0) (padding "  "))
+  (assert (streamp strm) (format nil "Stream Expected in: ~s~%" strm))
   (dolist (s (let ((slt (send msg :slots)))
                ;; `s' is a list of slots unique to the msg object
                ;; i.e. slots present in `ro' and inherited by `msg' are filtered
                (map nil #'(lambda (obj) (setq slt (remove obj slt :key #'car :count 1)))
-                    (send ro :slots))
+                    (send ros::object :slots))
                slt))
     (let ((sm (car s)))
-      (when (and (symbolp sm) (eq (symbol-package sm) rp))
+      (when (symbolp sm)
+        (dotimes (i tab) (format strm padding))
+        (format strm "~A: " (string-downcase (string-left-trim "_" (symbol-string sm))))
         (cond
-         ((derivedp (cdr s) ro)
-          (dotimes (i nest) (format t padding))
-          (format t ":~A -- ~A~%"
-                  (string-downcase (string-left-trim "_" (symbol-string sm)))
-                  (send (class (cdr s)) :name))
-          (print-ros-msg (cdr s) rp ro (1+ nest) padding))
+         ((derivedp (cdr s) ros::object)
+          (terpri strm)
+          (print-ros-msg (cdr s) strm (1+ tab) padding))
          ((and (listp (cdr s))
-               (derivedp (cadr s) ro))
-          (dotimes (i nest) (format t padding))
-          (format t ":~A [ ~A ]~%"
-                  (string-downcase (string-left-trim "_" (symbol-string sm)))
-                  (send (class (cadr s)) :name))
-          (dotimes (i (1+ nest)) (format t padding)) (format t "[~%")
+               (derivedp (cadr s) ros::object))
+          (terpri strm)
           (dolist (ss (cdr s))
-            (print-ros-msg ss rp ro (+ nest 2) padding))
-          (dotimes (i (1+ nest)) (format t padding)) (format t "]~%")
-          )
+            (dotimes (i (1+ tab)) (format strm padding))
+            (format strm "- ~%")
+            (print-ros-msg ss strm (+ tab 2) padding)))
          (t
-          (dotimes (i nest) (format t padding))
-          (format t ":~A~%" (string-downcase (string-left-trim "_" (symbol-string sm))))
-          (dotimes (i nest) (format t padding))
-          (print (cdr s)))
-         )))))
+          (format strm "~S~%" (cdr s))))))))
 
 ;; Sensors
 (ros::roseus-add-msgs "sensor_msgs")
@@ -1114,31 +1105,46 @@
     (ros::service-call srvname (instance std_srvs::EmptyRequest :init))
     t))
 
-(defun one-shot-subscribe (topic-name mclass &key (timeout) (after-stamp) (unsubscribe t))
+(defclass one-shot-subscribe-msg
+  :super propertied-object
+  :slots (msg-)
+  :documentation "The purpose of this class is to register class closures for callback function of (one-shot-subscribe). This allows the callback function to be accessed safely from anyscope. For detail, please refer to https://github.com/jsk-ros-pkg/jsk_roseus/issues/666
+"
+  )
+(defmethod one-shot-subscribe-msg
+  (:init () self)
+  (:update (msg) (setq msg- msg))
+  (:update-after-stamp (after-stamp msg)
+    (let ((st (send msg :header :stamp)))
+      (when (> (send st :to-sec)
+               (send after-stamp :to-sec))
+        (send self :update msg))))
+  (:get () msg-))
+
+(defun one-shot-subscribe (topic-name mclass &key (timeout) (after-stamp))
   "Subscribe message, just for once"
-  (let (lmsg)
-    (unless (ros::get-num-publishers topic-name)
+  (let ((lmsg (instance one-shot-subscribe-msg :init)))
+    (if (ros::get-topic-subscriber topic-name)
+      (progn
+        (ros::ros-error (format nil "There is already subscriber for ~A. If you want to use this function, please (ros::unsubscribe \"~A\")." topic-name topic-name))
+        (return-from one-shot-subscribe))
       (cond
        (after-stamp
         (ros::subscribe topic-name mclass
-                        #'(lambda (msg)
-                            (let ((st (send msg :header :stamp)))
-                              (when (> (send st :to-sec)
-                                       (send after-stamp :to-sec))
-                                (setq lmsg msg))))))
+                        #'send lmsg :update-after-stamp after-stamp))
        (t
         (ros::subscribe topic-name mclass
-                        #'(lambda (msg) (setq lmsg msg))))))
+                        #'send lmsg :update))))
     (let ((finishtm (if timeout (ros::time-now))))
       (when finishtm
         (setq finishtm (ros::time+ finishtm (ros::time (/ timeout 1000.0)))))
       (while (not (and finishtm
                        (< (send (ros::time- finishtm (ros::time-now)) :to-Sec) 0)))
         (unix::usleep (* 50 1000))
         (ros::spin-once)
-        (if lmsg (return))))
-    (if unsubscribe (ros::unsubscribe topic-name))
-    lmsg))
+        (if (send lmsg :get) (return))))
+    (ros::unsubscribe topic-name)
+    (send lmsg :get)))
 
 (defun one-shot-publish (topic-name msg &key (wait 500) (after-stamp) (unadvertise t))
   "Publish message just for once"

roseus/euslisp/roseus.l

@@ -14,32 +14,30 @@
 
 (in-package "ROS")
 
-(defun ros::roseus-sigint-handler (sig code)
-  (ros::ROS-WARN (format nil "ros::roseus-sigint-handler ~A" sig))
+(defun ros::roseus-sigint-handler (sig)
+  (ros::ros-error "Received shutdown request (SIGINT)")
   (exit 1))
 
-(defun ros::roseus-error (code msg1 form &optional (msg2))
-   (format *error-output* "~C[1;3~Cm~A roseus-error: ~A"
-                #x1b (+ 1 48)   *program-name* msg1)
-   (if msg2 (format *error-output* " ~A" msg2))
-   (if form (format *error-output* " in ~s" form))
-   (format *error-output* ", exitting...~C[0m~%"  #x1b)
-   (exit 1))
+(defun ros::roseus-error (err)
+  (when (send err :callstack)
+    (lisp::print-callstack (send err :callstack) lisp::*max-callstack-depth*))
+  (let ((errstr (with-output-to-string (str) (lisp::print-error-message err str))))
+    (ros::ros-error errstr))
+  (exit 1))
 
 ;; check if this process is created by roslaunch
 (when (let ((arg (car (last lisp::*eustop-argument*))))
         (and (>= (length arg) 7) (string= "__log:=" (subseq arg 0 7))))
   (ros::ROS-INFO "install ros::roseus-sigint-handler")
   (setq lisp::*exit-on-fatal-error* t)
-  (lisp::install-error-handler 'ros::roseus-error)
-  (unix:signal unix::sigint 'ros::roseus-sigint-handler)
+  (install-handler error 'ros::roseus-error)
+  (install-handler unix::sigint-received 'ros::roseus-sigint-handler)
   (unix:signal unix::sighup 'ros::roseus-sigint-handler)
   (defmacro user::do-until-key (&rest forms) `(while t ,@forms))
   (defun user::y-or-n-p (&rest args) t))
 
 (defun ros::roseus (name &key
                     (anonymous t)
-                    (logger "ros.roseus")
                     (level ros::*rosinfo*)
                     (args lisp::*eustop-argument*))
   "Register roseus client node with the master under the specified name"
@@ -54,7 +52,7 @@
           (setf (elt lisp::*eustop-argument* i)
                 (format nil "(warning-message 1 \"ignore ~a~%\")"
                         (elt lisp::*eustop-argument* i)))))
-      (ros::set-logger-level logger level)
+      (ros::set-logger-level level)
       )))
 
 (setq ros::*compile-message* nil)