diff --git a/roseus/CMakeLists.txt b/roseus/CMakeLists.txt index e732689e3..4812d008a 100644 --- a/roseus/CMakeLists.txt +++ b/roseus/CMakeLists.txt @@ -206,4 +206,6 @@ if(CATKIN_ENABLE_TESTING) add_rostest(test/test-geneus.test) add_rostest(test/test-compile-message.test) add_rostest(test/test-print-ros-msg.test) + add_rostest(test/test-dynamic-reconfigure-server.test) + add_rostest(test/test-dynamic-reconfigure-client.test) endif() diff --git a/roseus/cmake/roseus.cmake b/roseus/cmake/roseus.cmake index 085295cab..b04830b76 100644 --- a/roseus/cmake/roseus.cmake +++ b/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) diff --git a/roseus/euslisp/dynamic-reconfigure-server.l b/roseus/euslisp/dynamic-reconfigure-server.l new file mode 100644 index 000000000..4750dac78 --- /dev/null +++ b/roseus/euslisp/dynamic-reconfigure-server.l @@ -0,0 +1,306 @@ +(ros::load-ros-manifest "dynamic_reconfigure") +;; +;; refer to dynamic_reconfigure/src/dynamic_reconfigure/encoding.py +;; +(defun encode-description (descr) ;; description -> msg + (let ((msg (instance dynamic_reconfigure::ConfigDescription :init))) + (send msg :max (encode-config (get descr :max))) + (send msg :min (encode-config (get descr :min))) + (send msg :dflt (encode-config (get descr :defaults))) + (let (lst) + (dolist (param (get descr :config_description)) + (let ((pmd (instance dynamic_reconfigure::ParamDescription :init + :name (cdr (assoc :name param)) + :type (cdr (assoc :type param)) + :level (cdr (assoc :level param)) + :description (cdr (assoc :description param)) + :edit_method (cdr (assoc :edit_method param))))) + (push pmd lst))) + ;; TODO: default group is only supported + (send msg :groups + (list (instance dynamic_reconfigure::Group :init + :name "Default" + :parameters (nreverse lst))))) + msg)) + + +(defun encode-config (config) + (let ((msg (instance dynamic_reconfigure::Config :init))) + (dolist (cf config) + (let ((k (car cf)) + (v (cdr cf))) + (cond + ((integerp v) + (send msg :ints + (append (send msg :ints) (list (instance dynamic_reconfigure::IntParameter :init + :name k :value v))))) + ((or (eq v nil) + (eq v t)) + (send msg :bools + (append (send msg :bools) (list (instance dynamic_reconfigure::BoolParameter :init + :name k :value v))))) + ((stringp v) + (send msg :strs + (append (send msg :strs) (list (instance dynamic_reconfigure::StrParameter :init + :name k :value v))))) + ((floatp v) + (send msg :doubles + (append (send msg :doubles) (list (instance dynamic_reconfigure::DoubleParameter :init + :name k :value v))))) + ))) + msg + )) +(defun decode-description (msg) ;; msg -> description + (let (descr + (mins (decode-config (send msg :min))) + (maxes (decode-config (send msg :max))) + (defaults (decode-config (send msg :dflt)))) + (dolist (pm (send msg :parameters)) + (let ((nm (send pm :name))) + (push + (list + (cons :name nm) + (cons :min (cdr (assoc nm mins :test #'equal))) + (cons :max (cdr (assoc nm maxes :test #'equal))) + (cons :default (cdr (assoc nm defaults :test #'equal))) + (cons :type (send pm :type)) + (cons :description (send pm :description)) + (cons :edit_method (send pm :edit_method))) + descr) + )) + (nreverse dscr) + )) +(defun decode-config (msg) + (let ((lst + (append (send msg :bools) + (send msg :ints) + (send msg :strs) + (send msg :doubles))) + ret) + (dolist (l lst) + (push + (cons (send l :name) + (send l :value)) ret)) + (nreverse ret))) + +;; +;; refer to dynamic_reconfigure/src/dynamic_reconfigure/server.py +;; +(defclass reconfigure_server + :super propertied-object + :slots (type + config + description + callbackfunc ;; (defun func (cfg level) (setting-parameter cfg) cfg) + param-desc-topic + param-updates-topic + service-name + ) + ) + +(defmethod reconfigure_server + (:init + (init_type cbfunc) + (setq type init_type) + (setq callbackfunc cbfunc) + (setq config (get init_type :defaults)) + (setq description (encode-description init_type)) + (send self :copy-from-parameter-server) + (send self :clamp config) + + (let ((nname (ros::get-name))) + (setq param-desc-topic (format nil "~A/parameter_descriptions" nname)) + (ros::advertise param-desc-topic + dynamic_reconfigure::ConfigDescription 1 t) + (setq param-updates-topic (format nil "~A/parameter_updates" nname)) + (ros::advertise param-updates-topic + dynamic_reconfigure::Config 1 t) + (ros::publish param-desc-topic description) + (send self :change-config config (get init_type :all_level)) + (setq service-name (format nil "~A/set_parameters" nname)) + (ros::advertise-service service-name dynamic_reconfigure::Reconfigure + #'send self :set-callback)) + ) + (:update-configuration + (changes) + (let ((new-config (copy-object config))) + (dolist (cf changes) + (let ((elm + (assoc (car cf) new-config :test #'equal))) + (setf (car elm) (car cf)) + (setf (cdr elm) (cdr cf)))) + (send self :clamp new-config) + (send self :change-config new-config + (send self :calc-level new-config config)) + )) + (:change-config + (cfg level) + (setq config (funcall callbackfunc cfg level)) + (unless config + (warn ";; callbackfunc(~A) did not return config..." callbackfunc)) + (send self :copy-to-parameter-server) + (let ((ret-cfg + (encode-config config))) + (ros::publish param-updates-topic ret-cfg) + ret-cfg)) + (:copy-from-parameter-server + () + ;; not implemented yet + ) + (:copy-to-parameter-server + () + ;; not implemented yet + ) + (:calc-level + (cfg1 cfg2) + (let ((lv 0) + (desclst (get type :config_description))) + (dolist (l desclst) + (when (not (equal (cdr (assoc (cdr (assoc :name l)) cfg1 :test #'equal)) + (cdr (assoc (cdr (assoc :name l)) cfg2 :test #'equal)))) + (setq lv (logior lv (cdr (assoc :level l)))) + )) + lv)) + (:clamp + (cfg) ;; destructive + (let ((maxlst (get type :max)) + (minlst (get type :min)) + (desclst (get type :config_description))) + (dolist (l desclst) + (let ((maxval (cdr (assoc (cdr (assoc :name l)) maxlst :test #'equal))) + (minval (cdr (assoc (cdr (assoc :name l)) minlst :test #'equal))) + (val (cdr (assoc (cdr (assoc :name l)) cfg :test #'equal)))) + (if (and (not (equal maxval "")) + (and (numberp val) + (numberp maxval) + (> val maxval))) + (setf (cdr (assoc (cdr (assoc :name l)) cfg :test #'equal)) maxval) + (if (and (not (equal minval "")) + (and (numberp val) + (numberp maxval) + (< val minval))) + (setf (cdr (assoc (cdr (assoc :name l)) cfg :test #'equal)) minval) + )))) + )) + (:set-callback + (req) + (let ((res (send req :response))) + (send res :config + (send self :update-configuration (decode-config (send req :config)))) + res)) + ) + +;; +;; dynamic_reconfigure setting for roseus +;; +;; TODO: cfg file load is not supported +(defmacro def-dynamic-reconfigure-server (descriptions callback) + `(instance + reconfigure_server :init + (make-description-obj + (list ,@(mapcar #'(lambda (x) `(make-parameter-desc ,@x)) descriptions))) + ,callback)) +(defmacro make-parameter-desc (&rest args) + (let* ((tp (elt args 1)) + (tpstr (cond + ((eq 'int_t tp) "int") + ((eq 'double_t tp) "double") + ((eq 'str_t tp) "str") + ((eq 'bool_t tp) "bool") + (t (warn ";; unknown type ~A" tp)))) + ret) + (setq ret + (list + (cons :name (elt args 0)) + (cons :type tpstr) + (cons :level (elt args 2)) + (cons :description (elt args 3)))) + (setq ret (append ret (list (cons :default + (if (> (length args) 4) + (cond + ((eq 'int_t tp) (round (elt args 4))) + ((eq 'double_t tp) (float (elt args 4))) + ((eq 'str_t tp) (string (elt args 4))) + ((eq 'bool_t tp) (if (elt args 4) t nil))) + (cond + ((eq 'int_t tp) 0) + ((eq 'double_t tp) 0.0) + ((eq 'str_t tp) "") + ((eq 'bool_t tp) nil)) + ))))) + (setq ret (append ret (list (cons :min + (if (> (length args) 5) + (cond + ((eq 'int_t tp) (round (elt args 5))) + ((eq 'double_t tp) (float (elt args 5))) + ((eq 'str_t tp) (string (elt args 5))) + ((eq 'bool_t tp) (if (elt args 5) t nil))) + (cond + ((eq 'int_t tp) lisp::most-negative-fixnum) + ((eq 'double_t tp) lisp::most-negative-float) + ((eq 'str_t tp) "") + ((eq 'bool_t tp) nil)) + ))))) + (setq ret (append ret (list (cons :max + (if (> (length args) 6) + (cond + ((eq 'int_t tp) (round (elt args 6))) + ((eq 'double_t tp) (float (elt args 6))) + ((eq 'str_t tp) (string (elt args 6))) + ((eq 'bool_t tp) (if (elt args 6) t nil))) + (cond + ((eq 'int_t tp) lisp::most-positive-fixnum) + ((eq 'double_t tp) lisp::most-positive-float) + ((eq 'str_t tp) "") + ((eq 'bool_t tp) t)) + ))))) + (setq ret (append ret (list (cons :edit_method + (if (> (length args) 7) + (elt args 7) ""))))) + `',ret + )) + + +(defun make-description-obj (lst) + (let ((obj (instance propertied-object)) + max-lst + min-lst + level-lst + type-lst + default-lst + (clevel 0)) + (setf (get obj :config_description) lst) + (dolist (l lst) + (let ((nm (cdr (assoc :name l)))) + (push (cons nm (cdr (assoc :max l))) max-lst) + (push (cons nm (cdr (assoc :min l))) min-lst) + (push (cons nm (cdr (assoc :level l))) level-lst) + (push (cons nm (cdr (assoc :type l))) type-lst) + (push (cons nm (cdr (assoc :default l))) default-lst) + (setq clevel (logior clevel (cdr (assoc :level l)))) + )) + (setf (get obj :max) (nreverse max-lst)) + (setf (get obj :min) (nreverse min-lst)) + (setf (get obj :level) (nreverse level-lst)) + (setf (get obj :type) (nreverse type-lst)) + (setf (get obj :defaults) (nreverse default-lst)) + (setf (get obj :all_level) clevel) + obj)) +#| +;; sample usage +(ros::roseus "reconfigure_server") +(setq *reconfigure-server* + (def-dynamic-reconfigure-server + ;;; ((name type level description (default) (min) (max) (edit_method)) ... ) + (("int_param" int_t 1 "Int parameter" 0 -10 10) + ("double_param" double_t 2 "double parameter" 0.0 -2.0 10.0) + ("str_param" str_t 4 "String parameter" "foo") + ("bool_param" bool_t 8 "Boolean parameter" nil)) + ;;; callbackfunc (defun func (config level) ) -> return updated-config + #'(lambda (cfg level) + (pprint cfg) + ;; have to set parameter to user application + cfg))) +(while (ros::ok) + (ros::spin-once)) +|# diff --git a/roseus/euslisp/roseus-utils.l b/roseus/euslisp/roseus-utils.l index 99fc13e3a..7f6511665 100644 --- a/roseus/euslisp/roseus-utils.l +++ b/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") @@ -1132,21 +1123,36 @@ (unless (ros r :success) (ros::ros-warn "Call \"~A\" fails, it returns \"~A\"" srvname (send r :message))) r))) +(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) (unsubscribe t)) "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))))) @@ -1154,9 +1160,9 @@ (< (send (ros::time- finishtm (ros::time-now)) :to-Sec) 0))) (unix::usleep (* 50 1000)) (ros::spin-once) - (if lmsg (return)))) + (if (send lmsg :get) (return)))) (if unsubscribe (ros::unsubscribe topic-name)) - lmsg)) + (send lmsg :get))) (defun one-shot-publish (topic-name msg &key (wait 500) (after-stamp) (unadvertise t)) "Publish message just for once" diff --git a/roseus/euslisp/roseus.l b/roseus/euslisp/roseus.l index 11e4a6f2f..50d70e5ef 100644 --- a/roseus/euslisp/roseus.l +++ b/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) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index 017869c23..353fe07df 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -370,7 +370,7 @@ class EuslispSubscriptionCallbackHelper : public ros::SubscriptionCallbackHelper } // avoid gc pointer p=gensym(ctx); - setval(ctx,intern(ctx,(char*)(p->c.sym.pname->c.str.chars),strlen((char*)(p->c.sym.pname->c.str.chars)),lisppkg),cons(ctx,scb,args)); + defconst(ctx, (char*)(p->c.sym.pname->c.str.chars), cons(ctx,scb,args), lisppkg); } ~EuslispSubscriptionCallbackHelper() { ROS_ERROR("subscription gc"); @@ -453,7 +453,7 @@ class EuslispServiceCallbackHelper : public ros::ServiceCallbackHelper { } // avoid gc pointer p=gensym(ctx); - setval(ctx,intern(ctx,(char*)(p->c.sym.pname->c.str.chars),strlen((char*)(p->c.sym.pname->c.str.chars)),lisppkg),cons(ctx,scb,args)); + defconst(ctx, (char*)(p->c.sym.pname->c.str.chars), cons(ctx,scb,args), lisppkg); requestDataType = _req.__getDataType(); responseDataType = _res.__getDataType(); @@ -564,6 +564,7 @@ void roseusSignalHandler(int sig) // memoize for euslisp handler... context *ctx=euscontexts[thr_self()]; ctx->intsig = sig; + ros::Time::shutdown(); } /************************************************************ @@ -688,7 +689,7 @@ pointer ROSEUS_CREATE_NODEHANDLE(register context *ctx,int n,pointer *argv) } if( s_mapHandle.find(groupname) != s_mapHandle.end() ) { - ROS_DEBUG("groupname %s is already used", groupname.c_str()); + ROS_DEBUG("groupname \"%s\" is already used", groupname.c_str()); return (NIL); } @@ -709,7 +710,8 @@ pointer ROSEUS_CREATE_NODEHANDLE(register context *ctx,int n,pointer *argv) pointer ROSEUS_SPIN(register context *ctx,int n,pointer *argv) { isInstalledCheck; - while (ctx->intsig==0 && ros::ok()) { + while (ros::ok()) { + breakck; ros::spinOnce(); s_rate->sleep(); } @@ -722,6 +724,7 @@ pointer ROSEUS_SPINONCE(register context *ctx,int n,pointer *argv) ckarg2(0, 1); // ;; arguments ;; // [ groupname ] + CallbackQueue* queue; if ( n > 0 ) { string groupname; @@ -730,18 +733,20 @@ pointer ROSEUS_SPINONCE(register context *ctx,int n,pointer *argv) map >::iterator it = s_mapHandle.find(groupname); if( it == s_mapHandle.end() ) { - ROS_ERROR("Groupname %s is missing", groupname.c_str()); - return (T); + ROS_ERROR("Groupname \"%s\" is missing", groupname.c_str()); + error(E_USER, "groupname not found"); } boost::shared_ptr hdl = (it->second); - // spin this nodehandle - ((CallbackQueue *)hdl->getCallbackQueue())->callAvailable(); - - return (NIL); + queue = (CallbackQueue *)hdl->getCallbackQueue(); } else { - ros::spinOnce(); - return (NIL); + queue = ros::getGlobalCallbackQueue(); } + if (queue->isEmpty()) { + return (NIL);} + else { + // execute callbacks + queue->callAvailable();} + return (T); } pointer ROSEUS_TIME_NOW(register context *ctx,int n,pointer *argv) @@ -781,7 +786,19 @@ pointer ROSEUS_DURATION_SLEEP(register context *ctx,int n,pointer *argv) numunion nu; ckarg(1); float sleep=ckfltval(argv[0]); - ros::Duration(sleep).sleep(); + // overwrite in order to check for interruptions + // original behaviour is stated at `ros_wallsleep', in time.cpp + if (ros::Time::useSystemTime()) { + int sleep_sec=(int)sleep; + int sleep_nsec=(int)(1000000000*(sleep-sleep_sec)); + struct timespec treq,trem; + GC_REGION(treq.tv_sec = sleep_sec; + treq.tv_nsec = sleep_nsec); + while (nanosleep(&treq, &trem)<0) { + breakck; + treq=trem;}} + else { + ros::Duration(sleep).sleep();} return(T); } @@ -820,12 +837,12 @@ pointer ROSEUS_EXIT(register context *ctx,int n,pointer *argv) ROS_INFO("%s", __PRETTY_FUNCTION__); if( s_bInstalled ) { ROS_INFO("exiting roseus %ld", (n==0)?n:ckintval(argv[0])); + ros::shutdown(); s_mapAdvertised.clear(); s_mapSubscribed.clear(); s_mapServiced.clear(); s_mapTimered.clear(); s_mapHandle.clear(); - ros::shutdown(); } if (n==0) _exit(0); else _exit(ckintval(argv[0])); @@ -856,7 +873,7 @@ pointer ROSEUS_SUBSCRIBE(register context *ctx,int n,pointer *argv) ROS_DEBUG("subscribe with groupname=%s", groupname.c_str()); lnode = (it->second).get(); } else { - ROS_ERROR("Groupname %s is missing. Topic %s is not subscribed. Call (ros::create-nodehandle \"%s\") first.", + ROS_ERROR("Groupname \"%s\" is missing. Topic %s is not subscribed. Call (ros::create-nodehandle \"%s\") first.", groupname.c_str(), topicname.c_str(), groupname.c_str()); return (NIL); } @@ -871,10 +888,12 @@ pointer ROSEUS_SUBSCRIBE(register context *ctx,int n,pointer *argv) args=NIL; for (int i=n-1;i>=3;i--) args=cons(ctx,argv[i],args); + vpush(args); EuslispMessage msg(message); boost::shared_ptr *callback = (new boost::shared_ptr (new EuslispSubscriptionCallbackHelper(fncallback, args, message))); + vpop(); SubscribeOptions so(topicname, queuesize, msg.__getMD5Sum(), msg.__getDataType()); so.helper = *callback; Subscriber subscriber = lnode->subscribe(so); @@ -1081,6 +1100,7 @@ pointer ROSEUS_GETTOPICPUBLISHER(register context *ctx,int n,pointer *argv) pointer ROSEUS_WAIT_FOR_SERVICE(register context *ctx,int n,pointer *argv) { isInstalledCheck; + ros::Time start_time = ros::Time::now(); string service; numunion nu; @@ -1093,9 +1113,28 @@ pointer ROSEUS_WAIT_FOR_SERVICE(register context *ctx,int n,pointer *argv) if( n > 1 && argv[1] != NIL) timeout = ckfltval(argv[1]); - bool bSuccess = service::waitForService(service, ros::Duration(timeout)); + // Overwrite definition on service.cpp L87 to check for signal interruptions + // http://docs.ros.org/electric/api/roscpp/html/service_8cpp_source.html + bool printed = false; + bool result = false; + ros::Duration timeout_duration = ros::Duration(timeout); + while (ctx->intsig==0 && ros::ok()) { + if (service::exists(service, !printed)) { + result = true; + break;} + else { + printed = true; + if (timeout >= 0) { + ros::Time current_time = ros::Time::now(); + if ((current_time - start_time) >= timeout_duration) + return(NIL);} + ros::Duration(0.02).sleep();} + } - return (bSuccess?T:NIL); + if (result && printed && ros::ok()) + ROS_INFO("waitForService: Service [%s] is now available.", service.c_str()); + + return (result?T:NIL); } pointer ROSEUS_SERVICE_EXISTS(register context *ctx,int n,pointer *argv) @@ -1747,11 +1786,8 @@ pointer ROSEUS_GETNAMESPACE(register context *ctx,int n,pointer *argv) pointer ROSEUS_SET_LOGGER_LEVEL(register context *ctx, int n, pointer *argv) { - ckarg(2); - string logger; - if (isstring(argv[0])) logger.assign((char *)get_string(argv[0])); - else error(E_NOSTRING); - int log_level = intval(argv[1]); + ckarg(1); + int log_level = intval(argv[0]); ros::console::levels::Level level = ros::console::levels::Debug; switch(log_level){ case 1: @@ -1773,7 +1809,9 @@ pointer ROSEUS_SET_LOGGER_LEVEL(register context *ctx, int n, pointer *argv) return (NIL); } - bool success = ::ros::console::set_logger_level(logger, level); + // roseus currently does not support multiple loggers + // which must be outputted using the 'ROS_DEBUG_NAMED'-like macros + bool success = ::ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, level); if (success) { console::notifyLoggerLevelsChanged(); @@ -1975,7 +2013,7 @@ pointer ROSEUS_CREATE_TIMER(register context *ctx,int n,pointer *argv) // avoid gc pointer p=gensym(ctx); - setval(ctx,intern(ctx,(char*)(p->c.sym.pname->c.str.chars),strlen((char*)(p->c.sym.pname->c.str.chars)),lisppkg),cons(ctx,fncallback,args)); + defconst(ctx, (char*)(p->c.sym.pname->c.str.chars), cons(ctx,fncallback,args), lisppkg); // ;; store mapTimered ROS_DEBUG("create timer %s at %f (oneshot=%d) (groupname=%s)", fncallname.c_str(), period, oneshot, groupname.c_str()); @@ -2118,7 +2156,7 @@ pointer ___roseus(register context *ctx, int n, pointer *argv, pointer env) " (ros::subscribe \"/test\" std_msgs::String #'(lambda (m) (print m)) :groupname \"mygroup\")\n" " (ros::create-timer 0.1 #'(lambda (event) (print \"timer called\")) :groupname \"mygroup\")\n" " (while (ros::ok) (ros::spin-once \"mygroup\"))\n"); - defun(ctx,"SET-LOGGER-LEVEL",argv[0],(pointer (*)())ROSEUS_SET_LOGGER_LEVEL, ""); + defun(ctx,"SET-LOGGER-LEVEL",argv[0],(pointer (*)())ROSEUS_SET_LOGGER_LEVEL, "(level)"); defun(ctx,"GET-HOST",argv[0],(pointer (*)())ROSEUS_GET_HOST, "Get the hostname where the master runs."); defun(ctx,"GET-NODES",argv[0],(pointer (*)())ROSEUS_GET_NODES, "Retreives the currently-known list of nodes from the master."); diff --git a/roseus/scripts/generate-all-msg-srv.sh b/roseus/scripts/generate-all-msg-srv.sh index e63a04002..b7f7dd66d 100755 --- a/roseus/scripts/generate-all-msg-srv.sh +++ b/roseus/scripts/generate-all-msg-srv.sh @@ -102,7 +102,7 @@ function run-pkg() rosrun geneus gen_eus.py -m -o ${output_dir}/${pkg_name} ${pkg_name} ${pkg_msg_depends} check-error if [ "${COMPILE}" = "Yes" ]; then - rosrun roseus roseus "(progn (setq lisp::*error-handler* #'(lambda (&rest args) (print args *error-output*)(exit 1))) (setq ros::*compile-message* t) (ros::load-ros-manifest \"$pkg_name\") (exit 0))" + rosrun roseus roseus "(progn (install-handler error #'(lambda (c) (lisp::print-error-message c) (exit 1))) (setq ros::*compile-message* t) (ros::load-ros-manifest \"$pkg_name\") (exit 0))" check-error fi } diff --git a/roseus/test/simple-dynamic-reconfigure-server.l b/roseus/test/simple-dynamic-reconfigure-server.l new file mode 100644 index 000000000..a4c759274 --- /dev/null +++ b/roseus/test/simple-dynamic-reconfigure-server.l @@ -0,0 +1,50 @@ +#!/usr/bin/env roseus + +(load "package://roseus/euslisp/dynamic-reconfigure-server.l") + +(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-msgs "dynamic_reconfigure") + +(ros::roseus "simple_dynamic_reconfigure_server" :anonymous nil) + +(setq *int-param* nil) +(setq *double-param* nil) +(setq *str-param* nil) +(setq *bool-param* nil) + +(setq *reconfigure-server* + (def-dynamic-reconfigure-server + ;;; ((name type level description (default) (min) (max) (edit_method)) ... ) + (("int_param" int_t 1 "Int parameter" 0 -10 10) + ("double_param" double_t 2 "double parameter" 0.0 -2.0 10.0) + ("str_param" str_t 4 "String parameter" "foo") + ("bool_param" bool_t 8 "Boolean parameter" nil)) + ;;; callbackfunc (defun func (config level) ) -> return updated-config + '(lambda-closure nil 0 0 (cfg level) + (setq *updated* t) + (setq *int-param* (cdr (assoc "int_param" cfg :test #'equal))) + (setq *double-param* (cdr (assoc "double_param" cfg :test #'equal))) + (setq *str-param* (cdr (assoc "str_param" cfg :test #'equal))) + (setq *bool-param* (cdr (assoc "bool_param" cfg :test #'equal))) + cfg))) + +(ros::advertise "~/int_param" std_msgs::Int16) +(ros::advertise "~/double_param" std_msgs::Float32) +(ros::advertise "~/str_param" std_msgs::String) +(ros::advertise "~/bool_param" std_msgs::Bool) +(ros::advertise "~/updated" std_msgs::Bool) + +(setq *updated* nil) + +(ros::rate 10) +(while (ros::ok) + (if *int-param* + (ros::publish "~/int_param" (instance std_msgs::Int16 :data (round *int-param*)))) + (if *double-param* + (ros::publish "~/double_param" (instance std_msgs::Float32 :data *double-param*))) + (if *str-param* + (ros::publish "~/str_param" (instance std_msgs::String :data *str-param*))) + (ros::publish "~/bool_param" (instance std_msgs::Bool :data *bool-param*)) + (ros::publish "~/updated" (instance std_msgs::Bool :data *updated*)) + (ros::spin-once) + ) diff --git a/roseus/test/test-dynamic-reconfigure-client.l b/roseus/test/test-dynamic-reconfigure-client.l new file mode 100644 index 000000000..8e92d2dff --- /dev/null +++ b/roseus/test/test-dynamic-reconfigure-client.l @@ -0,0 +1,121 @@ +#!/usr/bin/env roseus +;; + +(require :unittest "lib/llib/unittest.l") +(load "package://roseus/euslisp/roseus-utils.l") + +(ros::roseus-add-msgs "std_msgs") + +(ros::roseus "test_dynamic_reconfigure_client" :anonymous nil) + +(init-unit-test) + + +(deftest test-dynamic-reconfigure-client-default () + (let* ((server-name "/simple_dynamic_reconfigure_server") + (int-topic-name (format nil "~A/int_param" server-name)) + (double-topic-name (format nil "~A/double_param" server-name)) + (str-topic-name (format nil "~A/str_param" server-name)) + (bool-topic-name (format nil "~A/bool_param" server-name)) + (int-msg nil) + (double-msg nil) + (str-msg nil) + (bool-msg nil) + (cnt 0)) + (ros::subscribe int-topic-name std_msgs::Int16 + #'(lambda (m) (setq int-msg m))) + (ros::subscribe double-topic-name std_msgs::Float32 + #'(lambda (m) (setq double-msg m))) + (ros::subscribe str-topic-name std_msgs::String + #'(lambda (m) (setq str-msg m))) + (ros::subscribe bool-topic-name std_msgs::Bool + #'(lambda (m) (setq bool-msg m))) + + (while (and (ros::ok) + (< cnt 100) + (or (null int-msg) (null double-msg) + (null str-msg) (null bool-msg)) + ) + (incf cnt) + (ros::spin-once) + (ros::sleep)) + + (ros::unadvertise int-topic-name) + (ros::unadvertise double-topic-name) + (ros::unadvertise str-topic-name) + (ros::unadvertise bool-topic-name) + + (pprint double-msg) + (assert (and int-msg (equal (send int-msg :data) 0)) + (format nil "int default value is wrong: ~A" (send int-msg :data))) + (assert (and double-msg (eps= (send double-msg :data) 0.0)) + (format nil "double default value is wrong: ~A" (send double-msg :data))) + (assert (and str-msg (equal (send str-msg :data) "foo")) + (format nil "str default value is wrong: ~A" (send str-msg :data))) + (assert (and bool-msg (equal (send bool-msg :data) nil)) + (format nil "bool default value is wrong: ~A" (send bool-msg :data))) + )) + +(deftest test-dynamic-reconfigure-client-update () + (let* ((server-name "/simple_dynamic_reconfigure_server") + (int-topic-name (format nil "~A/int_param" server-name)) + (double-topic-name (format nil "~A/double_param" server-name)) + (str-topic-name (format nil "~A/str_param" server-name)) + (bool-topic-name (format nil "~A/bool_param" server-name)) + (update-topic-name (format nil "~A/updated" server-name)) + (int-msg nil) + (double-msg nil) + (str-msg nil) + (bool-msg nil) + (update-msg nil) + (cnt 0)) + (ros::set-dynamic-reconfigure-param server-name "int_param" :int 8) + (ros::set-dynamic-reconfigure-param server-name "double_param" :double 7.3) + (ros::set-dynamic-reconfigure-param server-name "str_param" :string "test") + (ros::set-dynamic-reconfigure-param server-name "bool_param" :bool t) + + (ros::subscribe update-topic-name std_msgs::Bool + #'(lambda (m) (setq update-msg m))) + (while (and (ros::ok) (or (null update-msg) (null (send update-msg :data)))) + (incf cnt) + (ros::spin-once) + (ros::sleep)) + (ros::unsubscribe update-topic-name) + + (ros::subscribe int-topic-name std_msgs::Int16 + #'(lambda (m) (setq int-msg m))) + (ros::subscribe double-topic-name std_msgs::Float32 + #'(lambda (m) (setq double-msg m))) + (ros::subscribe str-topic-name std_msgs::String + #'(lambda (m) (setq str-msg m))) + (ros::subscribe bool-topic-name std_msgs::Bool + #'(lambda (m) (setq bool-msg m))) + + (while (and (ros::ok) + (< cnt 100) + (or (null int-msg) (null double-msg) + (null str-msg) (null bool-msg)) + ) + (incf cnt) + (ros::spin-once) + (ros::sleep)) + + (ros::unsubscribe int-topic-name) + (ros::unsubscribe double-topic-name) + (ros::unsubscribe str-topic-name) + (ros::unsubscribe bool-topic-name) + + (pprint double-msg) + (assert (and int-msg (equal (send int-msg :data) 8)) + (format nil "int default value is wrong: ~A" (send int-msg :data))) + (assert (and double-msg (eps= (send double-msg :data) 7.3)) + (format nil "double default value is wrong: ~A" (send double-msg :data))) + (assert (and str-msg (equal (send str-msg :data) "test")) + (format nil "str default value is wrong: ~A" (send str-msg :data))) + (assert (and bool-msg (equal (send bool-msg :data) t)) + (format nil "bool default value is wrong: ~A" (send bool-msg :data))) + )) + +(run-all-tests) + +(exit) diff --git a/roseus/test/test-dynamic-reconfigure-client.test b/roseus/test/test-dynamic-reconfigure-client.test new file mode 100644 index 000000000..bd774bf6c --- /dev/null +++ b/roseus/test/test-dynamic-reconfigure-client.test @@ -0,0 +1,4 @@ + + + + diff --git a/roseus/test/test-dynamic-reconfigure-server.l b/roseus/test/test-dynamic-reconfigure-server.l new file mode 100644 index 000000000..31076612d --- /dev/null +++ b/roseus/test/test-dynamic-reconfigure-server.l @@ -0,0 +1,130 @@ +#!/usr/bin/env roseus +;; + +(require :unittest "lib/llib/unittest.l") +(load "package://roseus/euslisp/dynamic-reconfigure-server.l") + +(ros::roseus-add-msgs "dynamic_reconfigure") + +(ros::roseus "test_dynamic_reconfigure_server" :anonymous nil) + + +(setq *reconfigure-server* + (def-dynamic-reconfigure-server + ;;; ((name type level description (default) (min) (max) (edit_method)) ... ) + (("int_param" int_t 1 "Int parameter" 0 -10 10) + ("double_param" double_t 2 "double parameter" 0.0 -2.0 10.0) + ("str_param" str_t 4 "String parameter" "foo") + ("bool_param" bool_t 8 "Boolean parameter" nil)) + ;;; callbackfunc (defun func (config level) ) -> return updated-config + '(lambda-closure nil 0 0 (cfg level) + cfg))) + + +(init-unit-test) + +(deftest test-dynamic-reconfigure-server-publish () + (let ((dsc-topic-name "/test_dynamic_reconfigure_server/parameter_descriptions") + (upd-topic-name "/test_dynamic_reconfigure_server/parameter_updates") + (dsc-msg nil) + (upd-msg nil) + (cnt 0)) + (ros::subscribe dsc-topic-name dynamic_reconfigure::ConfigDescription + #'(lambda (m) (setq dsc-msg m))) + (ros::subscribe upd-topic-name dynamic_reconfigure::Config + #'(lambda (m) (setq upd-msg m))) + + (while (and (ros::ok) (< cnt 100) (or (null dsc-msg) (null upd-msg))) + (incf cnt) + (ros::spin-once) + (ros::sleep)) + (ros::unsubscribe dsc-topic-name) + (ros::unsubscribe upd-topic-name) + + (assert dsc-msg "~parameter_descriptions is not published.") + (assert upd-msg "~parameter_updates is not published.") + (let ((dsc-msg-max (send dsc-msg :max)) + (dsc-msg-min (send dsc-msg :min)) + (dsc-msg-dflt (send dsc-msg :dflt)) + (upd-msg-bool (car (send upd-msg :bools))) + (upd-msg-int (car (send upd-msg :ints))) + (upd-msg-str (car (send upd-msg :strs))) + (upd-msg-double (car (send upd-msg :doubles)))) + + ;; max + (assert (equal (send (car (send dsc-msg-max :bools)) :name) "bool_param") + "~parameter_descriptions/max/bools name is wrong") + (assert (equal (send (car (send dsc-msg-max :bools)) :value) t) + "~parameter_descriptions/max/bools value is wrong") + (assert (equal (send (car (send dsc-msg-max :ints)) :name) "int_param") + "~parameter_descriptions/max/ints name is wrong") + (assert (equal (send (car (send dsc-msg-max :ints)) :value) 10) + "~parameter_descriptions/max/ints value is wrong") + (assert (equal (send (car (send dsc-msg-max :strs)) :name) "str_param") + "~parameter_descriptions/max/strs name is wrong") + (assert (equal (send (car (send dsc-msg-max :strs)) :value) "") + "~parameter_descriptions/max/strs value is wrong") + (assert (equal (send (car (send dsc-msg-max :doubles)) :name) "double_param") + "~parameter_descriptions/max/doubles name is wrong") + (assert (equal (send (car (send dsc-msg-max :doubles)) :value) 10.0) + "~parameter_descriptions/max/doubles value is wrong") + + ;; min + (assert (equal (send (car (send dsc-msg-min :bools)) :name) "bool_param") + "~parameter_descriptions/min/bools name is wrong") + (assert (equal (send (car (send dsc-msg-min :bools)) :value) nil) + "~parameter_descriptions/min/bools value is wrong") + (assert (equal (send (car (send dsc-msg-min :ints)) :name) "int_param") + "~parameter_descriptions/min/ints name is wrong") + (assert (equal (send (car (send dsc-msg-min :ints)) :value) -10) + "~parameter_descriptions/min/ints value is wrong") + (assert (equal (send (car (send dsc-msg-min :strs)) :name) "str_param") + "~parameter_descriptions/min/strs name is wrong") + (assert (equal (send (car (send dsc-msg-min :strs)) :value) "") + "~parameter_descriptions/min/strs value is wrong") + (assert (equal (send (car (send dsc-msg-min :doubles)) :name) "double_param") + "~parameter_descriptions/min/doubles name is wrong") + (assert (equal (send (car (send dsc-msg-min :doubles)) :value) -2.0) + "~parameter_descriptions/min/doubles value is wrong") + + ;; dflt + (assert (equal (send (car (send dsc-msg-dflt :bools)) :name) "bool_param") + "~parameter_descriptions/dflt/bools name is wrong") + (assert (equal (send (car (send dsc-msg-dflt :bools)) :value) nil) + "~parameter_descriptions/dflt/bools value is wrong") + (assert (equal (send (car (send dsc-msg-dflt :ints)) :name) "int_param") + "~parameter_descriptions/dflt/ints name is wrong") + (assert (equal (send (car (send dsc-msg-dflt :ints)) :value) 0) + "~parameter_descriptions/dflt/ints value is wrong") + (assert (equal (send (car (send dsc-msg-dflt :strs)) :name) "str_param") + "~parameter_descriptions/dflt/strs name is wrong") + (assert (equal (send (car (send dsc-msg-dflt :strs)) :value) "foo") + "~parameter_descriptions/dflt/strs value is wrong") + (assert (equal (send (car (send dsc-msg-dflt :doubles)) :name) "double_param") + "~parameter_descriptions/dflt/doubles name is wrong") + (assert (equal (send (car (send dsc-msg-dflt :doubles)) :value) 0.0) + "~parameter_descriptions/dflt/doubles value is wrong") + + (assert (and upd-msg-bool (equal (send upd-msg-bool :name) "bool_param")) + "~parameter_updates/bools name is wrong") + (assert (and upd-msg-bool (equal (send upd-msg-bool :value) nil)) + "~parameter_updates/bools value is wrong") + (assert (and upd-msg-int (equal (send upd-msg-int :name) "int_param")) + "~parameter_updates/ints name is wrong") + (assert (and upd-msg-int (equal (send upd-msg-int :value) 0)) + "~parameter_updates/ints value is wrong") + (assert (and upd-msg-str (equal (send upd-msg-str :name) "str_param")) + "~parameter_updates/strs name is wrong") + (assert (and upd-msg-str (equal (send upd-msg-str :value) "foo")) + "~parameter_updates/strs value is wrong") + (assert (and upd-msg-double (equal (send upd-msg-double :name) "double_param")) + "~parameter_updates/doubles name is wrong") + (assert (and upd-msg-double (equal (send upd-msg-double :value) 0.0)) + "~parameter_updates/doubles value is wrong") + ) + )) + + +(run-all-tests) + +(exit) diff --git a/roseus/test/test-dynamic-reconfigure-server.test b/roseus/test/test-dynamic-reconfigure-server.test new file mode 100644 index 000000000..a00e16ee9 --- /dev/null +++ b/roseus/test/test-dynamic-reconfigure-server.test @@ -0,0 +1,3 @@ + + +