diff --git a/.gitmodules b/.gitmodules index fccaa0ed0..fea6b0e18 100644 --- a/.gitmodules +++ b/.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 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 94fc9b17f..ccae884da 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(); } /************************************************************ @@ -642,12 +643,12 @@ pointer ROSEUS(register context *ctx,int n,pointer *argv) /* clear ros::master::g_uri which is defined in ros::master::init in __roseus. - this occurs if user set unix::setenv("ROS_MASTER_URI") between __roseus and + this occurs if user set (unix::putenv "ROS_MASTER_URI") between __roseus and ROSEUS. */ if (!ros::master::g_uri.empty()) { if ( ros::master::g_uri != getenv("ROS_MASTER_URI") ) { - ROS_WARN("ROS master uri will be changed!!, master uri %s, which is defineed previously is differ from current ROS_MASTE_URI(%s)", ros::master::g_uri.c_str(), getenv("ROS_MASTER_URI")); + ROS_WARN("ROS master uri will be changed!!, master uri %s, which is defined previously is differ from current ROS_MASTER_URI(%s)", ros::master::g_uri.c_str(), getenv("ROS_MASTER_URI")); ros::master::g_uri.clear(); } } @@ -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,20 +1100,41 @@ 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(); + numunion nu; string service; ckarg2(1,2); if (isstring(argv[0])) service = ros::names::resolve((char *)get_string(argv[0])); else error(E_NOSTRING); - int32_t timeout = -1; + float timeout = -1; if( n > 1 ) - timeout = (int32_t)ckintval(argv[1]); + 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) @@ -1746,11 +1786,11 @@ 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]); + ckarg2(1,2); + int log_level; + if (n==1) log_level = intval(argv[0]); + else log_level = intval(argv[1]); + ros::console::levels::Level level = ros::console::levels::Debug; switch(log_level){ case 1: @@ -1772,7 +1812,10 @@ 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 + // set all logging to the ROSCONSOLE_DEFAULT_NAME, independent of the argument + bool success = ::ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, level); if (success) { console::notifyLoggerLevelsChanged(); @@ -1974,7 +2017,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()); @@ -2117,7 +2160,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 @@ + + + diff --git a/roseus_bt/CMakeLists.txt b/roseus_bt/CMakeLists.txt new file mode 100644 index 000000000..37e0e7176 --- /dev/null +++ b/roseus_bt/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 2.8.3) +project(roseus_bt) + +SET(Boost_USE_STATIC_LIBS ON) + +find_package(catkin REQUIRED COMPONENTS + cmake_modules + roscpp + behaviortree_ros +) + +find_package(TinyXML2 REQUIRED) +find_package(Boost REQUIRED COMPONENTS log) +find_package(fmt) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES + CATKIN_DEPENDS + DEPENDS TinyXML2 fmt +) + +# git submodule update +find_package(Git QUIET) +option(GIT_SUBMODULE "Check submodules during build" ON) +if(GIT_SUBMODULE) + message(STATUS "Submodule update") + execute_process(COMMAND ${GIT_EXECUTABLE} submodule update --init --remote include/rosbridgecpp + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + RESULT_VARIABLE GIT_SUBMOD_RESULT) + if(NOT GIT_SUBMOD_RESULT EQUAL "0") + message(FATAL_ERROR "git submodule update failed with ${GIT_SUBMOD_RESULT}") + endif() +endif() + +include_directories( include ${catkin_INCLUDE_DIRS} ${TinyXML2_INCUDE_DIRS}) + + +add_executable(create_bt_package src/create_bt_package.cpp) +add_dependencies(create_bt_package ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(create_bt_package ${catkin_LIBRARIES} ${TinyXML2_LIBRARIES} ${Boost_LIBRARIES} fmt::fmt) + +add_executable(create_bt_tutorials src/create_bt_tutorials.cpp) +add_dependencies(create_bt_tutorials ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(create_bt_tutorials ${catkin_LIBRARIES} ${TinyXML2_LIBRARIES} ${Boost_LIBRARIES} fmt::fmt) diff --git a/roseus_bt/README.md b/roseus_bt/README.md new file mode 100644 index 000000000..237bd7d5e --- /dev/null +++ b/roseus_bt/README.md @@ -0,0 +1,49 @@ +# roseus_bt + +Generate glue code for connecting your roseus projects to [BehaviorTree.CPP](https://github.com/BehaviorTree/BehaviorTree.CPP), [BehaviorTree.ROS](https://github.com/BehaviorTree/BehaviorTree.ROS) and [Groot](https://github.com/BehaviorTree/Groot). + +## What are behavior trees + +Behavior Trees are a control structure used to better organize and design the logical flow of an application. When compared to State Machines they tend to display increased modularity (because there are no direct connections between the execution nodes) and reactivity (because the tree formulation includes implicit transitions). + +BehaviorTree.CPP documentation page: [https://www.behaviortree.dev/bt_basics](https://www.behaviortree.dev/bt_basics) + +Behavior Tree in Robotics and AI: [https://arxiv.org/pdf/1709.00084.pdf](https://arxiv.org/pdf/1709.00084.pdf) + +## Quick Setup + +Clone related directories +```bash +mkdir ~/catkin_ws/src -p +cd ~/catkin_ws/src +wget https://raw.githubusercontent.com/Affonso-Gui/jsk_roseus/roseus_bt/roseus_bt/roseus_bt.rosinstall -O .rosinstall +wstool update +``` + +Install dependencies +```bash +rosdep install -yr --ignore-src --from-paths BehaviorTree.ROS Groot jsk_roseus/roseus_bt +``` + +Build & source +```bash +cd ~/catkin_ws/ +catkin build roseus_bt groot +source ~/catkin_ws/devel/setup.bash +``` + +## Run + +The following command creates a new ros package with all the necessary glue code for running roseus code on the BehaviorTree.CPP engine. + +```bash +cd ~/catkin_ws/src +rosrun roseus_bt create_bt_package my_package path/to/model.xml +catkin build my_package +``` + +For more information on how to compose the model file and use the package check the [tutorials](https://github.com/Affonso-Gui/jsk_roseus/tree/roseus_bt/roseus_bt/sample) and the [BehaviorTree.CPP documentation](https://www.behaviortree.dev/bt_basics). + +## Samples + +Follow instructions at the [tutorial page](https://github.com/Affonso-Gui/jsk_roseus/tree/roseus_bt/roseus_bt/sample). diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l new file mode 100644 index 000000000..f4c6c486c --- /dev/null +++ b/roseus_bt/euslisp/nodes.l @@ -0,0 +1,156 @@ +(unless (find-package "ROSEUS_BT") + (make-package "ROSEUS_BT" :nicknames (list "ROSEUS-BT"))) + +(in-package "ROSEUS_BT") + +(export '(action-node resumable-action-node condition-node + cancel-action spin-once spin)) +;; import relevant ros::simple-action-server symbols +(import '(ros::name-space ros::status ros::execute-cb + ros::goal ros::goal-id)) + +(defvar *action-list*) +(defvar *condition-list*) + + +(defcondition cancel-action :slots (server goal)) + +(defun signals-cancel (server goal) + (signals cancel-action :server server :goal goal)) + +(defun check-goal (server new-goal old-goal) + (equal (send new-goal :goal) (send old-goal :goal))) + +;; classes +(defclass action-node :super ros::simple-action-server :slots (monitor-groupname)) +(defmethod action-node + (:init (ns spec &key execute-cb (preempt-cb 'signals-cancel) accept-cb groupname + ((:monitor-groupname monitor-gn))) + + (send-super :init ns spec + :execute-cb execute-cb + :preempt-cb preempt-cb + :accept-cb accept-cb + :groupname groupname) + (when monitor-gn + (setq monitor-groupname monitor-gn) + (ros::create-nodehandle monitor-groupname) + ;; resubscribe /cancel on separate handler + (ros::subscribe (format nil "~A/cancel" ns) + actionlib_msgs::GoalID #'send self :cancel-callback 50 + :groupname monitor-groupname)) + (push self *action-list*) + (if *condition-list* + (ros::ros-warn "Actions and Conditions detected in the same node! Start two separate nodes when reactivity is required.")) + self) + (:publish-status () + (let ((msg (instance actionlib_msgs::GoalStatusArray :init))) + (when goal-id + (send msg :status_list + (list (instance actionlib_msgs::goalstatus :init + :goal_id goal-id + :status status + :text (ros::goal-status-to-string status))))) + (send msg :header :seq (send self :next-seq-id)) + (send msg :header :stamp (ros::time-now)) + (ros::publish (format nil "~A/status" name-space) msg))) + (:execute-cb () + (when (and goal execute-cb) + (let ((res (funcall execute-cb self goal))) + (when (send self :is-active) + (send self :set-succeeded + (send self :result :success (not (not res))))))) + ;; TODO: publishing status at high frequencies is slow, but + ;; required to make a connection with the action client + (send self :publish-status)) + (:set-output (name val) + (let ((msg (send self :feedback (intern (string-upcase name) *keyword-package*) val))) + (send msg :feedback :update_field_name name) + (send self :publish-feedback msg))) + (:ok () + (send self :spin-monitor) + (not (send self :is-preempt-requested))) + (:spin-monitor () + (if monitor-groupname + (ros::spin-once monitor-groupname) + (send self :spin-once)))) + +(defclass resumable-action-node :super action-node :slots (resume-if timeout resume-cb cancel-cb)) +(defmethod resumable-action-node + (:init (ns spec &key execute-cb preempt-cb ((:resume-cb rsm-cb)) accept-cb groupname + ((:monitor-groupname monitor-gn)) + ((:resume-if res-if) 'check-goal) ((:timeout tmout) 10)) + (assert (numberp tmout)) + (setq resume-if res-if) + (setq timeout tmout) + (setq cancel-cb preempt-cb) + (setq resume-cb rsm-cb) + (send-super :init ns spec + :execute-cb execute-cb + :preempt-cb 'signals-cancel + :accept-cb accept-cb + :groupname groupname + :monitor-groupname monitor-gn)) + (:execute-cb () + (catch :execute-cb + (handler-bind + ((cancel-action + #'(lambda (c) + (let ((self (send c :server)) + (timer (instance user::mtimer :init))) + (print "Exitting...") + (if cancel-cb + (funcall cancel-cb (send c :server) (send c :goal))) + (send self :set-preempted) + (ros::ros-info ";; Stashing current progress") + ;; Wait until has new goal and is available + (until (or goal (> (send timer :stop) timeout)) + (send self :spin-once) + (send self :spin-monitor) + (ros::sleep)) + (cond + ((> (send timer :stop) timeout) + (ros::ros-warn ";; Resume timeout exceeded!") + (throw :execute-cb nil)) + ((not (funcall resume-if self goal (send c :goal))) + (ros::ros-warn ";; Resume conditions were not satisfied!") + (throw :execute-cb nil)) + (t + (ros::ros-info ";; Resuming...") + (if resume-cb (funcall resume-cb self (send c :goal))))))))) + (send-super :execute-cb))))) + +(defclass condition-node :super propertied-object :slots (execute-cb groupname)) +(defmethod condition-node + (:init (ns spec &key ((:execute-cb exec-cb)) ((:groupname gn))) + (setq execute-cb exec-cb) + (setq groupname gn) + (if groupname + (ros::advertise-service ns spec #'send self :request-cb :groupname groupname) + (ros::advertise-service ns spec #'send self :request-cb)) + (push self *condition-list*) + (if *action-list* + (ros::ros-warn "Actions and Conditions detected in the same node! Start two separate nodes when reacitivity is required.")) + self) + (:request-cb (msg) + (let ((response (send msg :response))) + (send response :success (not (not (funcall execute-cb self msg)))) + response)) + (:spin-once () + (if groupname + (ros::spin-once groupname) + (ros::spin-once)))) + + +;; functions +(defun spin-once () + (dolist (cc *condition-list*) + (send cc :spin-once)) + (dolist (ac *action-list*) + (send ac :spin-once) + (send ac :execute-cb))) + +(defun spin () + (while (ros::ok) + (spin-once) + (ros::sleep))) diff --git a/roseus_bt/include/rosbridgecpp b/roseus_bt/include/rosbridgecpp new file mode 160000 index 000000000..cb87f6966 --- /dev/null +++ b/roseus_bt/include/rosbridgecpp @@ -0,0 +1 @@ +Subproject commit cb87f6966a1debe4af70a38ae56f988d99645d3b diff --git a/roseus_bt/include/roseus_bt/basic_types.cpp b/roseus_bt/include/roseus_bt/basic_types.cpp new file mode 100644 index 000000000..be37ce3d1 --- /dev/null +++ b/roseus_bt/include/roseus_bt/basic_types.cpp @@ -0,0 +1,54 @@ +#include "roseus_bt/basic_types.h" + +namespace roseus_bt +{ + +std::string toStr(NodeType type) +{ + switch (type) + { + case NodeType::ACTION: + return "Action"; + case NodeType::CONDITION: + return "Condition"; + case NodeType::DECORATOR: + return "Decorator"; + case NodeType::CONTROL: + return "Control"; + case NodeType::SUBTREE: + return "SubTree"; + case NodeType::SUBSCRIBER: + return "Subscriber"; + case NodeType::REMOTE_SUBSCRIBER: + return "RemoteSubscriber"; + case NodeType::REMOTE_ACTION: + return "RemoteAction"; + case NodeType::REMOTE_CONDITION: + return "RemoteCondition"; + default: + return "Undefined"; + } +} + +NodeType convertFromString(BT::StringView str) +{ + if( str == "Action" ) return NodeType::ACTION; + if( str == "Condition" ) return NodeType::CONDITION; + if( str == "Control" ) return NodeType::CONTROL; + if( str == "Decorator" ) return NodeType::DECORATOR; + if( str == "SubTree" || str == "SubTreePlus" ) return NodeType::SUBTREE; + if( str == "Subscriber" ) return NodeType::SUBSCRIBER; + if( str == "RemoteAction" ) return NodeType::REMOTE_ACTION; + if( str == "RemoteCondition" ) return NodeType::REMOTE_CONDITION; + if( str == "RemoteSubscriber" ) return NodeType::REMOTE_SUBSCRIBER; + return NodeType::UNDEFINED; +} + +std::ostream& operator<<(std::ostream& os, const NodeType& type) +{ + os << toStr(type); + return os; +} + + +} // namespace roseus_bt diff --git a/roseus_bt/include/roseus_bt/basic_types.h b/roseus_bt/include/roseus_bt/basic_types.h new file mode 100644 index 000000000..b85a4d141 --- /dev/null +++ b/roseus_bt/include/roseus_bt/basic_types.h @@ -0,0 +1,30 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_NODE_TYPE_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_NODE_TYPE_HPP_ + +#include + + +namespace roseus_bt +{ + +enum class NodeType +{ + UNDEFINED = 0, + ACTION, + CONDITION, + CONTROL, + DECORATOR, + SUBTREE, + SUBSCRIBER, + REMOTE_ACTION, + REMOTE_CONDITION, + REMOTE_SUBSCRIBER, +}; + +std::string toStr(NodeType type); +NodeType convertFromString(BT::StringView str); +std::ostream& operator<<(std::ostream& os, const NodeType& type); + +} // namespace roseus_bt + +#endif // BEHAVIOR_TREE_ROSEUS_BT_EUS_NODE_TYPE_HPP_ diff --git a/roseus_bt/include/roseus_bt/bt_exceptions.h b/roseus_bt/include/roseus_bt/bt_exceptions.h new file mode 100644 index 000000000..e26604700 --- /dev/null +++ b/roseus_bt/include/roseus_bt/bt_exceptions.h @@ -0,0 +1,41 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_BT_EXCEPTIONS_ +#define BEHAVIOR_TREE_ROSEUS_BT_BT_EXCEPTIONS_ + +#include +#include +#include + +namespace RoseusBT +{ + +class BTError: public std::exception { +public: + BTError(std::string message): message(message) {} + + const char* what() const noexcept { + return message.c_str(); + } + + std::string message; +}; + +class InvalidOutputPort: public BTError { +public: + InvalidOutputPort() : BTError("Condition Node only accepts input ports!") {}; +}; + +class InvalidPackageName: public BTError { +public: + InvalidPackageName(std::string name) : + BTError(fmt::format("Package name {} does not follow naming conventions", name)) {}; +}; + +class InvalidInput: public BTError { +public: + InvalidInput() : BTError("Invalid Input") {}; +}; + + +} // namespace RoseusBT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_BT_EXCEPTIONS_ diff --git a/roseus_bt/include/roseus_bt/command_line_argument_mapping.h b/roseus_bt/include/roseus_bt/command_line_argument_mapping.h new file mode 100644 index 000000000..7c4f4e3d0 --- /dev/null +++ b/roseus_bt/include/roseus_bt/command_line_argument_mapping.h @@ -0,0 +1,80 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_COMMAND_LINE_ARGUMENT_MAPPING_ +#define BEHAVIOR_TREE_ROSEUS_BT_COMMAND_LINE_ARGUMENT_MAPPING_ + +#include +#include +#include +#include +#include +#include +#include + + +namespace roseus_bt +{ + +namespace po = boost::program_options; + +bool parse_command_line(int argc, char** argv, + const std::string& program_description, + std::map& argument_map) +{ + std::string example_description = + fmt::format("example:\n {0} --arg var1 hello --arg var2 world\n", argv[0]); + + po::options_description desc("usage"); + desc.add_options() + ("help,h", "show this help message and exit") + ("arg", po::value>()->multitoken(), + "Initial blackboard variable-value pairs"); + + po::positional_options_description pos; + po::parsed_options parsed_options = po::command_line_parser(argc, argv). + options(desc). + positional(pos). + style(po::command_line_style::unix_style ^ po::command_line_style::allow_short). + run(); + + po::variables_map vm; + po::store(parsed_options, vm); + po::notify(vm); + + if (vm.count("help")) { + std::cout << "\n" << program_description << "\n"; + std::cout << desc << "\n"; + std::cout << example_description << "\n"; + return false; + } + + for (const auto option : parsed_options.options) { + if (option.string_key == "arg") { + if (option.value.size() != 2) { + std::cerr << "Each argument must have exactly one name and one value!\n"; + std::cerr << desc << "\n"; + std::cerr << example_description << "\n"; + return false; + } + argument_map.insert( {option.value.at(0), option.value.at(1)} ); + } + } + return true; +} + +void register_blackboard_variables(BT::Tree* tree, + const std::map& argument_map) +{ + // New variables are registered as strings. + // However, if the port has already been registered + // in the tree, it can be default-casted. + for (const auto it: argument_map) { +#ifdef DEBUG + std::cout << "Initializing blackboard variable: " << it.first << + " with value: " << it.second << "\n"; +#endif + tree->rootBlackboard()->set(it.first, it.second); + } +} + +} // namespace roseus_bt + +#endif // BEHAVIOR_TREE_ROSEUS_BT_COMMAND_LINE_ARGUMENT_MAPPING_ diff --git a/roseus_bt/include/roseus_bt/convert_from_string.h b/roseus_bt/include/roseus_bt/convert_from_string.h new file mode 100644 index 000000000..aa71bcab7 --- /dev/null +++ b/roseus_bt/include/roseus_bt/convert_from_string.h @@ -0,0 +1,39 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_CONVERT_FROM_STRING_ +#define BEHAVIOR_TREE_ROSEUS_BT_CONVERT_FROM_STRING_ + +#include +#include + + +namespace BT +{ + +template <> short convertFromString(StringView str) { + return boost::lexical_cast(str); +} + +template <> unsigned short convertFromString(StringView str) { + return boost::lexical_cast(str); +} + +template <> signed char convertFromString(StringView str) { + // explicitly convert to numbers, not characters + signed char result = boost::lexical_cast(str); + return result; +} + +template <> unsigned char convertFromString(StringView str) { + if (str == "true" || str == "True") { + return true; + } + if (str == "false" || str == "False") { + return false; + } + // explicitly convert to numbers, not characters + unsigned char result = boost::lexical_cast(str); + return result; +} + +} // namespace BT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_CONVERT_FROM_STRING_ diff --git a/roseus_bt/include/roseus_bt/copy_document.h b/roseus_bt/include/roseus_bt/copy_document.h new file mode 100644 index 000000000..e3cb57cdb --- /dev/null +++ b/roseus_bt/include/roseus_bt/copy_document.h @@ -0,0 +1,68 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_JSON_COPY_DOCUMENT_ +#define BEHAVIOR_TREE_ROSEUS_BT_JSON_COPY_DOCUMENT_ + +#include "rapidjson/include/rapidjson/document.h" +#include "rapidjson/include/rapidjson/writer.h" +#include "rapidjson/include/rapidjson/prettywriter.h" +#include "rapidjson/include/rapidjson/stringbuffer.h" + + +namespace rapidjson +{ +class CopyDocument : public rapidjson::Document +{ +public: + CopyDocument() : Document() {} + + CopyDocument(Type type) : Document(type) {} + + CopyDocument(const CopyDocument& document) { + _clone(document); + } + + CopyDocument(const Document& document) { + _clone(document); + } + + CopyDocument(CopyDocument&& document) : Document(std::move(document)) {} + + CopyDocument(Document&& document) : Document(std::move(document)) {} + + CopyDocument& operator=(const CopyDocument& document) { + _clone(document); + return *this; + } + + CopyDocument& operator=(const Document& document) { + _clone(document); + return *this; + } + + CopyDocument& operator=(CopyDocument&& document) { + Swap(document); + return *this; + } + + CopyDocument& operator=(Document&& document) { + Swap(document); + return *this; + } + + std::string toStr() const + { + StringBuffer strbuf; + Writer writer(strbuf); + Accept(writer); + return strbuf.GetString(); + } + +protected: + void _clone(const Document& document) { + SetObject(); + CopyFrom(document, GetAllocator()); + } +}; + +} // namespace rapidjson + +#endif // BEHAVIOR_TREE_ROSEUS_BT_JSON_COPY_DOCUMENT_ diff --git a/roseus_bt/include/roseus_bt/eus_action_node.h b/roseus_bt/include/roseus_bt/eus_action_node.h new file mode 100644 index 000000000..68bcdd321 --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_action_node.h @@ -0,0 +1,114 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_ACTION_NODE_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_ACTION_NODE_HPP_ + +#include + +namespace BT +{ + +/** + * Include feedback callback to BT::RosActionNode + * Note that the user must implement the additional onFeedback method + * + */ +template +class EusActionNode: public BT::RosActionNode +{ +protected: + EusActionNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration& conf): + BT::RosActionNode(nh, name, conf) { + // check connection at init time + const unsigned msec = BT::TreeNode::getInput("timeout").value(); + const std::string server_name = BT::TreeNode::getInput("server_name").value(); + ros::Duration timeout(static_cast(msec) * 1e-3); + + ROS_DEBUG("Connecting to action server at '%s'...", server_name.c_str()); + bool connected = BT::RosActionNode::action_client_->waitForServer(timeout); + if (!connected) { + throw BT::RuntimeError("Couldn't connect to action server at: ", server_name); + } + }; + +public: + using FeedbackType = typename ActionT::_action_feedback_type::_feedback_type; + using Client = actionlib::SimpleActionClient; + + EusActionNode() = delete; + virtual ~EusActionNode() = default; + + virtual bool sendGoal(typename BT::RosActionNode::GoalType& goal) = 0; + virtual NodeStatus onResult( const typename BT::RosActionNode::ResultType& res) = 0; + virtual void onFeedback( const typename FeedbackType::ConstPtr& feedback) = 0; + + virtual void halt() override + { + BT::RosActionNode::halt(); + BT::RosActionNode::action_client_->waitForResult(); + } + +protected: + /** + * Override Behaviortree.ROS definition to pass the feedback callback + * + */ + BT::NodeStatus tick() override + { + unsigned msec = BT::TreeNode::getInput("timeout").value(); + ros::Duration timeout(static_cast(msec) * 1e-3); + + bool connected = BT::RosActionNode::action_client_->waitForServer(timeout); + if( !connected ){ + return BT::RosActionNode::onFailedRequest(BT::RosActionNode::MISSING_SERVER); + } + + // first step to be done only at the beginning of the Action + if (BT::TreeNode::status() == BT::NodeStatus::IDLE) { + // setting the status to RUNNING to notify the BT Loggers (if any) + BT::TreeNode::setStatus(BT::NodeStatus::RUNNING); + + typename BT::RosActionNode::GoalType goal; + bool valid_goal = sendGoal(goal); + if( !valid_goal ) + { + return NodeStatus::FAILURE; + } + BT::RosActionNode::action_client_->sendGoal( + goal, + NULL, // donecb + NULL, // activecb + boost::bind(&EusActionNode::onFeedback, this, _1)); + } + + // RUNNING + auto action_state = BT::RosActionNode::action_client_->getState(); + + // Please refer to these states + + if( action_state == actionlib::SimpleClientGoalState::PENDING || + action_state == actionlib::SimpleClientGoalState::ACTIVE ) + { + return NodeStatus::RUNNING; + } + else if( action_state == actionlib::SimpleClientGoalState::SUCCEEDED) + { + return onResult( *BT::RosActionNode::action_client_->getResult() ); + } + else if( action_state == actionlib::SimpleClientGoalState::ABORTED) + { + return BT::RosActionNode::onFailedRequest( BT::RosActionNode::ABORTED_BY_SERVER ); + } + else if( action_state == actionlib::SimpleClientGoalState::REJECTED) + { + return BT::RosActionNode::onFailedRequest( BT::RosActionNode::REJECTED_BY_SERVER ); + } + else + { + // FIXME: is there any other valid state we should consider? + throw std::logic_error("Unexpected state in RosActionNode::tick()"); + } + } +}; + +} // namespace BT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_EUS_ACTION_NODE_HPP_ diff --git a/roseus_bt/include/roseus_bt/eus_condition_node.h b/roseus_bt/include/roseus_bt/eus_condition_node.h new file mode 100644 index 000000000..fc61d13a7 --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_condition_node.h @@ -0,0 +1,33 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_CONDITION_NODE_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_CONDITION_NODE_HPP_ + +#include + +namespace BT +{ + +template +class EusConditionNode : public BT::RosServiceNode +{ +protected: + EusConditionNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration& conf): BT::RosServiceNode(nh, name, conf) { + const unsigned msec = BT::TreeNode::getInput("timeout").value(); + const std::string server_name = BT::TreeNode::getInput("service_name").value(); + ros::Duration timeout(static_cast(msec) * 1e-3); + BT::RosServiceNode::service_client_ = nh.serviceClient( server_name, true ); + + ROS_DEBUG("Connecting to service server at '%s'...", server_name.c_str()); + bool connected = BT::RosServiceNode::service_client_.waitForExistence(timeout); + if(!connected){ + throw BT::RuntimeError("Couldn't connect to service server at: ", server_name); + } + }; + +public: + EusConditionNode() = delete; + virtual ~EusConditionNode() = default; +}; + +} // namespace BT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_EUS_CONDITION_NODE_HPP_ diff --git a/roseus_bt/include/roseus_bt/eus_nodes.h b/roseus_bt/include/roseus_bt/eus_nodes.h new file mode 100644 index 000000000..5d97b4301 --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_nodes.h @@ -0,0 +1,13 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_NODES_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_NODES_HPP_ + +#include "convert_from_string.h" + +#include "eus_action_node.h" +#include "eus_condition_node.h" +#include "eus_subscriber_node.h" +#include "eus_remote_action_node.h" +#include "eus_remote_condition_node.h" +#include "eus_remote_subscriber_node.h" + +#endif // BEHAVIOR_TREE_ROSEUS_BT_EUS_NODES_HPP_ diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h new file mode 100644 index 000000000..441e89688 --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -0,0 +1,153 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_REMOTE_ACTION_NODE_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_REMOTE_ACTION_NODE_HPP_ + +#include +#include +#include +#include + + +namespace BT +{ + +// Helper Node to call a rosbridge websocket inside a BT::ActionNode +template +class EusRemoteActionNode : public BT::ActionNodeBase +{ +protected: + + EusRemoteActionNode(const std::string message_type, const std::string& name, const BT::NodeConfiguration & conf): + BT::ActionNodeBase(name, conf), + action_client_(getInput("host_name").value(), + getInput("host_port").value(), + getInput("server_name").value(), + message_type) + { + auto cb = std::bind(&EusRemoteActionNode::feedbackCallback, this, + std::placeholders::_1, + std::placeholders::_2); + action_client_.registerFeedbackCallback(cb); + } + +public: + + using BaseClass = EusRemoteActionNode; + using ActionType = ActionT; + using GoalType = typename ActionT::_action_goal_type::_goal_type; + using FeedbackType = typename ActionT::_action_feedback_type::_feedback_type; + + EusRemoteActionNode() = delete; + virtual ~EusRemoteActionNode() = default; + + static PortsList providedPorts() + { + return { + InputPort("server_name", "name of the Action Server"), + InputPort("host_name", "name of the rosbridge_server host"), + InputPort("host_port", "port of the rosbridge_server host") + }; + } + + virtual bool sendGoal(rapidjson::Document *goal) = 0; + + virtual NodeStatus onResult(const rapidjson::Value& res) = 0; + virtual void onFeedback(const rapidjson::Value& feedback) = 0; + + // virtual NodeStatus onFailedRequest(FailureCause failure) + // { + // return NodeStatus::FAILURE; + // } + + virtual void halt() override + { + if (action_client_.isActive()) { + action_client_.cancelGoal(); + } + setStatus(NodeStatus::IDLE); + action_client_.waitForResult(); + } + +protected: + roseus_bt::RosbridgeActionClient action_client_; + + std::string server_name_; + std::string goal_topic_; + std::string feedback_topic_; + std::string result_topic_; + + BT::NodeStatus tick() override + { + if (BT::TreeNode::status() == BT::NodeStatus::IDLE) { + BT::TreeNode::setStatus(BT::NodeStatus::RUNNING); + + rapidjson::Document goal; + goal.SetObject(); + bool valid_goal = sendGoal(&goal); + if( !valid_goal ) + { + return NodeStatus::FAILURE; + } + + action_client_.sendGoal(goal); + } + + if (action_client_.isActive()) { + return NodeStatus::RUNNING; + } + + // TODO: check slots and raise errors + // throw BT::RuntimeError("EusRemoteActionNode: ActionResult is not set properly"); + + return onResult( action_client_.getResult() ); + } + + // port related + // TODO: translate to ROS message: how to loop through slots? + + void feedbackCallback(std::shared_ptr connection, + std::shared_ptr in_message) + { + std::string message = in_message->string(); +#ifdef DEBUG + std::cout << "feedbackCallback(): Message Received: " << message << std::endl; +#endif + + rapidjson::Document document, feedbackMessage; + document.Parse(message.c_str()); + feedbackMessage.Swap(document["msg"]["feedback"]); + + onFeedback(feedbackMessage); + } + + void setOutputFromMessage(const std::string& name, const rapidjson::Value& message) + { + if (message["update_field_name"].GetString() != name) return; + + rapidjson::CopyDocument document; + document.CopyFrom(message[name.c_str()], document.GetAllocator()); + setOutput(name, document); + } +}; + + +/// Method to register the service into a factory. +template static + void RegisterRemoteAction(BT::BehaviorTreeFactory& factory, + const std::string& registration_ID) +{ + NodeBuilder builder = [](const std::string& name, const NodeConfiguration& config) { + return std::make_unique(name, config); + }; + + TreeNodeManifest manifest; + manifest.type = getType(); + manifest.ports = DerivedT::providedPorts(); + manifest.registration_ID = registration_ID; + const auto& basic_ports = EusRemoteActionNode::providedPorts(); + manifest.ports.insert( basic_ports.begin(), basic_ports.end() ); + factory.registerBuilder( manifest, builder ); +} + +} // namespace BT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_EUS_REMOTE_ACTION_NODE_HPP_ diff --git a/roseus_bt/include/roseus_bt/eus_remote_condition_node.h b/roseus_bt/include/roseus_bt/eus_remote_condition_node.h new file mode 100644 index 000000000..ee15b9208 --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_remote_condition_node.h @@ -0,0 +1,108 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_REMOTE_CONDITION_NODE_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_REMOTE_CONDITION_NODE_HPP_ + +#include +#include +#include + + +namespace BT +{ + +// Helper Node to call a rosbridge websocket inside a BT::ActionNode +template +class EusRemoteConditionNode : public BT::ActionNodeBase +{ +protected: + + EusRemoteConditionNode(const std::string& name, const BT::NodeConfiguration & conf): + BT::ActionNodeBase(name, conf), + service_client_(getInput("host_name").value(), + getInput("host_port").value(), + getInput("service_name").value()) + {} + +public: + + using BaseClass = EusRemoteConditionNode; + using ServiceType = ServiceT; + using RequestType = typename ServiceT::Request; + + EusRemoteConditionNode() = delete; + virtual ~EusRemoteConditionNode() = default; + + static PortsList providedPorts() + { + return { + InputPort("service_name", "name of the ROS service"), + InputPort("host_name", "name of the rosbridge_server host"), + InputPort("host_port", "port of the rosbridge_server host") + }; + } + + virtual void sendRequest(rapidjson::Document *request) = 0; + virtual NodeStatus onResponse(const rapidjson::Value& result) = 0; + + // enum FailureCause{ + // MISSING_SERVER = 0, + // FAILED_CALL = 1 + // }; + + // virtual NodeStatus onFailedRequest(FailureCause failure) + // { + // return NodeStatus::FAILURE; + // } + + virtual void halt() override + { + if (service_client_.isActive()) { + service_client_.cancelRequest(); + } + setStatus(NodeStatus::IDLE); + service_client_.waitForResult(); + } + +protected: + roseus_bt::RosbridgeServiceClient service_client_; + + BT::NodeStatus tick() override + { + if (BT::TreeNode::status() == BT::NodeStatus::IDLE) { + BT::TreeNode::setStatus(BT::NodeStatus::RUNNING); + + rapidjson::Document request; + request.SetObject(); + sendRequest(&request); + service_client_.call(request); + } + + // Conditions cannot operate asynchronously + service_client_.waitForResult(); + + return onResponse(service_client_.getResult()); + } +}; + + +/// Method to register the service into a factory. +template static + void RegisterRemoteCondition(BT::BehaviorTreeFactory& factory, + const std::string& registration_ID) +{ + NodeBuilder builder = [](const std::string& name, const NodeConfiguration& config) { + return std::make_unique(name, config); + }; + + TreeNodeManifest manifest; + manifest.type = getType(); + manifest.ports = DerivedT::providedPorts(); + manifest.registration_ID = registration_ID; + const auto& basic_ports = EusRemoteConditionNode::providedPorts(); + manifest.ports.insert( basic_ports.begin(), basic_ports.end() ); + + factory.registerBuilder( manifest, builder ); +} + +} // namespace BT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_EUS_REMOTE_CONDITION_NODE_HPP_ diff --git a/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h b/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h new file mode 100644 index 000000000..37fe5f5d0 --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h @@ -0,0 +1,109 @@ +#ifndef BEHAVIOR_TREE_EUS_REMOTE_SUBSCRIBER_NODE_HPP_ +#define BEHAVIOR_TREE_EUS_REMOTE_SUBSCRIBER_NODE_HPP_ + +#include +#include +#include +#include + +namespace BT +{ + +template +class EusRemoteSubscriberNode: public BT::ActionNodeBase +{ +protected: + EusRemoteSubscriberNode(const std::string& name, const BT::NodeConfiguration& conf): + BT::ActionNodeBase(name, conf), + subscriber_client_(getInput("host_name").value(), + getInput("host_port").value(), + getInput("topic_name").value(), + getInput("message_type").value()) + { + auto cb = std::bind(&EusRemoteSubscriberNode::topicCallback, this, + std::placeholders::_1, + std::placeholders::_2); + subscriber_client_.registerCallback(cb); + } + +public: + + using MessageType = MessageT; + + EusRemoteSubscriberNode() = delete; + virtual ~EusRemoteSubscriberNode() = default; + + static PortsList providedPorts() { + return { + InputPort("topic_name", "name of the subscribed topic"), + OutputPort("output_port", "port to where messages are redirected"), + OutputPort("received_port", "port set to true every time a message is received"), + InputPort("host_name", "name of the rosbridge_server host"), + InputPort("host_port", "port of the rosbridge_server host") + }; + } + + virtual BT::NodeStatus tick() override final + { + return NodeStatus::SUCCESS; + } + + virtual void halt() override final + { + setStatus(NodeStatus::IDLE); + } + +protected: + roseus_bt::RosbridgeSubscriberClient subscriber_client_; + +protected: + virtual void callback(const rapidjson::Value& msg) { + setOutputFromMessage("output_port", msg); + setOutput("received_port", (uint8_t)true); + } + + void topicCallback(std::shared_ptr connection, std::shared_ptr in_message) + { + std::string message = in_message->string(); +#ifdef DEBUG + std::cout << "topicCallback(): Message Received: " << message << std::endl; +#endif + + rapidjson::Document document, topicMessage; + document.Parse(message.c_str()); + topicMessage.Swap(document["msg"]); + + callback(topicMessage); + } + + void setOutputFromMessage(const std::string& name, const rapidjson::Value& message) + { + rapidjson::CopyDocument document; + document.CopyFrom(message, document.GetAllocator()); + setOutput(name, document); + } +}; + + +/// Method to register the subscriber into a factory. +template static + void RegisterRemoteSubscriber(BT::BehaviorTreeFactory& factory, + const std::string& registration_ID) +{ + NodeBuilder builder = [](const std::string& name, const NodeConfiguration& config) { + return std::make_unique(name, config ); + }; + + TreeNodeManifest manifest; + manifest.type = getType(); + manifest.ports = DerivedT::providedPorts(); + manifest.registration_ID = registration_ID; + const auto& basic_ports = EusRemoteSubscriberNode::providedPorts(); + manifest.ports.insert( basic_ports.begin(), basic_ports.end() ); + factory.registerBuilder( manifest, builder ); +} + + +} // namespace BT + +#endif // BEHAVIOR_TREE_EUS_REMOTE_SUBSCRIBER_NODE_HPP_ diff --git a/roseus_bt/include/roseus_bt/eus_subscriber_node.h b/roseus_bt/include/roseus_bt/eus_subscriber_node.h new file mode 100644 index 000000000..8cb259c18 --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_subscriber_node.h @@ -0,0 +1,78 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_SUBSCRIBER_NODE_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_SUBSCRIBER_NODE_HPP_ + + +namespace BT +{ + +template +class EusSubscriberNode: public BT::ActionNodeBase +{ +protected: + EusSubscriberNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration& conf): + BT::ActionNodeBase(name, conf), node_(nh) + { + const std::string topic_name = getInput("topic_name").value(); + sub_ = node_.subscribe(topic_name, 1000, &EusSubscriberNode::callback, this); + } + +public: + + using MessageType = MessageT; + + EusSubscriberNode() = delete; + virtual ~EusSubscriberNode() = default; + + static PortsList providedPorts() { + return { + InputPort("topic_name", "name of the subscribed topic"), + OutputPort("output_port", "port to where messages are redirected"), + OutputPort("received_port", "port set to true every time a message is received") + }; + } + + virtual BT::NodeStatus tick() override final + { + return NodeStatus::SUCCESS; + } + + virtual void halt() override final + { + setStatus(NodeStatus::IDLE); + } + +protected: + ros::NodeHandle& node_; + ros::Subscriber sub_; + +protected: + virtual void callback(MessageT msg) { + setOutput("output_port", msg); + setOutput("received_port", (uint8_t)true); + } + +}; + + +/// Binds the ros::NodeHandle and register to the BehaviorTreeFactory +template static + void RegisterSubscriberNode(BT::BehaviorTreeFactory& factory, + const std::string& registration_ID, + ros::NodeHandle& node_handle) +{ + NodeBuilder builder = [&node_handle](const std::string& name, const NodeConfiguration& config) { + return std::make_unique(node_handle, name, config ); + }; + + TreeNodeManifest manifest; + manifest.type = getType(); + manifest.ports = DerivedT::providedPorts(); + manifest.registration_ID = registration_ID; + const auto& basic_ports = EusSubscriberNode< typename DerivedT::MessageType>::providedPorts(); + manifest.ports.insert( basic_ports.begin(), basic_ports.end() ); + factory.registerBuilder( manifest, builder ); +} + +} // namespace BT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_EUS_SUBSCRIBER_NODE_HPP_ diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h new file mode 100644 index 000000000..931fdf5c9 --- /dev/null +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -0,0 +1,553 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_GEN_TEMPLATE_ +#define BEHAVIOR_TREE_ROSEUS_BT_GEN_TEMPLATE_ + +#include +#include +#include +#include +#include + +namespace RoseusBT +{ + +class GenTemplate +{ +public: + GenTemplate() {}; + ~GenTemplate() {}; + + std::string action_file_template(std::vector goal, + std::vector feedback); + std::string service_file_template(std::vector request); + std::string launch_file_template(std::vector launch_nodes); + std::string headers_template(std::vector headers); + + std::string action_class_template(std::string package_name, std::string nodeID, + std::vector provided_ports, + std::vector get_inputs, + std::vector set_outputs); + std::string remote_action_class_template(std::string package_name, std::string nodeID, + std::vector provided_ports, + std::vector get_inputs, + std::vector set_outputs); + std::string condition_class_template(std::string package_name, std::string nodeID, + std::vector provided_ports, + std::vector get_inputs); + std::string remote_condition_class_template(std::string package_name, std::string nodeID, + std::vector provided_ports, + std::vector get_inputs); + std::string subscriber_class_template(std::string nodeID, std::string message_type, + std::vector provided_ports); + std::string remote_subscriber_class_template(std::string nodeID, std::string message_type, + std::vector provided_ports); + std::string main_function_template(std::string roscpp_node_name, + std::string program_description, + std::string xml_filename, + std::vector register_actions, + std::vector register_conditions, + std::vector register_subscribers); + std::string eus_server_template(std::string server_type, + std::string package_name, + std::vector callbacks, + std::vector instances, + std::vector load_files); +}; + + +std::string GenTemplate::action_file_template(std::vector goal, + std::vector feedback) { + std::string fmt_string = 1 + R"( +%1% +--- +bool success +--- +string update_field_name +%2% +)"; + + boost::format bfmt = boost::format(fmt_string) % + boost::algorithm::join(goal, "\n") % + boost::algorithm::join(feedback, "\n"); + + return bfmt.str(); +} + + +std::string GenTemplate::service_file_template(std::vector request) { + std::string fmt_string = 1 + R"( +%1% +--- +bool success +)"; + + boost::format bfmt = boost::format(fmt_string) % + boost::algorithm::join(request, "\n"); + + return bfmt.str(); +} + + +std::string GenTemplate::launch_file_template(std::vector launch_nodes) +{ + std::string fmt_string = 1 + R"( + + + + + +%1% + + +)"; + + boost::format bfmt = boost::format(fmt_string) % + boost::algorithm::join(launch_nodes, "\n"); + return bfmt.str(); +} + + +std::string GenTemplate::headers_template(std::vector headers) { + std::string fmt_string = 1 + R"( +#include + +#define DEBUG // rosbridgecpp logging +#include +#include +#include +#include +#include + +%1% + +using namespace BT; +)"; + boost::format bfmt = boost::format(fmt_string) % + boost::algorithm::join(headers, "\n"); + + return bfmt.str(); +} + + +std::string GenTemplate::action_class_template(std::string package_name, std::string nodeID, + std::vector provided_ports, + std::vector get_inputs, + std::vector set_outputs) { + std::string fmt_string = 1 + R"( +class %2%: public EusActionNode<%1%::%2%Action> +{ + +public: + %2%(ros::NodeHandle& handle, const std::string& name, const NodeConfiguration& conf): +EusActionNode<%1%::%2%Action>(handle, name, conf) {} + + static PortsList providedPorts() + { + return { +%3% + }; + } + + bool sendGoal(GoalType& goal) override + { +%4% + return true; + } + + void onFeedback(const typename FeedbackType::ConstPtr& feedback) override + { +%5% + return; + } + + NodeStatus onResult(const ResultType& result) override + { + if (result.success) return NodeStatus::SUCCESS; + return NodeStatus::FAILURE; + } + + virtual NodeStatus onFailedRequest(FailureCause failure) override + { + return NodeStatus::FAILURE; + } + +}; +)"; + boost::format bfmt = boost::format(fmt_string) % + package_name % + nodeID % + boost::algorithm::join(provided_ports, ",\n") % + boost::algorithm::join(get_inputs, "\n") % + boost::algorithm::join(set_outputs, "\n"); + + return bfmt.str(); +} + + +std::string GenTemplate::remote_action_class_template( + std::string package_name, std::string nodeID, + std::vector provided_ports, + std::vector get_inputs, + std::vector set_outputs) +{ + auto format_send_goal = [](const std::string body) { + std::string decl = 1 + R"( + rapidjson::CopyDocument document; + GoalType ros_msg; +)"; + if (!body.empty()) return fmt::format("{}{}", decl, body); + return body; + }; + + std::string fmt_string = 1 + R"( +class %2%: public EusRemoteActionNode<%1%::%2%Action> +{ + +public: + %2%(const std::string& name, const NodeConfiguration& conf): +EusRemoteActionNode("%1%/%2%Action", name, conf) {} + + static PortsList providedPorts() + { + return { +%3% + }; + } + + bool sendGoal(rapidjson::Document* goal) override + { +%4% + return true; + } + + void onFeedback(const rapidjson::Value& feedback) override + { +%5% + return; + } + + NodeStatus onResult(const rapidjson::Value& result) override + { + if (result.HasMember("success") && + result["success"].IsBool() && + result["success"].GetBool()) { + return NodeStatus::SUCCESS; + } + return NodeStatus::FAILURE; + } + +}; +)"; + boost::format bfmt = boost::format(fmt_string) % + package_name % + nodeID % + boost::algorithm::join(provided_ports, ",\n") % + format_send_goal(boost::algorithm::join(get_inputs, "\n")) % + boost::algorithm::join(set_outputs, "\n"); + + return bfmt.str(); +} + + +std::string GenTemplate::condition_class_template(std::string package_name, std::string nodeID, + std::vector provided_ports, + std::vector get_inputs) { + std::string fmt_string = 1 + R"( +class %2%: public EusConditionNode<%1%::%2%> +{ + +public: + %2%(ros::NodeHandle& handle, const std::string& name, const NodeConfiguration& conf): + EusConditionNode<%1%::%2%>(handle, name, conf) {} + + static PortsList providedPorts() + { + return { +%3% + }; + } + + void sendRequest(RequestType& request) override + { +%4% + } + + NodeStatus onResponse(const ResponseType& res) override + { + if (res.success) return NodeStatus::SUCCESS; + return NodeStatus::FAILURE; + } + + virtual NodeStatus onFailedRequest(FailureCause failure) override + { + return NodeStatus::FAILURE; + } + +}; +)"; + + boost::format bfmt = boost::format(fmt_string) % + package_name % + nodeID % + boost::algorithm::join(provided_ports, ",\n") % + boost::algorithm::join(get_inputs, "\n"); + + return bfmt.str(); +} + + +std::string GenTemplate::remote_condition_class_template(std::string package_name, std::string nodeID, + std::vector provided_ports, + std::vector get_inputs) { + auto format_send_request = [](const std::string body) { + std::string decl = 1 + R"( + std::string json; + rapidjson::Document document; + RequestType ros_msg; +)"; + if (!body.empty()) return fmt::format("{}{}", decl, body); + return body; + }; + + std::string fmt_string = 1 + R"( +class %2%: public EusRemoteConditionNode<%1%::%2%> +{ + +public: + %2%(const std::string& name, const NodeConfiguration& conf): + EusRemoteConditionNode(name, conf) {} + + static PortsList providedPorts() + { + return { +%3% + }; + } + + void sendRequest(rapidjson::Document *request) override + { +%4% + } + + NodeStatus onResponse(const rapidjson::Value& result) override + { + if (result.HasMember("success") && + result["success"].IsBool() && + result["success"].GetBool()) { + return NodeStatus::SUCCESS; + } + return NodeStatus::FAILURE; + } + +}; +)"; + + boost::format bfmt = boost::format(fmt_string) % + package_name % + nodeID % + boost::algorithm::join(provided_ports, ",\n") % + format_send_request(boost::algorithm::join(get_inputs, "\n")); + + return bfmt.str(); +} + + +std::string GenTemplate::subscriber_class_template(std::string nodeID, + std::string message_type, + std::vector provided_ports) { + std::string provided_ports_body; + + if (!provided_ports.empty()) { + std::string fmt_string = R"( + static PortsList providedPorts() + { + return { +%1% + }; + })"; + + boost::format bfmt = boost::format(fmt_string) % + boost::algorithm::join(provided_ports, ",\n"); + + provided_ports_body = bfmt.str(); + } + + std::string fmt_string = 1 + R"( +class %1%: public EusSubscriberNode<%2%> +{ +public: + %1%(ros::NodeHandle& handle, const std::string& name, const NodeConfiguration& conf) : + EusSubscriberNode<%2%>(handle, name, conf) {} +%3% +}; +)"; + boost::format bfmt = boost::format(fmt_string) % + nodeID % + message_type % + provided_ports_body; + + return bfmt.str(); +} + + +std::string GenTemplate::remote_subscriber_class_template(std::string nodeID, + std::string message_type, + std::vector provided_ports) { + std::string provided_ports_body; + + if (!provided_ports.empty()) { + std::string fmt_string = R"( + static PortsList providedPorts() + { + return { +%1% + }; + })"; + + boost::format bfmt = boost::format(fmt_string) % + boost::algorithm::join(provided_ports, ",\n"); + + provided_ports_body = bfmt.str(); + } + + std::string fmt_string = 1 + R"( +class %1%: public EusRemoteSubscriberNode<%2%> +{ +public: + %1%(const std::string& name, const NodeConfiguration& conf) : + EusRemoteSubscriberNode<%2%>(name, conf) {} +%3% +}; +)"; + boost::format bfmt = boost::format(fmt_string) % + nodeID % + message_type % + provided_ports_body; + + return bfmt.str(); +} + + +std::string GenTemplate::main_function_template(std::string roscpp_node_name, + std::string program_description, + std::string xml_filename, + std::vector register_actions, + std::vector register_conditions, + std::vector register_subscribers) { + auto format_ros_init = [roscpp_node_name]() { + return fmt::format(" ros::init(argc, argv, \"{}\");", roscpp_node_name); + }; + auto format_create_tree = [xml_filename]() { + return fmt::format(" auto tree = factory.createTreeFromFile(\"{}\");", xml_filename); + }; + + std::string fmt_string = 1 + R"( +int main(int argc, char **argv) +{ + std::map init_variables; + if (!roseus_bt::parse_command_line(argc, argv, "%1%", init_variables)) { + return 1; + } + +%2% + ros::NodeHandle nh; + + BehaviorTreeFactory factory; + +%4%%5%%6% +%3% + roseus_bt::register_blackboard_variables(&tree, init_variables); + + std::string timestamp = std::to_string(ros::Time::now().toNSec()); + std::string log_filename(fmt::format("%7%", timestamp)); + + StdCoutLogger logger_cout(tree); + FileLogger logger_file(tree, log_filename.c_str()); + PublisherZMQ publisher_zmq(tree); + + NodeStatus status = NodeStatus::IDLE; + + while( ros::ok() && (status == NodeStatus::IDLE || status == NodeStatus::RUNNING)) + { + ros::spinOnce(); + status = tree.tickRoot(); + ros::Duration sleep_time(0.005); + sleep_time.sleep(); + } + + std::cout << "Writed log to file: " << log_filename << std::endl; + std::cout << "Behavior Tree execution finished with " << toStr(status, true).c_str() << std::endl; + return (status != NodeStatus::SUCCESS); +} +)"; + + if (register_actions.size() != 0) register_actions.push_back(""); + if (register_conditions.size() != 0) register_conditions.push_back(""); + if (register_subscribers.size() != 0) register_subscribers.push_back(""); + + boost::format file_format = boost::format("%1%/.ros/%2%_{0}.fbl") % + getenv("HOME") % + roscpp_node_name; + boost::format bfmt = boost::format(fmt_string) % + program_description % + format_ros_init() % + format_create_tree() % + boost::algorithm::join(register_actions, "\n") % + boost::algorithm::join(register_conditions, "\n") % + boost::algorithm::join(register_subscribers, "\n") % + file_format.str(); + + return bfmt.str(); +} + + +std::string GenTemplate::eus_server_template(std::string server_type, + std::string package_name, + std::vector callbacks, + std::vector instances, + std::vector load_files) { + auto format_ros_roseus = [server_type]() { + return fmt::format("(ros::roseus \"{}_server\")", server_type); + }; + auto format_load_ros_package = [package_name]() { + return fmt::format("(ros::load-ros-package \"{}\")", package_name); + }; + auto format_load_file = [](std::string filename) { + return fmt::format("(load \"{}\")", filename); + }; + + std::transform(load_files.begin(), load_files.end(), load_files.begin(), format_load_file); + if (load_files.size() != 0) load_files.push_back(""); + + std::string fmt_string = 1 + R"( +%1% +%2% +%3% +%4% + +;; define callbacks +%5% + +;; create server instances +%6% + +;; set rate +(ros::rate 100) + +;; spin +(roseus_bt:spin) +)"; + + boost::format bfmt = boost::format(fmt_string) % + format_ros_roseus() % + format_load_ros_package() % + "(load \"package://roseus_bt/euslisp/nodes.l\")" % + boost::algorithm::join(load_files, "\n") % + boost::algorithm::join(callbacks, "\n") % + boost::algorithm::join(instances, "\n"); + + return bfmt.str(); +} + +} // namespace RoseusBT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_GEN_TEMPLATE_ diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h new file mode 100644 index 000000000..21108232c --- /dev/null +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -0,0 +1,328 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_PACKAGE_GENERATOR_ +#define BEHAVIOR_TREE_ROSEUS_BT_PACKAGE_GENERATOR_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace RoseusBT +{ + +class Query +{ +private: + const std::regex yes, no; +public: + Query() : + yes("y|yes", std::regex::icase | std::regex::optimize), + no("n|no", std::regex::icase | std::regex::optimize) + {}; + ~Query() {}; + + bool yn(const std::string message); +}; + +template +class PackageGenerator +{ +public: + PackageGenerator(const std::string package_name, + const std::vector xml_filenames, + const std::vector target_filenames, + const std::string author_name, + bool force_overwrite) : + query(), + pkg_template(), + parser_vector(xml_filenames.begin(), xml_filenames.end()), + package_name(package_name), + xml_filenames(xml_filenames), + target_filenames(target_filenames), + author_name(author_name), + force_overwrite(force_overwrite) + { + // Check package name + std::regex valid_naming ("^[a-zA-Z0-9][a-zA-Z0-9_-]*$"); + if (!std::regex_match(package_name, valid_naming)) { + throw InvalidPackageName(package_name); + } + }; + + ~PackageGenerator() {}; + +private: + Query query; + PkgTemplate pkg_template; + std::vector parser_vector; + std::string package_name; + std::vector xml_filenames; + std::vector target_filenames; + std::vector euslisp_filenames; + std::string author_name; + bool force_overwrite; + +protected: + bool overwrite(const std::string filename); + +public: + void copy_xml_file(std::string* xml_filename); + void write_action_files(Parser* parser); + void write_service_files(Parser* parser); + void write_launch_file(Parser* parser, + const std::string target_filename); + void write_cpp_file(Parser* parser, + const std::string target_filename, const std::string xml_filename); + void write_eus_action_server(Parser* parser, const std::string target_filename); + void write_eus_condition_server(Parser* parser, const std::string target_filename); + void write_cmake_lists(const std::vector message_packages, + const std::vector service_files, + const std::vector action_files); + void write_package_xml(const std::vector message_packages); + void write_all_files(); +}; + + +bool Query::yn(const std::string message) { + auto prompt = [message]() { + std::cout << message << " [Y/n] "; + }; + + std::string answer; + + while(prompt(), std::cin >> answer) { + if (std::regex_match(answer, yes)) return true; + if (std::regex_match(answer, no)) return false; + } + + throw InvalidInput(); +} + +template +bool PackageGenerator::overwrite(const std::string filename) { + return force_overwrite || query.yn(fmt::format("Overwrite {}?", filename)); +} + +template +void PackageGenerator::copy_xml_file(std::string* xml_filename) { + std::string base_dir = fmt::format("{}/models", package_name); + std::string dest_file = fmt::format("{}/{}", + base_dir, + boost::filesystem::path(*xml_filename).filename().c_str()); + + if (dest_file == *xml_filename) + return; + + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) + return; + + BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; + boost::filesystem::create_directories(base_dir); + boost::filesystem::copy_file(*xml_filename, dest_file, + boost::filesystem::copy_option::overwrite_if_exists); + *xml_filename = dest_file; +} + +template +void PackageGenerator::write_action_files(Parser* parser) { + std::string base_dir = fmt::format("{}/action", package_name); + boost::filesystem::create_directories(base_dir); + + std::map action_files = parser->generate_all_action_files(); + + for (auto it=action_files.begin(); it!=action_files.end(); ++it) { + std::string dest_file = fmt::format("{}/{}", base_dir, it->first); + BOOST_LOG_TRIVIAL(debug) << "Writing " << dest_file << "..."; + std::ofstream output_file(dest_file); + output_file << it->second; + output_file.close(); + } +} + +template +void PackageGenerator::write_service_files(Parser* parser) { + std::string base_dir = fmt::format("{}/srv", package_name); + boost::filesystem::create_directories(base_dir); + + std::map service_files = parser->generate_all_service_files(); + + for (auto it=service_files.begin(); it!=service_files.end(); ++it) { + std::string dest_file = fmt::format("{}/{}", base_dir, it->first); + BOOST_LOG_TRIVIAL(debug) << "Writing " << dest_file << "..."; + std::ofstream output_file(dest_file); + output_file << it->second; + output_file.close(); + } +} + +template +void PackageGenerator::write_launch_file(Parser* parser, + const std::string target_filename) { + std::string base_dir = fmt::format("{}/launch", package_name); + boost::filesystem::create_directories(base_dir); + + std::string dest_file = fmt::format("{}/{}_server.launch", base_dir, target_filename); + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) + return; + + BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; + std::ofstream output_file(dest_file); + output_file << parser->generate_launch_file(package_name, euslisp_filenames); + output_file.close(); +} + +template +void PackageGenerator::write_cpp_file(Parser* parser, + const std::string target_filename, + const std::string xml_filename) { + std::string base_dir = fmt::format("{}/src", package_name); + boost::filesystem::create_directories(base_dir); + + std::string roscpp_node_name = fmt::format("{}_engine", target_filename); + std::string program_description = fmt::format("Run the {} task.", target_filename); + std::string dest_file = fmt::format("{}/{}.cpp", base_dir, target_filename); + + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) + return; + + BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; + std::ofstream output_file(dest_file); + output_file << parser->generate_cpp_file(package_name, roscpp_node_name, program_description, + boost::filesystem::absolute(xml_filename).c_str()); + output_file.close(); +} + +template +void PackageGenerator::write_eus_action_server(Parser* parser, + const std::string target_filename) { + std::string base_dir = fmt::format("{}/euslisp", package_name); + boost::filesystem::create_directories(base_dir); + + std::map server_files = parser->generate_all_eus_action_servers(package_name); + + for (auto it=server_files.begin(); it!=server_files.end(); ++it) { + std::string remote_host = it->first; + std::string body = it->second; + std::string euslisp_filename = fmt::format("{}{}-action-server", + target_filename, remote_host); + std::replace(euslisp_filename.begin(), euslisp_filename.end(), '_', '-'); + std::string dest_file = fmt::format("{}/{}.l", + base_dir, euslisp_filename); + if (body.empty()) continue; + euslisp_filenames.push_back(euslisp_filename); + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) continue; + + BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; + std::ofstream output_file(dest_file); + output_file << body; + output_file.close(); + } +} + +template +void PackageGenerator::write_eus_condition_server(Parser* parser, + const std::string target_filename) { + std::string base_dir = fmt::format("{}/euslisp", package_name); + boost::filesystem::create_directories(base_dir); + + std::map server_files = parser->generate_all_eus_condition_servers(package_name); + + for (auto it=server_files.begin(); it!=server_files.end(); ++it) { + std::string remote_host = it->first; + std::string body = it->second; + std::string euslisp_filename = fmt::format("{}{}-condition-server", + target_filename, remote_host); + std::replace(euslisp_filename.begin(), euslisp_filename.end(), '_', '-'); + std::string dest_file = fmt::format("{}/{}.l", + base_dir, euslisp_filename); + if (body.empty()) continue; + euslisp_filenames.push_back(euslisp_filename); + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) continue; + + BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; + std::ofstream output_file(dest_file); + output_file << body; + output_file.close(); + } +} + +template +void PackageGenerator::write_cmake_lists(const std::vector message_packages, + const std::vector service_files, + const std::vector action_files) { + std::string base_dir = package_name; + boost::filesystem::create_directories(base_dir); + + std::string dest_file = fmt::format("{}/CMakeLists.txt", base_dir); + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) + return; + + BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; + std::ofstream output_file(dest_file); + output_file << pkg_template.generate_cmake_lists(package_name, target_filenames, + message_packages, + service_files, + action_files); + output_file.close(); +} + +template +void PackageGenerator::write_package_xml(const std::vector message_packages) { + std::string base_dir = package_name; + boost::filesystem::create_directories(base_dir); + + std::string dest_file = fmt::format("{}/package.xml", base_dir); + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) + return; + + BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; + std::ofstream output_file(dest_file); + output_file << pkg_template.generate_package_xml(package_name, author_name, + message_packages); + output_file.close(); +} + +template +void PackageGenerator::write_all_files() { + std::vector message_packages; + std::vector action_files; + std::vector service_files; + + message_packages.push_back("std_msgs"); + message_packages.push_back("actionlib_msgs"); + + for (int i=0; ipush_dependencies(&message_packages, &service_files, &action_files); + + copy_xml_file(&xml_filename); + write_action_files(parser); + write_service_files(parser); + write_cpp_file(parser, target_filename, xml_filename); + write_eus_action_server(parser, target_filename); + write_eus_condition_server(parser, target_filename); + write_launch_file(parser, target_filename); + } + + write_cmake_lists(message_packages, service_files, action_files); + write_package_xml(message_packages); +} + + +} // namespaceRoseusBT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_PACKAGE_GENERATOR_ diff --git a/roseus_bt/include/roseus_bt/pkg_template.h b/roseus_bt/include/roseus_bt/pkg_template.h new file mode 100644 index 000000000..909f9a620 --- /dev/null +++ b/roseus_bt/include/roseus_bt/pkg_template.h @@ -0,0 +1,230 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_PKG_TEMPLATE_ +#define BEHAVIOR_TREE_ROSEUS_BT_PKG_TEMPLATE_ + +#include +#include +#include +#include +#include + +namespace RoseusBT +{ + +class PkgTemplate +{ +public: + PkgTemplate() {}; + ~PkgTemplate() {}; + +protected: + std::string cmake_lists_template(std::string package_name, + std::vector message_packages, + std::vector service_files, + std::vector action_files, + std::vector add_executables); + std::string package_xml_template(std::string package_name, + std::string author_name, + std::vector build_dependencies, + std::vector exec_dependencies); + +public: + std::string generate_cmake_lists(std::string package_name, + std::vector executables, + std::vector message_packages, + std::vector service_files, + std::vector action_files); + std::string generate_package_xml(std::string package_name, + std::string author_name, + std::vector message_packages); +}; + + +std::string PkgTemplate::cmake_lists_template(std::string package_name, + std::vector message_packages, + std::vector service_files, + std::vector action_files, + std::vector add_executables) { + std::string fmt_string = 1+ R"( +cmake_minimum_required(VERSION 2.8.3) +project(%1%) + +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + behaviortree_ros + roseus_bt +%2% +) +find_package(fmt) + +add_service_files( + FILES +%3% +) + +add_action_files( + FILES +%4% +) + +generate_messages( + DEPENDENCIES +%2% +) + +catkin_package( + INCLUDE_DIRS + LIBRARIES + CATKIN_DEPENDS + message_runtime +%2% + DEPENDS fmt +) + + +include_directories(${catkin_INCLUDE_DIRS}) +add_subdirectory(${roseus_bt_SOURCE_PREFIX}/include/rosbridgecpp rosbridgecpp) + +%5% +)"; + + boost::format bfmt = boost::format(fmt_string) % + package_name % + boost::algorithm::join(message_packages, "\n") % + boost::algorithm::join(service_files, "\n") % + boost::algorithm::join(action_files, "\n") % + boost::algorithm::join(add_executables, "\n"); + + return bfmt.str(); +} + + +std::string PkgTemplate::package_xml_template(std::string package_name, + std::string author_name, + std::vector build_dependencies, + std::vector exec_dependencies) { + + std::string author_email(author_name); + std::transform(author_email.begin(), author_email.end(), author_email.begin(), + [](unsigned char c){ return std::tolower(c); }); + std::replace(author_email.begin(), author_email.end(), ' ', '_'); + + std::string fmt_string = 1 + R"( + + + %1% + 0.0.0 + The %1% package + + + %2% + + + + + + TODO + + + + + + + + + + + + + + + catkin + message_generation + roscpp + behaviortree_ros + roseus_bt +%4% + + message_runtime + roscpp + behaviortree_ros + roseus_bt +%5% + + + + +)"; + + boost::format bfmt = boost::format(fmt_string) % + package_name % + author_name % + author_email % + boost::algorithm::join(build_dependencies, ",\n") % + boost::algorithm::join(exec_dependencies, ",\n"); + + return bfmt.str(); +} + + +std::string PkgTemplate::generate_cmake_lists(std::string package_name, + std::vector executables, + std::vector message_packages, + std::vector service_files, + std::vector action_files) { + auto format_pkg = [](std::string pkg) { + return fmt::format(" {}", pkg); + }; + + auto format_executable = [](std::string target_name) { + std::string fmt_string = 1+ R"( +add_executable(%1% src/%1%.cpp) +add_dependencies(%1% ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(%1% ${catkin_LIBRARIES} rosbridgecpp fmt::fmt) + +)"; + + boost::format bfmt = boost::format(fmt_string) % + target_name; + return bfmt.str(); + }; + + + std::transform(message_packages.begin(), message_packages.end(), + message_packages.begin(), format_pkg); + std::transform(executables.begin(), executables.end(), + executables.begin(), format_executable); + + return cmake_lists_template(package_name, message_packages, + service_files, action_files, + executables); +} + + +std::string PkgTemplate::generate_package_xml(std::string package_name, + std::string author_name, + std::vector message_packages) { + auto format_build_depend = [](std::string pkg) { + return fmt::format(" {}", pkg); + }; + auto format_exec_depend = [](std::string pkg) { + return fmt::format(" {}", pkg); + }; + std::vector build_dependencies; + std::vector exec_dependencies; + build_dependencies.resize(message_packages.size()); + exec_dependencies.resize(message_packages.size()); + + std::transform(message_packages.begin(), message_packages.end(), + build_dependencies.begin(), format_build_depend); + std::transform(message_packages.begin(), message_packages.end(), + exec_dependencies.begin(), format_exec_depend); + + return package_xml_template(package_name, author_name, + build_dependencies, exec_dependencies); +} + + +} // namespace RoseusBT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_PKG_TEMPLATE_ diff --git a/roseus_bt/include/roseus_bt/tutorial_parser.h b/roseus_bt/include/roseus_bt/tutorial_parser.h new file mode 100644 index 000000000..5202f7700 --- /dev/null +++ b/roseus_bt/include/roseus_bt/tutorial_parser.h @@ -0,0 +1,161 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_TUTORIAL_PARSER_ +#define BEHAVIOR_TREE_ROSEUS_BT_TUTORIAL_PARSER_ + +#include +#include + +namespace RoseusBT +{ + +using namespace tinyxml2; + +class TutorialParser : public XMLParser +{ + +public: + + TutorialParser(std::string filename): XMLParser(filename) {}; + + ~TutorialParser() {}; + +protected: + virtual std::string format_node_body(const XMLElement* node, int padding) override; + +public: + virtual std::string generate_eus_action_server(const std::string package_name) override; + virtual std::string generate_eus_remote_action_server(const std::string package_name, + const std::string remote_host) override; + virtual std::string generate_eus_condition_server(const std::string package_name) override; + virtual std::string generate_eus_remote_condition_server(const std::string package_name, + const std::string remote_host) override; +}; + + +std::string TutorialParser::format_node_body(const XMLElement* node, int padding) { + std::string pad(padding, ' '); + std::string id = node->Attribute("ID"); + std::vector param_list, output_list; + collect_param_list(node, ¶m_list, &output_list); + + // Conditions + if (id == "CheckTrue") return std::string("(send value :data)").insert(0, padding, ' '); + if (id == "atTable") return std::string("(at-spot \"table-front\")").insert(0, padding, ' '); + if (id == "atSpot" || id == "atTableSpot" || id == "atBroomSpot") + return fmt::format("{}(at-spot {})", pad, param_list.at(0)); + if (id == "CheckCoords") return fmt::format( + "{}{}", pad, param_list.at(0)); + + // Actions + if (id == "Init") return std::string("(init nil t)").insert(0, padding, ' '); + if (id == "InitWithBroom") return std::string("(init t t)").insert(0, padding, ' '); + if (id == "MoveToTable") return std::string("(go-to-spot \"table-front\")").insert(0, padding, ' '); + if (id == "MoveToBroom") return std::string("(go-to-spot \"broom-front\")").insert(0, padding, ' '); + if (id == "PickBottle") return std::string("(pick-sushi-bottle)").insert(0, padding, ' '); + if (id == "PourBottle") return std::string("(pour-sushi-bottle)").insert(0, padding, ' '); + + if (id == "PlaceBottle") + return fmt::format("{0}(place-sushi-bottle)\n{0}(reset-pose)", pad); + if (id == "MoveTo") + return fmt::format("{}(go-to-spot {})", pad, param_list.at(0)); + if (id == "PickBottleAt") + return fmt::format("{}(pick-sushi-bottle (ros::tf-pose->coords {}))", pad, param_list.at(0)); + if (id == "setCoords") { + std::string fmt_string = 1 + R"( +{0}(send server :set-output "{1}" +{0} (ros::coords->tf-pose (make-coords :pos #f(1850 400 700)))))"; + return fmt::format(fmt_string, pad, output_list.at(0)); + } + if (id == "SweepFloor") { + std::string fmt_string = 1 + R"( +{0}(handler-case (sweep-floor) +{0} (roseus_bt:cancel-action () nil)) +{0}(reset-pose))"; + return fmt::format(fmt_string, pad); + } + + throw XMLError::UnknownNode(node); +} + +std::string TutorialParser::generate_eus_action_server(const std::string package_name) { + + std::vector callback_definition; + std::vector instance_creation; + std::vector load_files; + + // Add load files + load_files.push_back("package://roseus_bt/sample/sample-task.l"); + + collect_eus_actions(package_name, &callback_definition, &instance_creation); + collect_eus_conditions(package_name, &callback_definition, &instance_creation, + NULL, NULL); + + if (callback_definition.empty()) return ""; + + return gen_template.eus_server_template("action", package_name, + callback_definition, instance_creation, + load_files); +} + +std::string TutorialParser::generate_eus_remote_action_server(const std::string package_name, + const std::string remote_host) { + + std::vector callback_definition; + std::vector instance_creation; + std::vector load_files; + + // Add load files + load_files.push_back("package://roseus_bt/sample/sample-task.l"); + + collect_eus_actions(package_name, &callback_definition, &instance_creation, remote_host); + collect_eus_conditions(package_name, &callback_definition, &instance_creation, + NULL, NULL, + remote_host); + + if (callback_definition.empty()) return ""; + + return gen_template.eus_server_template("action", package_name, + callback_definition, instance_creation, + load_files); +} + +std::string TutorialParser::generate_eus_condition_server(const std::string package_name) { + std::vector callback_definition; + std::vector instance_creation; + std::vector load_files; + + // Add load files + load_files.push_back("package://roseus_bt/sample/sample-task.l"); + + collect_eus_conditions(package_name, NULL, NULL, + &callback_definition, &instance_creation); + + if (callback_definition.empty()) return ""; + + return gen_template.eus_server_template("condition", package_name, + callback_definition, instance_creation, + load_files); +} + +std::string TutorialParser::generate_eus_remote_condition_server(const std::string package_name, + const std::string remote_host) { + std::vector callback_definition; + std::vector instance_creation; + std::vector load_files; + + // Add load files + load_files.push_back("package://roseus_bt/sample/sample-task.l"); + + collect_eus_conditions(package_name, NULL, NULL, + &callback_definition, &instance_creation, + remote_host); + + if (callback_definition.empty()) return ""; + + return gen_template.eus_server_template("condition", package_name, + callback_definition, instance_creation, + load_files); +} + +} // namespace RoseusBT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_TUTORIAL_PARSER_ diff --git a/roseus_bt/include/roseus_bt/ws_action_client.h b/roseus_bt/include/roseus_bt/ws_action_client.h new file mode 100644 index 000000000..6797f1ea3 --- /dev/null +++ b/roseus_bt/include/roseus_bt/ws_action_client.h @@ -0,0 +1,119 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_WS_ACTION_CLIENT_ +#define BEHAVIOR_TREE_ROSEUS_BT_WS_ACTION_CLIENT_ + +#include +#include + + +namespace roseus_bt +{ + +class RosbridgeActionClient +{ +public: + RosbridgeActionClient(const std::string& master, int port, const std::string& server_name, const std::string& action_type): + rbc_(fmt::format("{}:{}", master, std::to_string(port))), + server_name_(server_name), + is_active_(false) + { + goal_topic_ = fmt::format("{}/goal", server_name_); + result_topic_ = fmt::format("{}/result", server_name_); + cancel_topic_ = fmt::format("{}/cancel", server_name_); + feedback_topic_ = fmt::format("{}/feedback", server_name_); + + action_goal_type_ = fmt::format("{}Goal", action_type); + + rbc_.addClient("goal_advertiser"); + rbc_.addClient("cancel_advertiser"); + rbc_.addClient("result_subscriber"); + rbc_.addClient("feedback_subscriber"); + rbc_.advertise("goal_advertiser", goal_topic_, action_goal_type_); + rbc_.advertise("cancel_advertiser", cancel_topic_, "actionlib_msgs/GoalID"); + auto res_sub = std::bind(&RosbridgeActionClient::resultCallback, this, + std::placeholders::_1, + std::placeholders::_2); + rbc_.subscribe("result_subscriber", result_topic_, res_sub); + } + + ~RosbridgeActionClient() { + rbc_.removeClient("goal_advertiser"); + rbc_.removeClient("cancel_advertiser"); + rbc_.removeClient("result_subscriber"); + rbc_.removeClient("feedback_subscriber"); + } + + void registerFeedbackCallback(auto callback) { + rbc_.subscribe("feedback_subscriber", feedback_topic_, callback); + } + + void sendGoal(const rapidjson::Value& goal) { + // TODO: add header and goal_id + + rapidjson::Document action_goal; + action_goal.SetObject(); + rapidjson::Value g(goal, action_goal.GetAllocator()); + action_goal.AddMember("goal", g, action_goal.GetAllocator()); + + is_active_ = true; + rbc_.publish(goal_topic_, action_goal); + } + + void cancelGoal() { + rapidjson::Document msg; + msg.SetObject(); + rbc_.publish(cancel_topic_, msg); + } + + bool isActive() { + return is_active_; + } + + rapidjson::Value getResult() { + // TODO: reset result after getting + return result_["msg"].GetObject()["result"].GetObject(); + } + + void waitForResult() { + std::cout << "RemoteAction: waiting for result: " << result_topic_ << std::endl; + while (is_active_) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + +// waitForServer + +protected: + RosbridgeWsClient rbc_; + + bool is_active_; + rapidjson::Value result_; + + std::string server_name_; + std::string goal_topic_; + std::string result_topic_; + std::string cancel_topic_; + std::string feedback_topic_; + + std::string action_goal_type_; + +protected: + + void resultCallback(std::shared_ptr connection, std::shared_ptr in_message) + { + std::string message = in_message->string(); +#ifdef DEBUG + std::cout << "resultCallback(): Message Received: " << message << std::endl; +#endif + + rapidjson::Document document(rapidjson::kObjectType); + document.Parse(message.c_str()); + rapidjson::Value res(document, document.GetAllocator()); + result_ = res; + + is_active_ = false; + } +}; + +} // namespace roseus_bt + +#endif // BEHAVIOR_TREE_ROSEUS_BT_WS_ACTION_CLIENT_ diff --git a/roseus_bt/include/roseus_bt/ws_service_client.h b/roseus_bt/include/roseus_bt/ws_service_client.h new file mode 100644 index 000000000..dbe3e8734 --- /dev/null +++ b/roseus_bt/include/roseus_bt/ws_service_client.h @@ -0,0 +1,82 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_WS_SERVICE_CLIENT_ +#define BEHAVIOR_TREE_ROSEUS_BT_WS_SERVICE_CLIENT_ + +#include +#include + + +namespace roseus_bt +{ + +class RosbridgeServiceClient +{ +public: + RosbridgeServiceClient(const std::string& master, int port, const std::string& service_name): + rbc_(fmt::format("{}:{}", master, std::to_string(port))), + service_name_(service_name), + is_active_(false) + {} + + ~RosbridgeServiceClient() {} + + bool call(const rapidjson::Document& request) { + auto service_cb = std::bind(&RosbridgeServiceClient::serviceCallback, this, + std::placeholders::_1, + std::placeholders::_2); + is_active_ = true; + rbc_.callService(service_name_, service_cb, request); + return true; + } + + void cancelRequest() { + // connection->send_close(1000); + } + + bool isActive() { + return is_active_; + } + + rapidjson::Value getResult() { + // TODO: reset result after getting + return result_["values"].GetObject(); + } + + void waitForResult() { + std::cout << "RemoteService: waiting for result: " << service_name_ << std::endl; + while (is_active_) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + +// waitForServer + +protected: + RosbridgeWsClient rbc_; + + bool is_active_; + rapidjson::Value result_; + + std::string service_name_; + +protected: + + void serviceCallback(std::shared_ptr connection, std::shared_ptr in_message) + { + std::string message = in_message->string(); +#ifdef DEBUG + std::cout << "serviceResponseCallback(): Message Received: " << message << std::endl; +#endif + + rapidjson::Document document(rapidjson::kObjectType); + document.Parse(message.c_str()); + rapidjson::Value res(document, document.GetAllocator()); + result_ = res; + + is_active_ = false; + connection->send_close(1000); + } +}; + +} // namespace roseus_bt + +#endif // BEHAVIOR_TREE_ROSEUS_BT_WS_SERVICE_CLIENT_ diff --git a/roseus_bt/include/roseus_bt/ws_subscriber_client.h b/roseus_bt/include/roseus_bt/ws_subscriber_client.h new file mode 100644 index 000000000..324cbe2ee --- /dev/null +++ b/roseus_bt/include/roseus_bt/ws_subscriber_client.h @@ -0,0 +1,40 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_WS_SUBSCRIBER_CLIENT_ +#define BEHAVIOR_TREE_ROSEUS_BT_WS_SUBSCRIBER_CLIENT_ + +#include +#include + + +namespace roseus_bt +{ + +class RosbridgeSubscriberClient +{ +public: + RosbridgeSubscriberClient(const std::string& master, int port, + const std::string& topic_name, + const std::string& topic_type): + rbc_(fmt::format("{}:{}", master, std::to_string(port))), + topic_name_(topic_name), + topic_type_(topic_type) + { + rbc_.addClient("topic_subscriber"); + } + + ~RosbridgeSubscriberClient() { + rbc_.removeClient("topic_subscriber"); + } + + void registerCallback(auto callback) { + rbc_.subscribe("topic_subscriber", topic_name_, callback, "", topic_type_); + } + +protected: + RosbridgeWsClient rbc_; + std::string topic_name_; + std::string topic_type_; +}; + +} // namespace roseus_bt + +#endif // BEHAVIOR_TREE_ROSEUS_BT_WS_SUBSCRIBER_CLIENT_ diff --git a/roseus_bt/include/roseus_bt/xml_exceptions.h b/roseus_bt/include/roseus_bt/xml_exceptions.h new file mode 100644 index 000000000..23edb6121 --- /dev/null +++ b/roseus_bt/include/roseus_bt/xml_exceptions.h @@ -0,0 +1,87 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_XML_EXCEPTIONS_ +#define BEHAVIOR_TREE_ROSEUS_BT_XML_EXCEPTIONS_ + +#include +#include +#include +#include + + +namespace XMLError +{ + +using namespace tinyxml2; + +class XMLError: public std::exception { +public: + XMLError(std::string message): message(message) {} + + const char* what() const noexcept { + return message.c_str(); + } + + std::string message; +}; + +std::string get_place(const XMLElement* node) { + XMLPrinter printer; + node->Accept(&printer); + return fmt::format(" at line {}: {}", node->GetLineNum(), printer.CStr()); +}; + +class FileNotFound: public XMLError { +public: + FileNotFound(std::string filename) : + XMLError(fmt::format("File not found: {}", filename)) {}; +}; + +class ParsingError: public XMLError { +public: + ParsingError() : XMLError("Could not parse the XML file") {}; +}; + +class WrongRoot: public XMLError { +public: + WrongRoot() : XMLError("The XML must have a root node called ") {}; +}; + +class MissingRequiredNode: public XMLError { +public: + MissingRequiredNode(std::string node_type) : + XMLError(fmt::format("The XML must have a <{}> node", node_type)) {}; +}; + +class MissingRequiredAttribute: public XMLError { +public: + MissingRequiredAttribute(std::string attribute, const XMLElement* node) : + XMLError(fmt::format("Missing \"{}\" attribute{}", attribute, get_place(node))) + {}; +}; + +class UnknownNode: public XMLError { +public: + UnknownNode(const XMLElement* node) : + XMLError(fmt::format("Unknown node type {}{}", node->Name(), get_place(node))) {}; +}; + +class UnknownPortNode: public XMLError { +public: + UnknownPortNode(const XMLElement* node) : + XMLError(fmt::format("Unknown port node {}{}", node->Name(), get_place(node))) {}; +}; + +class InvalidTopicType: public XMLError { +public: + InvalidTopicType(std::string type, const XMLElement* node) : + XMLError(fmt::format("Invalid topic type {}{}", type, get_place(node))) {}; +}; + +class InvalidPortType: public XMLError { +public: + InvalidPortType(std::string type, const XMLElement* node) : + XMLError(fmt::format("Invalid port type {}{}", type, get_place(node))) {}; +}; + +} // namespace XMLError + +#endif // BEHAVIOR_TREE_ROSEUS_BT_XML_EXCEPTIONS_ diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h new file mode 100644 index 000000000..c2edf456a --- /dev/null +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -0,0 +1,1391 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ +#define BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ + +#include +#include +#include +#include +#include +#include +#include +#include + +void push_new(std::string elem, std::vector* vec) { + if (std::find(vec->begin(), vec->end(), elem) == vec->end()) { + vec->push_back(elem); + } +} + +namespace RoseusBT +{ + +using namespace tinyxml2; + +class XMLParser +{ + +public: + + XMLParser(std::string filename) { + if (!boost::filesystem::exists(filename)) { + throw XMLError::FileNotFound(filename); + } + BOOST_LOG_TRIVIAL(debug) << "Initializing XMLParser from " << filename << "..."; + doc.LoadFile(filename.c_str()); + check_xml_file(filename); + } + + ~XMLParser() {}; + +protected: + + XMLDocument doc; + static GenTemplate gen_template; + void check_xml_file(std::string filename); + bool is_reactive(const XMLElement* node); + bool is_reactive_base(const XMLElement* node, const XMLElement* ref_node, bool reactive_parent); + void collect_param_list(const XMLElement* node, std::vector* param_list, + std::vector* output_list, + std::function param_fn, + std::function output_fn); + void collect_param_list(const XMLElement* node, std::vector* param_list, + std::vector* output_list); + void collect_remote_hosts(std::vector* host_list); + void collect_eus_actions(const std::string package_name, + std::vector* callback_definition, + std::vector* instance_creation, + const std::string remote_host); + void collect_eus_actions(const std::string package_name, + std::vector* callback_definition, + std::vector* instance_creation); + void collect_eus_conditions(const std::string package_name, + std::vector* callback_definition, + std::vector* instance_creation, + std::vector* parallel_callback_definition, + std::vector* parallel_instance_creation, + const std::string remote_host); + void collect_eus_conditions(const std::string package_name, + std::vector* callback_definition, + std::vector* instance_creation, + std::vector* parallel_callback_definition, + std::vector* parallel_instance_creation); + void maybe_push_message_package(const std::string message_type, + std::vector* message_packages); + std::string format_eus_name(const std::string input); + std::string format_message_description(const XMLElement* port_node); + std::string format_server_name(const XMLElement* node); + std::string format_service_name(const XMLElement* node); + std::string format_host_name(const XMLElement* node); + std::string format_host_port(const XMLElement* node); + std::string format_remote_host(const XMLElement* node); + std::string format_get_remote_input(const XMLElement* node, const std::string name); + std::string format_set_remote_output(const XMLElement* node); + std::string format_launch_node(const std::string package_name, + const std::string euslisp_filename); + std::string generate_action_file_contents(const XMLElement* node); + std::string generate_service_file_contents(const XMLElement* node); + std::string generate_headers(const std::string package_name); + std::string generate_action_class(const XMLElement* node, const std::string package_name); + std::string generate_remote_action_class(const XMLElement* node, const std::string package_name); + std::string generate_condition_class(const XMLElement* node, const std::string package_name); + std::string generate_remote_condition_class(const XMLElement* node, const std::string package_name); + std::string generate_subscriber_class(const XMLElement* node); + std::string generate_remote_subscriber_class(const XMLElement* node); + std::string generate_main_function(const std::string roscpp_node_name, + const std::string program_description, + const std::string xml_filename); + + virtual std::string format_node_body(const XMLElement* node, int padding); + + virtual std::string generate_eus_action_server(const std::string package_name); + virtual std::string generate_eus_remote_action_server(const std::string package_name, + const std::string remote_host); + virtual std::string generate_eus_condition_server(const std::string package_name); + virtual std::string generate_eus_remote_condition_server(const std::string package_name, + const std::string remote_host); + +public: + + std::map generate_all_action_files(); + std::map generate_all_service_files(); + std::map generate_all_eus_action_servers(const std::string package_name); + std::map generate_all_eus_condition_servers(const std::string package_name); + std::string generate_cpp_file(const std::string package_name, + const std::string roscpp_node_name, + const std::string program_description, + const std::string xml_filename); + std::string generate_launch_file(const std::string package_name, + std::vector euslisp_filenames); + void push_dependencies(std::vector* message_packages, + std::vector* action_files, + std::vector* service_files); +}; + +GenTemplate XMLParser::gen_template = GenTemplate(); + +void XMLParser::check_xml_file(std::string filename) { + auto check_push = [this](XMLElement* node, std::vector* vec, + std::vector *duplicated_nodes) { + if (std::find(vec->begin(), vec->end(), node->Attribute("ID")) == vec->end()) { + vec->push_back(node->Attribute("ID")); + } + else { + duplicated_nodes->push_back(node); + } + }; + + XMLElement* root = doc.RootElement(); + if (root == nullptr) { + throw XMLError::ParsingError(); + } + if (std::string(root->Name()) != "root") { + throw XMLError::WrongRoot(); + } + + XMLElement* bt_root = root->FirstChildElement("TreeNodesModel"); + std::vector actions, conditions, subscribers; + std::vector duplicated_nodes; + + if (bt_root == nullptr) { + throw XMLError::MissingRequiredNode("TreeNodesModel"); + } + + // check tree model + for (auto node = bt_root->FirstChildElement(); + node != nullptr; + node = node->NextSiblingElement()) { + + std::string name = node->Name(); + + if (name != "Action" && + name != "RemoteAction" && + name != "Condition" && + name != "RemoteCondition" && + name != "Subscriber" && + name != "RemoteSubscriber" && + name != "SubTree") { + throw XMLError::UnknownNode(node); + } + + if (!node->Attribute("ID")) { + throw XMLError::MissingRequiredAttribute("ID", node); + } + + if (name == "Action" || name == "RemoteAction") { + if (!node->Attribute("server_name")) { + throw XMLError::MissingRequiredAttribute("server_name", node); + } + check_push(node, &actions, &duplicated_nodes); + } + + if (name == "Condition" || name == "RemoteCondition") { + if (!node->Attribute("service_name")) { + throw XMLError::MissingRequiredAttribute("service_name", node); + } + check_push(node, &conditions, &duplicated_nodes); + } + + if (name == "Subscriber" || name == "RemoteSubscriber") { + if (!node->Attribute("message_type")) { + throw XMLError::MissingRequiredAttribute("message_type", node); + } + check_push(node, &subscribers, &duplicated_nodes); + } + + // check ports + for (auto port_node = node->FirstChildElement(); + port_node != nullptr; + port_node = port_node->NextSiblingElement()) { + + std::string port_name = port_node->Name(); + + if (port_name != "input_port" && + port_name != "output_port" && + port_name != "inout_port") { + throw XMLError::UnknownPortNode(port_node); + } + + if (!port_node->Attribute("name")) { + throw XMLError::MissingRequiredAttribute("name", port_node); + } + + if (!port_node->Attribute("type")) { + throw XMLError::MissingRequiredAttribute("type", port_node); + } + } + } + + // delete duplicated nodes + for (int i = 0; i < duplicated_nodes.size(); i++) { + XMLElement* node = duplicated_nodes.at(i); + BOOST_LOG_TRIVIAL(warning) << fmt::format("Ignoring duplicated {} node {} at {} line {}", + node->Name(), node->Attribute("ID"), filename, node->GetLineNum()); + doc.DeleteNode(node); + } +} + +bool XMLParser::is_reactive(const XMLElement* node) { + const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); + + for (auto bt_node = bt_root; + bt_node != nullptr; + bt_node = bt_node->NextSiblingElement("BehaviorTree")) { + if (is_reactive_base(bt_node, node, false)) + return true; + } + return false; +} + +bool XMLParser::is_reactive_base(const XMLElement* node, const XMLElement* ref_node, + bool reactive_parent) { + + std::string name = node->Name(); + BOOST_LOG_TRIVIAL(trace) << "is_reactive_base: transversing " << name << "..."; + + // is possibly reactive control node + if (name.find("Fallback") != std::string::npos || + name.find("Sequence") != std::string::npos) { + reactive_parent = (name.find("Reactive") != std::string::npos); + } + + // parent node is reactive, node name and ID matches + if (reactive_parent && + !std::strcmp(node->Name(), ref_node->Name()) && + !std::strcmp(node->Attribute("ID"), ref_node->Attribute("ID"))) { + return true; + } + + for (auto child_node = node->FirstChildElement(); + child_node != nullptr; + child_node = child_node->NextSiblingElement()) + { + if (is_reactive_base(child_node, ref_node, reactive_parent)) + return true; + } + return false; +} + +void XMLParser::collect_param_list(const XMLElement* node, + std::vector* param_list, + std::vector* output_list, + std::function param_fn, + std::function output_fn) { + + for (auto port_node = node->FirstChildElement(); + port_node != nullptr; + port_node = port_node->NextSiblingElement()) + { + std::string name = port_node->Name(); + std::string port_name = port_node->Attribute("name"); + BOOST_LOG_TRIVIAL(trace) << "collect_param_list: transversing " << + name << ": " << port_name << "..."; + + if (name == "input_port" || name == "inout_port") { + if (param_list != NULL) { + BOOST_LOG_TRIVIAL(trace) << "collect_param_list: collecting input: " << + port_name << "..."; + param_list->push_back(param_fn(port_node)); + } + } + if (name == "output_port" || name == "inout_port") { + if (output_list != NULL) { + BOOST_LOG_TRIVIAL(trace) << "collect_param_list: collecting output: " << + port_name << "..."; + output_list->push_back(output_fn(port_node)); + } + } + } +} + +void XMLParser::collect_param_list(const XMLElement* node, + std::vector* param_list, + std::vector* output_list) { + auto format_port = [](const XMLElement* port_node) { + return port_node->Attribute("name"); + }; + collect_param_list(node, param_list, output_list, format_port, format_port); +} + +void XMLParser::collect_remote_hosts(std::vector* host_list) { + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); + + for (auto node = root->FirstChildElement(); + node != nullptr; + node = node->NextSiblingElement()) + { + std::string name = node->Name(); + if (name == "RemoteAction" || name == "RemoteCondition") { + if (node->Attribute("host_name") && node->Attribute("host_port")) { + push_new(format_remote_host(node), host_list); + } + } + } +} + +void XMLParser::collect_eus_actions(const std::string package_name, + std::vector* callback_definition, + std::vector* instance_creation, + const std::string remote_host) +{ + auto format_action_param = [](const XMLElement* node) { + return fmt::format("({0} (send goal :goal :{0}))", node->Attribute("name")); + }; + auto format_callback = [this, format_action_param](const XMLElement* node) { + std::vector param_list; + collect_param_list(node, ¶m_list, NULL, format_action_param, NULL); + + // has parameters + if (param_list.size()) { + std::string fmt_string = 1 + R"( +(defun {0}-execute-cb (server goal) + (let ({1}) +{2} + t)) +)"; + return fmt::format(fmt_string, + format_eus_name(node->Attribute("ID")), + boost::algorithm::join(param_list, "\n "), + format_node_body(node, 4)); + } + + // no parameters + std::string fmt_string = 1 + R"( +(defun {0}-execute-cb (server goal) +{1} + t) +)"; + return fmt::format(fmt_string, + format_eus_name(node->Attribute("ID")), + format_node_body(node, 2)); + }; + + auto format_instance = [this, package_name](const XMLElement* node, std::string server_name) { + std::string fmt_string = 1 + R"( +(instance roseus_bt:action-node :init + "{3}" {0}::{1}Action + :execute-cb '{2}-execute-cb) +)"; + return fmt::format(fmt_string, + package_name, + node->Attribute("ID"), + format_eus_name(node->Attribute("ID")), + server_name); + }; + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + + if (remote_host.empty()) { + for (auto node = root->FirstChildElement("Action"); + node != nullptr; + node = node->NextSiblingElement("Action")) + { + std::string server_name = node->Attribute("server_name"); + BOOST_LOG_TRIVIAL(trace) << "collect_eus_actions: transversing " << server_name << "..."; + push_new(format_callback(node), callback_definition); + push_new(format_instance(node, server_name), instance_creation); + } + } + else { + for (auto node = root->FirstChildElement("RemoteAction"); + node != nullptr; + node = node->NextSiblingElement("RemoteAction")) + { + std::string server_name = node->Attribute("server_name"); + if (!node->Attribute("host_name") || !node->Attribute("host_port")) { + BOOST_LOG_TRIVIAL(warning) << + fmt::format("Ignoring {} node {} with improper host specification at line {}", + node->Name(), node->Attribute("ID"), node->GetLineNum()); + continue; + } + if (format_remote_host(node) != remote_host) { + BOOST_LOG_TRIVIAL(trace) << "collect_eus_actions: skipping " << server_name << "..."; + continue; + } + BOOST_LOG_TRIVIAL(trace) << "collect_eus_actions: transversing " << server_name << "..."; + push_new(format_callback(node), callback_definition); + push_new(format_instance(node, server_name), instance_creation); + } + } +} + +void XMLParser::collect_eus_actions(const std::string package_name, + std::vector* callback_definition, + std::vector* instance_creation) +{ + collect_eus_actions(package_name, callback_definition, instance_creation, ""); +} + +void XMLParser::collect_eus_conditions(const std::string package_name, + std::vector* callback_definition, + std::vector* instance_creation, + std::vector* parallel_callback_definition, + std::vector* parallel_instance_creation, + const std::string remote_host) +{ + auto format_condition_param = [](const XMLElement* node) { + return fmt::format("({0} (send request :{0}))", node->Attribute("name")); + }; + auto format_callback = [this, format_condition_param](const XMLElement* node) { + std::vector param_list; + collect_param_list(node, ¶m_list, NULL, format_condition_param, NULL); + + // has parameters + if (param_list.size()) { + std::string fmt_string = 1 + R"( +(defun {0}-cb (server request) + (let ({1}) +{2} + )) +)"; + + return fmt::format(fmt_string, + format_eus_name(node->Attribute("ID")), + boost::algorithm::join(param_list, "\n "), + format_node_body(node, 4)); + } + + // no parameters + std::string fmt_string = 1 + R"( +(defun {0}-cb (server request) +{1} + ) +)"; + return fmt::format(fmt_string, + format_eus_name(node->Attribute("ID")), + format_node_body(node, 2)); + }; + + auto format_instance = [this, package_name](const XMLElement* node, std::string service_name) { + std::string fmt_string = 1 + R"( +(instance roseus_bt:condition-node :init + "{3}" {0}::{1} + :execute-cb '{2}-cb) +)"; + return fmt::format(fmt_string, + package_name, + node->Attribute("ID"), + format_eus_name(node->Attribute("ID")), + service_name); + }; + + auto maybe_push = [=] (const XMLElement* node, std::string service_name) { + if (is_reactive(node)) { + if (parallel_callback_definition != NULL && + parallel_instance_creation != NULL) { + push_new(format_callback(node), parallel_callback_definition); + push_new(format_instance(node, service_name), parallel_instance_creation); + } + } + else { + if (callback_definition != NULL && instance_creation != NULL) { + push_new(format_callback(node), callback_definition); + push_new(format_instance(node, service_name), instance_creation); + } + } + }; + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + + if (remote_host.empty()) { + for (auto node = root->FirstChildElement("Condition"); + node != nullptr; + node = node->NextSiblingElement("Condition")) + { + std::string service_name = node->Attribute("service_name"); + BOOST_LOG_TRIVIAL(trace) << "collect_eus_conditions: transversing " << service_name << "..."; + maybe_push(node, service_name); + } + } + else { + for (auto node = root->FirstChildElement("RemoteCondition"); + node != nullptr; + node = node->NextSiblingElement("RemoteCondition")) + { + std::string service_name = node->Attribute("service_name"); + if (!node->Attribute("host_name") || !node->Attribute("host_port")) { + BOOST_LOG_TRIVIAL(warning) << + fmt::format("Ignoring {} node {} with improper host specification at line {}", + node->Name(), node->Attribute("ID"), node->GetLineNum()); + continue; + } + if (format_remote_host(node) != remote_host) { + BOOST_LOG_TRIVIAL(trace) << "collect_eus_conditions: skipping " << service_name << "..."; + continue; + } + BOOST_LOG_TRIVIAL(trace) << "collect_eus_conditions: transversing " << service_name << "..."; + maybe_push(node, service_name); + } + } +} + +void XMLParser::collect_eus_conditions(const std::string package_name, + std::vector* callback_definition, + std::vector* instance_creation, + std::vector* parallel_callback_definition, + std::vector* parallel_instance_creation) +{ + collect_eus_conditions(package_name, callback_definition, instance_creation, + parallel_callback_definition, parallel_instance_creation, + ""); +} + +void XMLParser::maybe_push_message_package(const std::string message_type, + std::vector* message_packages) { + std::size_t pos = message_type.find('/'); + if (pos != std::string::npos) { + std::string pkg = message_type.substr(0, pos); + push_new(pkg, message_packages); + } +} + +std::string XMLParser::format_eus_name(const std::string input) { + std::regex e ("([^A-Z]+)([A-Z]+)"); + std::string out = std::regex_replace(input, e, "$1-$2"); + std::transform(out.begin(), out.end(), out.begin(), + [](unsigned char c){ return std::tolower(c); }); + return out; +} + +std::string XMLParser::format_node_body(const XMLElement* node, int padding) { + auto format_setoutput = [padding](const XMLElement* port_node) { + return fmt::format(";; (send server :set-output \"{0}\" <{1}>)", + port_node->Attribute("name"), + port_node->Attribute("type")). + insert(0, padding, ' '); + }; + + std::vector output_list; + output_list.push_back(std::string(";; do something").insert(0, padding, ' ')); + collect_param_list(node, NULL, &output_list, NULL, format_setoutput); + + return boost::algorithm::join(output_list, "\n"); +} + +std::string XMLParser::format_message_description(const XMLElement* port_node) { + std::string output = fmt::format("{} {}", + port_node->Attribute("type"), + port_node->Attribute("name")); + return output; +} + +std::string XMLParser::format_server_name(const XMLElement* node) { + std::string output = fmt::format( + " InputPort(\"server_name\", \"{0}\", \"name of the Action Server\")", + node->Attribute("server_name")); + return output; +} + +std::string XMLParser::format_service_name(const XMLElement* node) { + std::string output = fmt::format( + " InputPort(\"service_name\", \"{0}\", \"name of the ROS service\")", + node->Attribute("service_name")); + return output; +} + +std::string XMLParser::format_host_name(const XMLElement* node) { + std::string output = fmt::format( + " InputPort(\"host_name\", \"{0}\", \"name of the rosbridge_server host\")", + node->Attribute("host_name")); + return output; + } + +std::string XMLParser::format_host_port(const XMLElement* node) { + std::string output = fmt::format( + " InputPort(\"host_port\", {0}, \"port of the rosbridge_server host\")", + node->Attribute("host_port")); + return output; +} + +std::string XMLParser::format_remote_host(const XMLElement* node) { + std::string output = fmt::format("_{0}{1}", + node->Attribute("host_name"), + node->Attribute("host_port")); + return output; +} + +std::string XMLParser::format_get_remote_input(const XMLElement* node, const std::string name) { + auto format_setvalue = [name](const XMLElement* node) { + // all ros types defined in: http://wiki.ros.org/msg + // TODO: accept arrays + std::string msg_type = node->Attribute("type"); + if (msg_type == "bool") + return fmt::format("SetBool(ros_msg.{0})", node->Attribute("name")); + if (msg_type == "int8" || msg_type == "int16" || msg_type == "int32") + return fmt::format("SetInt(ros_msg.{0})", node->Attribute("name")); + if (msg_type == "uint8" || msg_type == "uint16" || msg_type == "uint32") + return fmt::format("SetUint(ros_msg.{0})", node->Attribute("name")); + if (msg_type == "int64") + return fmt::format("SetInt64(ros_msg.{0})", node->Attribute("name")); + if (msg_type == "uint64") + return fmt::format("SetUint64(ros_msg.{0})", node->Attribute("name")); + if (msg_type == "float32" || msg_type == "float64") + return fmt::format("SetDouble(ros_msg.{0})", node->Attribute("name")); + if (msg_type == "string") + return fmt::format("SetString(ros_msg.{0}.c_str(), ros_msg.{0}.size(), {1}->GetAllocator())", + node->Attribute("name"), name); + // time, duration + throw XMLError::InvalidPortType(msg_type, node); + }; + + std::string msg_type = node->Attribute("type"); + if (msg_type.find('/') != std::string::npos) + return fmt::format(R"( + getInput("{0}", document); + rapidjson::Value {0}(document, document.GetAllocator()); + {1}->AddMember("{0}", {0}, {1}->GetAllocator());)", + node->Attribute("name"), + name); + return fmt::format(R"( + getInput("{0}", ros_msg.{0}); + rapidjson::Value {0}; + {0}.{2}; + {1}->AddMember("{0}", {0}, {1}->GetAllocator());)", + node->Attribute("name"), + name, + format_setvalue(node)); +} + +std::string XMLParser::format_set_remote_output(const XMLElement* node) { + auto format_getvalue = [](const XMLElement* node) { + // all ros types defined in: http://wiki.ros.org/msg + // TODO: accept arrays + std::string msg_type = node->Attribute("type"); + if (msg_type == "bool") + return "GetBool()"; + if (msg_type == "int8" || msg_type == "int16" || msg_type == "int32") + return "GetInt()"; + if (msg_type == "uint8" || msg_type == "uint16" || msg_type == "uint32") + return "GetUint"; + if (msg_type == "int64") + return "GetInt64()"; + if (msg_type == "uint64") + return "GetUint64()"; + if (msg_type == "float32" || msg_type == "float64") + return "GetDouble()"; + if (msg_type == "string") + return "GetString()"; + // time, duration + throw XMLError::InvalidPortType(msg_type, node); + }; + + std::string msg_type = node->Attribute("type"); + if (msg_type.find('/') != std::string::npos) + return fmt::format(1 + R"( + setOutputFromMessage("{0}", feedback);)", + node->Attribute("name")); + return fmt::format(1 + R"( + if (feedback["update_field_name"].GetString() == std::string("{0}")) {{ + setOutput("{0}", feedback["{0}"].{1}); + }})", + node->Attribute("name"), + format_getvalue(node)); +} + +std::string XMLParser::format_launch_node(const std::string package_name, + const std::string euslisp_filename) { + std::string fmt_string = 1 + R"( + +)"; + + std::string node_name = euslisp_filename; + std::replace(node_name.begin(), node_name.end(), '-', '_'); + + boost::format bfmt = boost::format(fmt_string) % + package_name % + euslisp_filename % + node_name; + return bfmt.str(); + }; + +std::string XMLParser::generate_action_file_contents(const XMLElement* node) { + std::vector goal, feedback; + + for (auto port_node = node->FirstChildElement(); + port_node != nullptr; + port_node = port_node->NextSiblingElement()) + { + std::string name = port_node->Name(); + std::string text = format_message_description(port_node); + + if (name == "input_port" || name == "inout_port") { + goal.push_back(text); + } + if (name == "output_port" || name == "inout_port") { + feedback.push_back(text); + } + } + + return gen_template.action_file_template(goal, feedback); +} + +std::string XMLParser::generate_service_file_contents(const XMLElement* node) { + std::vector request; + + for (auto port_node = node->FirstChildElement(); + port_node != nullptr; + port_node = port_node->NextSiblingElement()) + { + std::string name = port_node->Name(); + std::string text = format_message_description(port_node); + + if (name == "input_port") { + request.push_back(text); + } + else { + throw InvalidOutputPort(); + } + } + + return gen_template.service_file_template(request); +} + +std::string XMLParser::generate_headers(const std::string package_name) { + auto format_action_node = [](const XMLElement* node, std::string package_name) { + return fmt::format("#include <{}/{}Action.h>", + package_name, + node->Attribute("ID")); + }; + + auto format_condition_node = [](const XMLElement* node, std::string package_name) { + return fmt::format("#include <{}/{}.h>", + package_name, + node->Attribute("ID")); + }; + + auto format_subscriber_node = [](const XMLElement* node) { + return fmt::format("#include <{}.h>", node->Attribute("message_type")); + }; + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + std::vector headers; + + for (auto action_node = root->FirstChildElement("Action"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("Action")) + { + push_new(format_action_node(action_node, package_name), &headers); + } + + for (auto action_node = root->FirstChildElement("RemoteAction"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("RemoteAction")) + { + push_new(format_action_node(action_node, package_name), &headers); + } + + for (auto condition_node = root->FirstChildElement("Condition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("Condition")) + { + push_new(format_condition_node(condition_node, package_name), &headers); + } + + for (auto condition_node = root->FirstChildElement("RemoteCondition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("RemoteCondition")) + { + push_new(format_condition_node(condition_node, package_name), &headers); + } + + for (auto subscriber_node = root->FirstChildElement("Subscriber"); + subscriber_node != nullptr; + subscriber_node = subscriber_node->NextSiblingElement("Subscriber")) + { + push_new(format_subscriber_node(subscriber_node), &headers); + } + + for (auto subscriber_node = root->FirstChildElement("RemoteSubscriber"); + subscriber_node != nullptr; + subscriber_node = subscriber_node->NextSiblingElement("RemoteSubscriber")) + { + push_new(format_subscriber_node(subscriber_node), &headers); + } + + return gen_template.headers_template(headers); +} + +std::string XMLParser::generate_action_class(const XMLElement* node, const std::string package_name) { + auto format_input_port = [](const XMLElement* node) { + return fmt::format(" InputPort(\"{0}\")", + node->Attribute("name")); + }; + auto format_output_port = [](const XMLElement* node) { + return fmt::format(" OutputPort(\"{0}\")", + node->Attribute("name")); + }; + auto format_get_input = [](const XMLElement* node) { + return fmt::format(" getInput(\"{0}\", goal.{0});", + node->Attribute("name")); + }; + auto format_set_output = [](const XMLElement* node) { + return fmt::format( + " if (feedback->update_field_name == \"{0}\") setOutput(\"{0}\", feedback->{0});", + node->Attribute("name")); + }; + + std::vector provided_input_ports; + std::vector provided_output_ports; + std::vector get_inputs; + std::vector set_outputs; + + provided_input_ports.push_back(format_server_name(node)); + + for (auto port_node = node->FirstChildElement(); + port_node != nullptr; + port_node = port_node->NextSiblingElement()) + { + std::string name = port_node->Name(); + if (name == "input_port" || name == "inout_port") { + provided_input_ports.push_back(format_input_port(port_node)); + get_inputs.push_back(format_get_input(port_node)); + } + if (name == "output_port" || name == "inout_port") { + provided_output_ports.push_back(format_output_port(port_node)); + set_outputs.push_back(format_set_output(port_node)); + } + } + + std::vector provided_ports; + provided_ports.insert(provided_ports.end(), + provided_input_ports.begin(), + provided_input_ports.end()); + provided_ports.insert(provided_ports.end(), + provided_output_ports.begin(), + provided_output_ports.end()); + + + return gen_template.action_class_template(package_name, node->Attribute("ID"), + provided_ports, get_inputs, set_outputs); +} + +std::string XMLParser::generate_remote_action_class(const XMLElement* node, const std::string package_name) { + auto format_input_port = [](const XMLElement* node) { + std::string msg_type = node->Attribute("type"); + if (msg_type.find('/') != std::string::npos) + // ros messages are represented as json documents + return fmt::format(" InputPort(\"{0}\")", + node->Attribute("name")); + return fmt::format(" InputPort(\"{0}\")", + node->Attribute("name")); + }; + auto format_output_port = [](const XMLElement* node) { + std::string msg_type = node->Attribute("type"); + if (msg_type.find('/') != std::string::npos) + return fmt::format(" OutputPort(\"{0}\")", + node->Attribute("name")); + return fmt::format(" OutputPort(\"{0}\")", + node->Attribute("name")); + }; + + std::vector provided_input_ports; + std::vector provided_output_ports; + std::vector get_inputs; + std::vector set_outputs; + + provided_input_ports.push_back(format_server_name(node)); + if (node->Attribute("host_name")) { + provided_input_ports.push_back(format_host_name(node));} + if (node->Attribute("host_port")) { + provided_input_ports.push_back(format_host_port(node));} + + for (auto port_node = node->FirstChildElement(); + port_node != nullptr; + port_node = port_node->NextSiblingElement()) + { + std::string name = port_node->Name(); + if (name == "input_port" || name == "inout_port") { + provided_input_ports.push_back(format_input_port(port_node)); + get_inputs.push_back(format_get_remote_input(port_node, "goal")); + } + if (name == "output_port" || name == "inout_port") { + provided_output_ports.push_back(format_output_port(port_node)); + set_outputs.push_back(format_set_remote_output(port_node)); + } + } + + std::vector provided_ports; + provided_ports.insert(provided_ports.end(), + provided_input_ports.begin(), + provided_input_ports.end()); + provided_ports.insert(provided_ports.end(), + provided_output_ports.begin(), + provided_output_ports.end()); + + + return gen_template.remote_action_class_template(package_name, node->Attribute("ID"), + provided_ports, get_inputs, set_outputs); +} + +std::string XMLParser::generate_condition_class(const XMLElement* node, const std::string package_name) { + auto format_input_port = [](const XMLElement* node) { + return fmt::format(" InputPort(\"{0}\")", + node->Attribute("name")); + }; + auto format_get_input = [](const XMLElement* node) { + return fmt::format(" getInput(\"{0}\", request.{0});", + node->Attribute("name")); + }; + + std::vector provided_ports; + std::vector get_inputs; + + provided_ports.push_back(format_service_name(node)); + + for (auto port_node = node->FirstChildElement("input_port"); + port_node != nullptr; + port_node = port_node->NextSiblingElement("input_port")) + { + provided_ports.push_back(format_input_port(port_node)); + get_inputs.push_back(format_get_input(port_node)); + } + + return gen_template.condition_class_template(package_name, node->Attribute("ID"), + provided_ports, get_inputs); +} + +std::string XMLParser::generate_remote_condition_class(const XMLElement* node, const std::string package_name) { + auto format_input_port = [](const XMLElement* node) { + return fmt::format(" InputPort(\"{0}\")", + node->Attribute("name")); + }; + + std::vector provided_ports; + std::vector get_inputs; + + provided_ports.push_back(format_service_name(node)); + if (node->Attribute("host_name")) { + provided_ports.push_back(format_host_name(node));} + if (node->Attribute("host_port")) { + provided_ports.push_back(format_host_port(node));} + + for (auto port_node = node->FirstChildElement("input_port"); + port_node != nullptr; + port_node = port_node->NextSiblingElement("input_port")) + { + provided_ports.push_back(format_input_port(port_node)); + get_inputs.push_back(format_get_remote_input(port_node, "request")); + } + + return gen_template.remote_condition_class_template(package_name, node->Attribute("ID"), + provided_ports, get_inputs); +} + +std::string XMLParser::generate_subscriber_class(const XMLElement* node) { + auto format_type = [](const XMLElement* node) { + std::string type = node->Attribute("message_type"); + std::size_t pos = type.find('/'); + if (pos == std::string::npos) { + throw XMLError::InvalidTopicType(type, node); + } + return fmt::format("{}::{}", type.substr(0, pos), type.substr(1+pos)); + }; + auto format_port = [](const XMLElement* port_node) { + return fmt::format( + " InputPort(\"topic_name\", \"{0}\", \"name of the subscribed topic\")", + port_node->Attribute("default")); + }; + + std::vector provided_ports; + + for (auto port_node = node->FirstChildElement("input_port"); + port_node != nullptr; + port_node = port_node->NextSiblingElement("input_port")) + { + std::string name = port_node->Attribute("name"); + if (name == "topic_name" && port_node->Attribute("default")) { + provided_ports.push_back(format_port(port_node)); + } + } + + return gen_template.subscriber_class_template(node->Attribute("ID"), + format_type(node), + provided_ports); +} + +std::string XMLParser::generate_remote_subscriber_class(const XMLElement* node) { + auto format_type = [](const XMLElement* node) { + std::string type = node->Attribute("message_type"); + std::size_t pos = type.find('/'); + if (pos == std::string::npos) { + throw XMLError::InvalidTopicType(type, node); + } + return fmt::format("{}::{}", type.substr(0, pos), type.substr(1+pos)); + }; + auto format_port = [](const XMLElement* port_node) { + return fmt::format( + " InputPort(\"topic_name\", \"{0}\", \"name of the subscribed topic\")", + port_node->Attribute("default")); + }; + + std::vector provided_ports; + + if (node->Attribute("host_name")) { + provided_ports.push_back(format_host_name(node));} + if (node->Attribute("host_port")) { + provided_ports.push_back(format_host_port(node));} + + for (auto port_node = node->FirstChildElement("input_port"); + port_node != nullptr; + port_node = port_node->NextSiblingElement("input_port")) + { + std::string name = port_node->Attribute("name"); + if (name == "topic_name" && port_node->Attribute("default")) { + provided_ports.push_back(format_port(port_node)); + } + } + + return gen_template.remote_subscriber_class_template(node->Attribute("ID"), + format_type(node), + provided_ports); +} + +std::string XMLParser::generate_main_function(const std::string roscpp_node_name, + const std::string program_description, + const std::string xml_filename) { + auto format_action_node = [](const XMLElement* node) { + return fmt::format(" RegisterRosAction<{0}>(factory, \"{0}\", nh);", + node->Attribute("ID")); + }; + auto format_remote_action_node = [](const XMLElement* node) { + return fmt::format(" RegisterRemoteAction<{0}>(factory, \"{0}\");", + node->Attribute("ID")); + }; + auto format_condition_node = [](const XMLElement* node) { + return fmt::format(" RegisterRosService<{0}>(factory, \"{0}\", nh);", + node->Attribute("ID")); + }; + auto format_remote_condition_node = [](const XMLElement* node) { + return fmt::format(" RegisterRemoteCondition<{0}>(factory, \"{0}\");", + node->Attribute("ID")); + }; + auto format_subscriber_node = [](const XMLElement* node) { + return fmt::format(" RegisterSubscriberNode<{0}>(factory, \"{0}\", nh);", + node->Attribute("ID")); + }; + auto format_remote_subscriber_node = [](const XMLElement* node) { + return fmt::format(" RegisterRemoteSubscriber<{0}>(factory, \"{0}\");", + node->Attribute("ID")); + }; + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + std::vector register_actions; + std::vector register_conditions; + std::vector register_subscribers; + + for (auto action_node = root->FirstChildElement("Action"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("Action")) + { + register_actions.push_back(format_action_node(action_node)); + } + + for (auto action_node = root->FirstChildElement("RemoteAction"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("RemoteAction")) + { + register_actions.push_back(format_remote_action_node(action_node)); + } + + for (auto condition_node = root->FirstChildElement("Condition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("Condition")) + { + register_conditions.push_back(format_condition_node(condition_node)); + } + + for (auto condition_node = root->FirstChildElement("RemoteCondition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("RemoteCondition")) + { + register_conditions.push_back(format_remote_condition_node(condition_node)); + } + + for (auto subscriber_node = root->FirstChildElement("Subscriber"); + subscriber_node != nullptr; + subscriber_node = subscriber_node->NextSiblingElement("Subscriber")) + { + register_subscribers.push_back(format_subscriber_node(subscriber_node)); + } + + for (auto subscriber_node = root->FirstChildElement("RemoteSubscriber"); + subscriber_node != nullptr; + subscriber_node = subscriber_node->NextSiblingElement("RemoteSubscriber")) + { + register_subscribers.push_back(format_remote_subscriber_node(subscriber_node)); + } + + return gen_template.main_function_template(roscpp_node_name, + program_description, + xml_filename, + register_actions, + register_conditions, + register_subscribers); +} + +std::string XMLParser::generate_eus_action_server(const std::string package_name) { + + std::vector callback_definition; + std::vector instance_creation; + std::vector load_files; + + collect_eus_actions(package_name, &callback_definition, &instance_creation); + collect_eus_conditions(package_name, &callback_definition, &instance_creation, + NULL, NULL); + + if (callback_definition.empty()) return ""; + + return gen_template.eus_server_template("action", package_name, + callback_definition, instance_creation, + load_files); +} + +std::string XMLParser::generate_eus_remote_action_server(const std::string package_name, + const std::string remote_host) +{ + std::vector callback_definition; + std::vector instance_creation; + std::vector load_files; + + collect_eus_actions(package_name, &callback_definition, &instance_creation, remote_host); + collect_eus_conditions(package_name, &callback_definition, &instance_creation, + NULL, NULL, remote_host); + + if (callback_definition.empty()) return ""; + + return gen_template.eus_server_template("action", package_name, + callback_definition, instance_creation, + load_files); +} + +std::string XMLParser::generate_eus_condition_server(const std::string package_name) { + std::vector callback_definition; + std::vector instance_creation; + std::vector load_files; + + collect_eus_conditions(package_name, NULL, NULL, + &callback_definition, &instance_creation); + + if (callback_definition.empty()) return ""; + + return gen_template.eus_server_template("condition", package_name, + callback_definition, instance_creation, + load_files); +} + +std::string XMLParser::generate_eus_remote_condition_server(const std::string package_name, + const std::string remote_host) +{ + std::vector callback_definition; + std::vector instance_creation; + std::vector load_files; + + collect_eus_conditions(package_name, NULL, NULL, + &callback_definition, &instance_creation, + remote_host); + + if (callback_definition.empty()) return ""; + + return gen_template.eus_server_template("condition", package_name, + callback_definition, instance_creation, + load_files); +} + +std::map XMLParser::generate_all_action_files() { + auto format_filename = [](const XMLElement* node) { + return fmt::format("{}.action", node->Attribute("ID")); + }; + + std::map result; + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + + for (auto action_node = root->FirstChildElement("Action"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("Action")) + { + result[format_filename(action_node)] = generate_action_file_contents(action_node); + } + + for (auto action_node = root->FirstChildElement("RemoteAction"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("RemoteAction")) + { + result[format_filename(action_node)] = generate_action_file_contents(action_node); + } + + return result; +} + +std::map XMLParser::generate_all_service_files() { + auto format_filename = [](const XMLElement* node) { + return fmt::format("{}.srv", node->Attribute("ID")); + }; + + std::map result; + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + for (auto condition_node = root->FirstChildElement("Condition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("Condition")) + { + result[format_filename(condition_node)] = generate_service_file_contents(condition_node); + } + + for (auto condition_node = root->FirstChildElement("RemoteCondition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("RemoteCondition")) + { + result[format_filename(condition_node)] = generate_service_file_contents(condition_node); + } + return result; +} + +std::map XMLParser::generate_all_eus_action_servers(const std::string package_name) +{ + std::map result; + std::vector remote_hosts; + + collect_remote_hosts(&remote_hosts); + + for (int i = 0; i < remote_hosts.size(); i++) { + std::string r_host = remote_hosts.at(i); + result[r_host] = generate_eus_remote_action_server(package_name, r_host); + } + result[""] = generate_eus_action_server(package_name); + + return result; +} + +std::map XMLParser::generate_all_eus_condition_servers(const std::string package_name) +{ + std::map result; + std::vector remote_hosts; + + collect_remote_hosts(&remote_hosts); + + for (int i = 0; i < remote_hosts.size(); i++) { + std::string r_host = remote_hosts.at(i); + result[r_host] = generate_eus_remote_condition_server(package_name, r_host); + } + result[""] = generate_eus_condition_server(package_name); + + return result; +} + +std::string XMLParser::generate_cpp_file(const std::string package_name, + const std::string roscpp_node_name, + const std::string program_description, + const std::string xml_filename) { + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + std::string output; + output.append(generate_headers(package_name)); + output.append("\n\n"); + + for (auto action_node = root->FirstChildElement("Action"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("Action")) + { + output.append(generate_action_class(action_node, package_name)); + output.append("\n\n"); + } + + for (auto action_node = root->FirstChildElement("RemoteAction"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("RemoteAction")) + { + output.append(generate_remote_action_class(action_node, package_name)); + output.append("\n\n"); + } + + for (auto condition_node = root->FirstChildElement("Condition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("Condition")) + { + output.append(generate_condition_class(condition_node, package_name)); + output.append("\n\n"); + } + + for (auto condition_node = root->FirstChildElement("RemoteCondition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("RemoteCondition")) + { + output.append(generate_remote_condition_class(condition_node, package_name)); + output.append("\n\n"); + } + + for (auto subscriber_node = root->FirstChildElement("Subscriber"); + subscriber_node != nullptr; + subscriber_node = subscriber_node->NextSiblingElement("Subscriber")) + { + output.append(generate_subscriber_class(subscriber_node)); + output.append("\n\n"); + } + + for (auto subscriber_node = root->FirstChildElement("RemoteSubscriber"); + subscriber_node != nullptr; + subscriber_node = subscriber_node->NextSiblingElement("RemoteSubscriber")) + { + output.append(generate_remote_subscriber_class(subscriber_node)); + output.append("\n\n"); + } + + output.append(generate_main_function(roscpp_node_name, program_description, xml_filename)); + + return output; +} + +std::string XMLParser::generate_launch_file(const std::string package_name, + std::vector euslisp_filenames) { + std::vector launch_nodes; + for (auto eus_file: euslisp_filenames) { + launch_nodes.push_back(format_launch_node(package_name, eus_file)); + } + + return gen_template.launch_file_template(launch_nodes); +} + +void XMLParser::push_dependencies(std::vector* message_packages, + std::vector* service_files, + std::vector* action_files) { + auto format_action_file = [](const XMLElement* node) { + return fmt::format(" {}.action", node->Attribute("ID")); + }; + auto format_service_file = [](const XMLElement* node) { + return fmt::format(" {}.srv", node->Attribute("ID")); + }; + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + + for (auto node = root->FirstChildElement(); + node != nullptr; + node = node->NextSiblingElement()) { + + std::string name = node->Name(); + + if (name == "Action" || name == "RemoteAction") { + push_new(format_action_file(node), action_files); + } + if (name == "Condition" || name == "RemoteCondition") { + push_new(format_service_file(node), service_files); + } + if (name == "Action" || name == "RemoteAction" || + name == "Condition" || name == "RemoteCondition") { + for (auto port_node = node->FirstChildElement(); + port_node != nullptr; + port_node = port_node->NextSiblingElement()) + { + maybe_push_message_package(port_node->Attribute("type"), message_packages); + } + } + if (name == "Subscriber" || name == "RemoteSubscriber") { + maybe_push_message_package(node->Attribute("message_type"), message_packages); + } + } +} + + +} // namespace RoseusBT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ diff --git a/roseus_bt/package.xml b/roseus_bt/package.xml new file mode 100644 index 000000000..ce017bf03 --- /dev/null +++ b/roseus_bt/package.xml @@ -0,0 +1,24 @@ + + + roseus_bt + 0.0.1 + The roseus_bt package + + affonso + BSD + + catkin + + roscpp + tinyxml2 + fmt + behaviortree_ros + + roscpp + tinyxml2 + fmt + behaviortree_ros + + + + diff --git a/roseus_bt/roseus_bt.rosinstall b/roseus_bt/roseus_bt.rosinstall new file mode 100644 index 000000000..188a54ba1 --- /dev/null +++ b/roseus_bt/roseus_bt.rosinstall @@ -0,0 +1,14 @@ +- git: + local-name: jsk_roseus + uri: https://github.com/jsk-ros-pkg/jsk_roseus.git + version: master + +- git: + local-name: BehaviorTree.ROS + uri: https://github.com/BehaviorTree/BehaviorTree.ROS + version: master + +- git: + local-name: Groot + uri: https://github.com/BehaviorTree/Groot.git + version: master diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md new file mode 100644 index 000000000..43e9cad30 --- /dev/null +++ b/roseus_bt/sample/README.md @@ -0,0 +1,286 @@ +# roseus_bt_tutorials + +Create and build the tutorial package + +```bash +cd ~/catkin_ws/src +rosrun roseus_bt create_bt_tutorials +catkin build roseus_bt_tutorials +``` + +## t01_simple_tree +![t01](https://user-images.githubusercontent.com/20625381/125036489-082d3f80-e0ce-11eb-8cce-d87a06b2c1d8.gif) + +We start with a simple behavior tree, composed only by a few actions organized into a single sequence. +The model file https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_bt/sample/models/t01_simple_tree.xml is divided into the following two main sections: +- `` specifies the tree structure +- `` specifies the custom node palette + +Every `` tag in the `` must be provided with an arbitrary `server_name` field. + +The recommended way to write a xml model file is to use the Groot editor and then edit in the required fields afterwards. + +Both the `` tag in the xml model file and the euslisp server are loaded at runtime, but changes in the node palette (``) must be re-generated and re-compiled. + +#### Run the code + +Run the roseus server +```bash +roscd roseus_bt_tutorials/euslisp +roseus t01-simple-tree-action-server.l +``` + +Run the cpp client +```bash +rosrun roseus_bt_tutorials t01_simple_tree +``` + +Optionally run Groot for visualization +```bash +rosrun groot Groot +``` + +## t02_conditions +![t02](https://user-images.githubusercontent.com/20625381/125036852-707c2100-e0ce-11eb-99b8-a8d568e6e97c.gif) + +The second example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_bt/sample/models/t02_conditions.xml also includes condition and fallback nodes. + +Every `` tag in the `` must be provided with an arbitrary `service_name` field. + +#### Run the code + +Run the roseus server +```bash +roscd roseus_bt_tutorials/euslisp +roseus t02-conditions-action-server.l +``` + +Run the cpp client +```bash +rosrun roseus_bt_tutorials t02_conditions +``` + +Optionally run Groot for visualization +```bash +rosrun groot Groot +``` + +## t03_ports +![t03](https://user-images.githubusercontent.com/20625381/125036607-25faa480-e0ce-11eb-9013-28b2c41c90f2.gif) + +The third example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_bt/sample/models/t03_ports.xml introduces Ports, which act as the input and output arguments for nodes. + +Ports are strongly typed and can take any type which can be used in a ROS message field (e.g. `int64` and `int32` are accepted but `int` is not supported). + +Port variables can be assigned/referenced with the `${variable_name}` syntax and are stored in the behavior tree blackboard. + +The name and type of each node port are specified in the `` tag, and its value in the `` tag. + +Ports can be declared as either ``, `` or ``. +Input ports are passed to the roseus layer as function arguments; Output ports can be set at any point of execution through the `(roseus_bt:set-output "name" value)` function. + +Conditions only support input ports, as they are not meant to do any changes in the behavior tree state. + + +#### Run the code + +Run the roseus server +```bash +roscd roseus_bt_tutorials/euslisp +roseus t03-ports-action-server.l +``` + +Run the cpp client +```bash +rosrun roseus_bt_tutorials t03_ports +``` + +Optionally run Groot for visualization +```bash +rosrun groot Groot +``` + +## t04_subscriber +![t04](https://user-images.githubusercontent.com/20625381/125036625-2b57ef00-e0ce-11eb-8198-974d1b45855a.gif) + +The fourth example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_bt/sample/models/t04_subscriber.xml shows how we can remap topic messages to port variables. + +Such port variables are initialized with an empty message instance and updated every time a new topic message arrives. + +To do this we add a `` node, specifying the input ports `topic_name` and `message_type` and the output ports `output_port` and `received_port`. The `output_port` variable is initilized with an instance of the given message type and updated every time a new message is received. The `received_port` variable is a boolean initialized with false and set to true at every new message. + +Only proper ROS message types are supported by subscriber nodes (e.g. `std_msgs/Int64` instead of `int64`). + + +#### Run the code + +Publish the topic: +```bash +rostopic pub -r 10 /petbottle/coords geometry_msgs/Pose "position: + x: 1.85 + y: 0.5 + z: 0.7 +orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0" +``` + +Run the roseus server +```bash +roscd roseus_bt_tutorials/euslisp +roseus t04-subscriber-action-server.l +``` + +Run the cpp client +```bash +rosrun roseus_bt_tutorials t04_subscriber +``` + +Optionally run Groot for visualization +```bash +rosrun groot Groot +``` + +## t05_subtrees +![t05](https://user-images.githubusercontent.com/20625381/125036658-3448c080-e0ce-11eb-9592-23f6b424bcd1.gif) + +The fifth example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_bt/sample/models/t05_subtrees.xml wraps up the previous task in a subtree and adds another example task. + +Nested trees can be defined with multiple `` tags and referenced with the `` tag. + +Each subtree inherits a separate blackboard and accepts remaps in the `inner_name="outer_name"` syntax. + +#### Run the code + +Run the roseus server +```bash +roscd roseus_bt_tutorials/euslisp +roseus t05-subtrees-action-server.l +``` + +Run the cpp client +```bash +rosrun roseus_bt_tutorials t05_subtrees +``` + +Optionally run Groot for visualization +```bash +rosrun groot Groot +``` + +## t06_reactive +![t06](https://user-images.githubusercontent.com/20625381/125036676-390d7480-e0ce-11eb-813e-69784c2053a9.gif) + +The sixth example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_bt/sample/models/t06_reactive.xml uses reactive fallbacks to constantly check and respond to user requests. + +The main difference of reactive nodes (e.g. `` and ``) is that when a child returns RUNNING the reactive node will resume ticking from its first child. This forces the node to re-evaluate any conditions preceding the execution node, therefore achieving enhanced reactivity. + +In order to compose a reactive program in single-threded eus, we need to ensure that: +1. Condition nodes can be evaluated while executing actions +2. Interruption requests are checked while executing actions + +In this example, we achieve the first point by preparing two distinct roseus servers -- one for actions and the other for conditions. +The second point is achieved by constantly spinning the ros node during the execution loop, which by default raises a `roseus_bt:cancel-action` condition whenever an interruption request is received. Finally, the execution callback is also responsible for correctly handling this condition. + +When using the real robot, the suggested way to achieve concurrent evaluation is to register condition nodes `:groupname` and action nodes `:monitor-groupname` with the robot's interface groupname (e.g. `(*ri* . groupname)`). This allows for checking conditions and interruption requests at every robot spin, which naturally happens at several points of the control loop, such as during `:wait-interpolation`. + +It is also possible to manually spin the monitor groupname with `(send server :spin-monitor)`, or to check for the presence of interruption requests with the `(send server :ok)` when using custom preemption callbacks. + +#### Run the code + +Run the roseus action server +```bash +roscd roseus_bt_tutorials/euslisp +roseus t06-reactive-action-server.l +``` + +Run the roseus condition server +```bash +roscd roseus_bt_tutorials/euslisp +roseus t06-reactive-condition-server.l +``` + +Run the cpp client +```bash +rosrun roseus_bt_tutorials t06_reactive +``` + +Send the request +```bash +rostopic pub --once /get_drink/request std_msgs/Bool true +``` + +Optionally run Groot for visualization +```bash +rosrun groot Groot +``` + +## t07_xacro + +In this example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_bt/sample/models/t07_xacro.xml.xacro we illustrate how it is possible to use the xacro package to improve readability and modularity of the model file descriptions. + +The following will create the `t07_xacro.xml` file equivalent to the `t05_subtrees.xml` example +```bash +roscd roseus_bt/sample/models +rosrun xacro xacro t07_xacro.xml.xacro -o t07_xacro.xml +``` + +And it is also possible to create independent models for each of the subtrees (in this case setting the `main` argument) +```bash +rosrun xacro xacro t07_xacro_pour_task.xml.xacro -o t07_xacro_pour_task.xml main:=true +rosrun xacro xacro t07_xacro_sweep_task.xml.xacro -o t07_xacro_sweep_task.xml main:=true +``` + +Note how port variables need to be quoted (e.g.`$${var}`) to use the xacro syntax. +The `{var}` notation also works. + +## t08_multimaster + +This example shows how to use the rosbridge interface to assign different hosts to each action in a multimaster application. +https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_bt/sample/models/t08_multimaster.xml + +To do this we declare the actions with the `` and conditions with the `` tag in the ``, and add a `host_name` and `host_port` field to them. + +Make sure that the rosbridge server is started after sourcing all of the package's messages and services. Setting a large `unregister_timeout` is also desirable to avoid problems described in https://github.com/knorth55/jsk_robot/pull/230 . + + +#### Run the code + +Run the first rosbridge_server: +```bash +# source package before running this +roslaunch rosbridge_server rosbridge_websocket.launch unregister_timeout:=100000 +``` + +Run the first roseus server: +```bash +roscd roseus_bt_tutorials/euslisp +roseus t08-multimaster-localhost9090-action-server.l +``` + +Run the second rosbridge_server: +```bash +# source package before running this +export ROS_MASTER_URI=http://localhost:11312 +roslaunch rosbridge_server rosbridge_websocket.launch port:=9091 unregister_timeout:=100000 +``` + +Run the second roseus server: +```bash +export ROS_MASTER_URI=http://localhost:11312 +roscd roseus_bt_tutorials/euslisp +roseus t08-multimaster-localhost9091-action-server.l +``` + +Run the cpp client +```bash +rosrun roseus_bt_tutorials t08_multimaster +``` + +Optionally run Groot for visualization +```bash +rosrun groot Groot +``` + diff --git a/roseus_bt/sample/models/t01_simple_tree.xml b/roseus_bt/sample/models/t01_simple_tree.xml new file mode 100644 index 000000000..fa93d60b0 --- /dev/null +++ b/roseus_bt/sample/models/t01_simple_tree.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/models/t02_conditions.xml b/roseus_bt/sample/models/t02_conditions.xml new file mode 100644 index 000000000..27264e9c0 --- /dev/null +++ b/roseus_bt/sample/models/t02_conditions.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/models/t03_ports.xml b/roseus_bt/sample/models/t03_ports.xml new file mode 100644 index 000000000..ae769d1f6 --- /dev/null +++ b/roseus_bt/sample/models/t03_ports.xml @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/models/t04_subscriber.xml b/roseus_bt/sample/models/t04_subscriber.xml new file mode 100644 index 000000000..8a1a478a4 --- /dev/null +++ b/roseus_bt/sample/models/t04_subscriber.xml @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/models/t05_subtrees.xml b/roseus_bt/sample/models/t05_subtrees.xml new file mode 100644 index 000000000..3b6787b1b --- /dev/null +++ b/roseus_bt/sample/models/t05_subtrees.xml @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/models/t06_reactive.xml b/roseus_bt/sample/models/t06_reactive.xml new file mode 100644 index 000000000..68d056e81 --- /dev/null +++ b/roseus_bt/sample/models/t06_reactive.xml @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/models/t07_xacro.xml.xacro b/roseus_bt/sample/models/t07_xacro.xml.xacro new file mode 100644 index 000000000..a7c5cd7b5 --- /dev/null +++ b/roseus_bt/sample/models/t07_xacro.xml.xacro @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/models/t07_xacro_pour_task.xml.xacro b/roseus_bt/sample/models/t07_xacro_pour_task.xml.xacro new file mode 100644 index 000000000..081046229 --- /dev/null +++ b/roseus_bt/sample/models/t07_xacro_pour_task.xml.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/models/t07_xacro_sweep_task.xml.xacro b/roseus_bt/sample/models/t07_xacro_sweep_task.xml.xacro new file mode 100644 index 000000000..91d4a940a --- /dev/null +++ b/roseus_bt/sample/models/t07_xacro_sweep_task.xml.xacro @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/models/t08_multimaster.xml b/roseus_bt/sample/models/t08_multimaster.xml new file mode 100644 index 000000000..040c370f4 --- /dev/null +++ b/roseus_bt/sample/models/t08_multimaster.xml @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/sample-robot-utils.l b/roseus_bt/sample/sample-robot-utils.l new file mode 100644 index 000000000..80b045c89 --- /dev/null +++ b/roseus_bt/sample/sample-robot-utils.l @@ -0,0 +1,36 @@ +(load "irteus/demo/sample-robot-model.l") +(load "models/broom-object.l") + + +(defmethod sample-robot + (:get-legcoords () + (midcoords 0.5 + (send self :lleg :end-coords :copy-worldcoords) + (send self :rleg :end-coords :copy-worldcoords))) + + (:go-to (x y theta &key debug-view) + (send self :calc-walk-pattern-from-footstep-list + (send self :go-pos-params->footstep-list x y theta) + :debug-view debug-view) + (send self :get-legcoords)) + + (:go-to-coords (c &key debug-view) + (let* ((dest (send (send self :get-legcoords) :transformation c)) + (x (elt (send dest :worldpos) 0)) + (y (elt (send dest :worldpos) 1)) + (theta (rad2deg (caar (send dest :rpy-angle))))) + (print (list x y theta)) + (send self :go-to x y theta :debug-view debug-view))) + + (:dual-arm-ik (target-lst &key debug-view (view-target :midpoint)) + (let* ((move-target (send self :arms :end-coords)) + (link-list (mapcar #'(lambda (mt) (send self :link-list mt)) + (send-all move-target :parent)))) + + (send self :head :look-at + (apply #'midpoint 0.5 (send-all target-lst :worldpos))) + (send self :inverse-kinematics target-lst + :link-list link-list :move-target move-target + :stop 500 :thre '(10 10) + :rotation-axis '(nil nil) :debug-view debug-view :dump-command nil))) +) diff --git a/roseus_bt/sample/sample-room.l b/roseus_bt/sample/sample-room.l new file mode 100644 index 000000000..3eef55db0 --- /dev/null +++ b/roseus_bt/sample/sample-room.l @@ -0,0 +1,32 @@ +(load "models/karimoku-1200-desk-object.l") +(load "models/petbottle-object.l") +(load "models/sushi-cup-object.l") +(load "models/broom-object.l") + +(defclass sample-room-scene + :super scene-model + :slots ()) + +(defmethod sample-room-scene + (:init (&rest args &key broom (name "sample-room")) + (let ((obj-lst + (list + (send (karimoku-1200-desk) :transform + (make-coords :pos (float-vector 2000 500 0))) + (send (sushi-cup) :transform + (make-coords :pos (float-vector 1850 100 700))) + (send (petbottle) :transform + (make-coords :pos (float-vector 1850 400 700))))) + (spot-lst + (list + (make-cascoords :name "table-front" :pos (float-vector 1350 500 0))))) + + (when broom + (push (send (broom) :transform (make-coords :pos (float-vector 400 -1500 0) + :rpy (float-vector -pi/2 0 0))) + obj-lst) + (push (make-cascoords :name "broom-front" :pos (float-vector 400 -1250 0) + :rpy (float-vector -pi/2 0 0)) + spot-lst)) + + (send-super :init :name name :objects (append obj-lst spot-lst))))) diff --git a/roseus_bt/sample/sample-task.l b/roseus_bt/sample/sample-task.l new file mode 100644 index 000000000..414cd4063 --- /dev/null +++ b/roseus_bt/sample/sample-task.l @@ -0,0 +1,97 @@ +(load "sample-robot-utils.l") +(load "sample-room.l") + + +;; Initialize *room* and *robot* +(defun init (&optional (broom t) (start-viewer t)) + (defvar *room* (instance sample-room-scene :init :broom broom)) + (unless (boundp '*robot*) + (defvar *robot* (instance sample-robot :init)) + (send *robot* :reset-pose) + (send *robot* :fix-leg-to-coords (make-coords)) + (send *robot* :update-descendants)) + (if start-viewer (objects (list *room* *robot*)))) + + +;; Utility +(defun draw () + (when (boundp '*irtviewer*) + (send *irtviewer* :draw-objects) + (unix:usleep 500000))) + +(defun reset-pose () + (unless (v= (send *robot* :angle-vector) (send *robot* :reset-pose)) + (draw))) + +(defun dual-arm-grasp-obj (lobj robj) + (send *robot* :dual-arm-ik + (list + (send lobj :handle-handle0) + (send robj :handle-handle0)) + :debug-view t) + (send (send *robot* :larm :end-coords) :assoc lobj) + (send (send *robot* :rarm :end-coords) :assoc robj)) + + +;; Go to spot +(defun go-to-spot (name) + (send *robot* :go-to-coords (send *room* :spot name) :debug-view t)) + +;; At spot +(defun at-spot (name) + (let ((robot-coords (send *robot* :get-legcoords)) + (spot-coords (send *room* :spot name))) + (and (< (norm (send robot-coords :difference-position spot-coords)) 100) + (< (norm (send robot-coords :difference-rotation spot-coords)) (deg2rad 10))))) + +;; Pour task +(defun pick-sushi-bottle (&optional bottle-coords) + (let ((petbottle-obj (send *room* :object "petbottle")) + (sushi-cup-obj (send *room* :object "sushi-cup"))) + (if bottle-coords + (send petbottle-obj :newcoords bottle-coords)) + (dual-arm-grasp-obj petbottle-obj sushi-cup-obj) + (send *robot* :dual-arm-ik + (list + (make-coords :pos #f(1736.53 754.746 835.385) + :rpy #f(-0.160248 -0.502549 0.31589) + :name "petbottle-pos") + (make-coords :pos #f(1736.82 265.63 805.839) + :rpy #f(0.50965 0.109477 -1.08614) + :name "sushi-cup-pos")) + :debug-view t))) + +(defun pour-sushi-bottle () + (send *robot* :larm :move-end-pos #f(0 -250 50) :world :debug-view t) + (draw) + (send *robot* :larm-wrist-r :joint-angle -30) + (draw) + (send *robot* :larm-wrist-r :joint-angle 30) + (draw)) + +(defun place-sushi-bottle () + (let ((petbottle-obj (send *room* :object "petbottle")) + (sushi-cup-obj (send *room* :object "sushi-cup"))) + (send *robot* :dual-arm-ik + (list + (make-coords :pos #f(1850.0 400.0 797.5) :name "petbottle-pos") + (make-coords :pos #f(1850.0 100.0 746.0) :name "sushi-cup-pos")) + :debug-view t) + (send (send *robot* :larm :end-coords) :dissoc petbottle-obj) + (send (send *robot* :rarm :end-coords) :dissoc sushi-cup-obj))) + + +;; Sweep task +(defun sweep-floor () + (let ((broom-obj (send *room* :object "broom")) + (i 0)) + (send *robot* :dual-arm-ik (send broom-obj :handle) :debug-view t) + + (while (ros::ok) + (ros::spin-once) + (send broom-obj :rpy -pi/2 0 (* 0.2 (sin (/ i 10.0)))) + (send broom-obj :locate (float-vector (+ 400 (* 250 (sin (/ (incf i) 10.0)))) -1500 0) :world) + (send *robot* :dual-arm-ik (send broom-obj :handle)) + (send *irtviewer* :draw-objects) + (unix:usleep 50000) + (incf i)))) diff --git a/roseus_bt/src/create_bt_package.cpp b/roseus_bt/src/create_bt_package.cpp new file mode 100644 index 000000000..d174b5377 --- /dev/null +++ b/roseus_bt/src/create_bt_package.cpp @@ -0,0 +1,78 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +namespace po = boost::program_options; + +int main(int argc, char** argv) +{ + std::string package_name, author; + std::vector model_files, executable_names; + + po::options_description desc("usage"); + desc.add_options() + ("help,h", "show this help message and exit") + ("package_name", po::value(&package_name), "package name") + ("model_file", po::value>(&model_files), "model file") + ("executable,e", po::value>(&executable_names), + "executable name (defaults to model filename)") + ("author,a", po::value(&author)->default_value("The Author"), + "author name") + ("overwrite,y", "overwrite all existing files") + ("verbose,v", "print all logging messages"); + + po::positional_options_description positional_arguments; + positional_arguments.add("package_name", 1); + positional_arguments.add("model_file", -1); + + po::variables_map args; + po::store(po::command_line_parser(argc, argv).options(desc).positional(positional_arguments).run(), args); + po::notify(args); + + // Initialize Logger + auto logger_level = boost::log::trivial::warning; + if (args.count("verbose")) { + logger_level = boost::log::trivial::trace; + } + boost::log::core::get()->set_filter( + boost::log::trivial::severity >= logger_level); + + // Help + if (args.count("help")) { + std::cout << "\n" << "Create behavior tree package." << "\n"; + std::cout << desc << std::endl; + return 0; + } + + // Wrong usage + if (package_name.empty()) { + std::cout << desc << std::endl; + return 1; + } + + // resize up to the number of model_files + executable_names.resize(model_files.size()); + + // fill executable_names with defaults + for (auto model_it = model_files.begin(), exec_it = executable_names.begin(); + model_it != model_files.end(); + ++model_it, ++exec_it) { + std::string model_file = *model_it; + std::string executable = *exec_it; + + if (executable.empty()) + *exec_it = boost::filesystem::path(model_file).stem().string(); + } + + RoseusBT::PackageGenerator pg(package_name, + model_files, executable_names, + author, args.count("overwrite")); + pg.write_all_files(); + + return 0; +} diff --git a/roseus_bt/src/create_bt_tutorials.cpp b/roseus_bt/src/create_bt_tutorials.cpp new file mode 100644 index 000000000..5c0450958 --- /dev/null +++ b/roseus_bt/src/create_bt_tutorials.cpp @@ -0,0 +1,73 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace po = boost::program_options; + +int main(int argc, char** argv) +{ + std::string package_name = "roseus_bt_tutorials"; + std::string author = "Guilherme Affonso"; + std::vector model_files, executable_names; + std::string path = ros::package::getPath("roseus_bt") + "/sample/models/"; + + po::options_description desc("usage"); + desc.add_options() + ("help,h", "show this help message and exit") + ("overwrite,y", "overwrite all existing files") + ("verbose,v", "print all logging messages"); + + po::variables_map args; + po::store(po::parse_command_line(argc, argv, desc), args); + po::notify(args); + + // Initialize Logger + auto logger_level = boost::log::trivial::warning; + if (args.count("verbose")) { + logger_level = boost::log::trivial::trace; + } + + boost::log::core::get()->set_filter( + boost::log::trivial::severity >= logger_level); + + // Help + if (args.count("help")) { + std::cout << "\n" << "Create a behavior tree package with tutorial code." << "\n"; + std::cout << desc << std::endl; + return 0; + } + + // Set model files + model_files.push_back(path + "t01_simple_tree.xml"); + model_files.push_back(path + "t02_conditions.xml"); + model_files.push_back(path + "t03_ports.xml"); + model_files.push_back(path + "t04_subscriber.xml"); + model_files.push_back(path + "t05_subtrees.xml"); + model_files.push_back(path + "t06_reactive.xml"); + // model_files.push_back(path + "t07_xacro.xml"); + model_files.push_back(path + "t08_multimaster.xml"); + + // Set executable names + executable_names.resize(model_files.size()); + for (auto model_it = model_files.begin(), exec_it = executable_names.begin(); + model_it != model_files.end(); + ++model_it, ++exec_it) { + std::string model_file = *model_it; + std::string executable = *exec_it; + *exec_it = boost::filesystem::path(model_file).stem().string(); + } + + // Generate files + RoseusBT::PackageGenerator pg(package_name, + model_files, executable_names, + author, args.count("overwrite")); + pg.write_all_files(); + + return 0; +} diff --git a/roseus_smach/src/state-machine-utils.l b/roseus_smach/src/state-machine-utils.l index 033129604..8272d4691 100644 --- a/roseus_smach/src/state-machine-utils.l +++ b/roseus_smach/src/state-machine-utils.l @@ -1,7 +1,9 @@ ;; state-machine-utils.l (defun exec-state-machine (sm &optional (mydata '(nil)) - &key (spin t) (hz 1) (root-name "SM_ROOT") (srv-name "/server_name") iterate) + &key (spin t) (hz 1) (root-name "SM_ROOT") (srv-name "/server_name") iterate + (before-hook-func) (after-hook-func) + ) "Execute state machine Args: @@ -21,28 +23,31 @@ Returns: (apply #'send sm :arg-keys (union (send sm :arg-keys) (mapcar #'car mydata))) (ros::rate hz) - (while (ros::ok) - (when spin - (send insp :spin-once) - (if (and (boundp '*ri*) *ri*) (send *ri* :spin-once))) - (send insp :publish-status mydata) - (when (send sm :goal-reached) - (return)) - (when iterate - (cond - ((functionp iterate) - (unless (funcall iterate (send sm :active-state)) - (ros::ros-warn "set abort in iteration") - (return)) - (iterate - (unless (y-or-n-p (format nil "Execute ~A ? " - (send (send sm :active-state) :name))) - (ros::ros-warn "aborting...") - (return)) - (t (error "value of key :iterate must be t or function")))))) - (send sm :execute mydata :step -1) - (ros::sleep)) - (send sm :active-state))) + (catch :exec-state-machine-loop + (while (ros::ok) + (when spin + (send insp :spin-once) + (if (and (boundp '*ri*) *ri*) (send *ri* :spin-once))) + (send insp :publish-status mydata) + (when (send sm :goal-reached) + (return)) + (when iterate + (cond + ((functionp iterate) + (unless (funcall iterate (send sm :active-state)) + (ros::ros-warn "set abort in iteration") + (return)) + (iterate + (unless (y-or-n-p (format nil "Execute ~A ? " + (send (send sm :active-state) :name))) + (ros::ros-warn "aborting...") + (return)) + (t (error "value of key :iterate must be t or function")))))) + (if before-hook-func (funcall before-hook-func mydata)) + (send sm :execute mydata :step -1) + (if after-hook-func (funcall after-hook-func mydata)) + (ros::sleep)) + (send sm :active-state)))) (defun smach-exec (sm) "Deprecated function" diff --git a/roseus_smach/src/state-machine.l b/roseus_smach/src/state-machine.l index 505fcb2ac..cd34d4cfa 100644 --- a/roseus_smach/src/state-machine.l +++ b/roseus_smach/src/state-machine.l @@ -1,7 +1,7 @@ ;; state-machine.l ;; manipulate a list of cons as a associative array -(defun set-alist (k v alist &key (key 'car) (test 'eq)) +(defun set-alist (k v alist &key (key) (test 'eq)) (let ((cur-cons (assoc k alist :key key :test test))) (if cur-cons (progn (setf (cdr cur-cons) v) alist)