From e6312baf794278543b82637df7bda048bb64d251 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 18 Oct 2022 22:21:37 +0900 Subject: [PATCH 001/176] add before and after hook for state machine execution --- roseus_smach/src/state-machine-utils.l | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/roseus_smach/src/state-machine-utils.l b/roseus_smach/src/state-machine-utils.l index 033129604..c32ac5d43 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: @@ -40,7 +42,9 @@ Returns: (ros::ros-warn "aborting...") (return)) (t (error "value of key :iterate must be t or function")))))) + (if before-hook-func (funcall before-hook-func)) (send sm :execute mydata :step -1) + (if after-hook-func (funcall after-hook-func)) (ros::sleep)) (send sm :active-state))) From 8c52eb31d24d6c338eab41d26ff740d126c9c2c7 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 18 Oct 2022 22:21:52 +0900 Subject: [PATCH 002/176] add catch loop for global exit --- roseus_smach/src/state-machine-utils.l | 49 +++++++++++++------------- 1 file changed, 25 insertions(+), 24 deletions(-) diff --git a/roseus_smach/src/state-machine-utils.l b/roseus_smach/src/state-machine-utils.l index c32ac5d43..e2ee985f7 100644 --- a/roseus_smach/src/state-machine-utils.l +++ b/roseus_smach/src/state-machine-utils.l @@ -23,30 +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")))))) - (if before-hook-func (funcall before-hook-func)) - (send sm :execute mydata :step -1) - (if after-hook-func (funcall after-hook-func)) - (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)) + (send sm :execute mydata :step -1) + (if after-hook-func (funcall after-hook-func)) + (ros::sleep)) + (send sm :active-state)))) (defun smach-exec (sm) "Deprecated function" From ad849b6faf8b152f82ebc993db7ce2b83e811845 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 20 May 2021 12:53:02 +0900 Subject: [PATCH 003/176] Make sure that there is no subscriber when (one-shot-subscribe) is called --- roseus/euslisp/roseus-utils.l | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/roseus/euslisp/roseus-utils.l b/roseus/euslisp/roseus-utils.l index 99fc13e3a..4abae704b 100644 --- a/roseus/euslisp/roseus-utils.l +++ b/roseus/euslisp/roseus-utils.l @@ -1134,8 +1134,11 @@ (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 ((num-publishers (ros::get-num-publishers topic-name)) lmsg) + (when num-publishers + (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)) + (unless num-publishers (cond (after-stamp (ros::subscribe topic-name mclass From 8a6657641273f8b7283fe20618689628755bcf85 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 20 May 2021 13:14:03 +0900 Subject: [PATCH 004/176] Use pointer closures for callback function so that they can be accessed from anyscope --- roseus/euslisp/roseus-utils.l | 31 ++++++++++++++++++++++--------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/roseus/euslisp/roseus-utils.l b/roseus/euslisp/roseus-utils.l index 4abae704b..10194640e 100644 --- a/roseus/euslisp/roseus-utils.l +++ b/roseus/euslisp/roseus-utils.l @@ -1132,9 +1132,26 @@ (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 ((num-publishers (ros::get-num-publishers topic-name)) lmsg) + (let ((num-publishers (ros::get-num-publishers topic-name)) + (lmsg (instance one-shot-subscribe-msg :init))) (when num-publishers (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)) @@ -1142,14 +1159,10 @@ (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))))) @@ -1157,9 +1170,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" From fd432e30777ed5e77126228f6bab0d1584b9ca86 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 20 May 2021 21:01:07 +0900 Subject: [PATCH 005/176] Use if statement instead of when ... unless ... --- roseus/euslisp/roseus-utils.l | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/roseus/euslisp/roseus-utils.l b/roseus/euslisp/roseus-utils.l index 10194640e..9b432b60b 100644 --- a/roseus/euslisp/roseus-utils.l +++ b/roseus/euslisp/roseus-utils.l @@ -1150,12 +1150,11 @@ (defun one-shot-subscribe (topic-name mclass &key (timeout) (after-stamp) (unsubscribe t)) "Subscribe message, just for once" - (let ((num-publishers (ros::get-num-publishers topic-name)) - (lmsg (instance one-shot-subscribe-msg :init))) - (when num-publishers - (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)) - (unless num-publishers + (let ((lmsg (instance one-shot-subscribe-msg :init))) + (if (ros::get-num-publishers 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 From 694371feda3436970aae4c666b7a29e675f98fd6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 6 Mar 2019 18:30:49 +0900 Subject: [PATCH 006/176] Changes to 'print-ros-msg' --- roseus/euslisp/roseus-utils.l | 39 ++++++++++++++--------------------- 1 file changed, 15 insertions(+), 24 deletions(-) diff --git a/roseus/euslisp/roseus-utils.l b/roseus/euslisp/roseus-utils.l index 9b432b60b..d5e023f3a 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") From 57501eb54a9478fbaf9425da27347008e3ac5150 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 14 Apr 2021 14:10:11 +0900 Subject: [PATCH 007/176] Update sigint and error handlers --- roseus/euslisp/roseus.l | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/roseus/euslisp/roseus.l b/roseus/euslisp/roseus.l index 11e4a6f2f..a293340e4 100644 --- a/roseus/euslisp/roseus.l +++ b/roseus/euslisp/roseus.l @@ -14,25 +14,24 @@ (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)) From 680ed2da5febbb93ef51d33903b7be7342731ca2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 14 Apr 2021 14:18:16 +0900 Subject: [PATCH 008/176] Update script handlers --- roseus/cmake/roseus.cmake | 2 +- roseus/scripts/generate-all-msg-srv.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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/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 } From d532be860d92caeb98556ae28a9d379e6883dc72 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 14 Apr 2021 14:31:36 +0900 Subject: [PATCH 009/176] Always use ROSCONSOLE_DEFAULT_NAME on set-logger-level --- roseus/euslisp/roseus.l | 3 +-- roseus/roseus.cpp | 13 ++++++------- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/roseus/euslisp/roseus.l b/roseus/euslisp/roseus.l index a293340e4..50d70e5ef 100644 --- a/roseus/euslisp/roseus.l +++ b/roseus/euslisp/roseus.l @@ -38,7 +38,6 @@ (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" @@ -53,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..d32ca4b28 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -1746,11 +1746,8 @@ pointer ROSEUS_GETNAMESPACE(register context *ctx,int n,pointer *argv) pointer ROSEUS_SET_LOGGER_LEVEL(register context *ctx, int n, pointer *argv) { - ckarg(2); - string logger; - if (isstring(argv[0])) logger.assign((char *)get_string(argv[0])); - else error(E_NOSTRING); - int log_level = intval(argv[1]); + ckarg(1); + int log_level = intval(argv[0]); ros::console::levels::Level level = ros::console::levels::Debug; switch(log_level){ case 1: @@ -1772,7 +1769,9 @@ pointer ROSEUS_SET_LOGGER_LEVEL(register context *ctx, int n, pointer *argv) return (NIL); } - bool success = ::ros::console::set_logger_level(logger, level); + // roseus currently does not support multiple loggers + // which must be outputted using the 'ROS_DEBUG_NAMED'-like macros + bool success = ::ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, level); if (success) { console::notifyLoggerLevelsChanged(); @@ -2117,7 +2116,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."); From ab6150958a97d7c7aa0497a7acedd70b6058b741 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 14 Apr 2021 15:00:13 +0900 Subject: [PATCH 010/176] Give ros::spin-once more meaningful return values --- roseus/roseus.cpp | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index d32ca4b28..b4cfbd6ee 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -688,7 +688,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); } @@ -722,6 +722,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 +731,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) @@ -856,7 +859,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); } From e1540fbe3876caf26c1ed51f51d2481a606c7e6f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 14 Apr 2021 15:37:17 +0900 Subject: [PATCH 011/176] Allow interruptions on ROS sleep functions --- roseus/roseus.cpp | 47 +++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 41 insertions(+), 6 deletions(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index b4cfbd6ee..490d31b64 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -564,6 +564,7 @@ void roseusSignalHandler(int sig) // memoize for euslisp handler... context *ctx=euscontexts[thr_self()]; ctx->intsig = sig; + ros::Time::shutdown(); } /************************************************************ @@ -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(); } @@ -784,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); } @@ -1084,20 +1098,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]); + + // 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();} + } - bool bSuccess = service::waitForService(service, ros::Duration(timeout)); + if (result && printed && ros::ok()) + ROS_INFO("waitForService: Service [%s] is now available.", service.c_str()); - return (bSuccess?T:NIL); + return (result?T:NIL); } pointer ROSEUS_SERVICE_EXISTS(register context *ctx,int n,pointer *argv) From 847f55a07d922451b867c72a0ea1b75a2953ef51 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 16 Apr 2021 17:10:36 +0900 Subject: [PATCH 012/176] Define gc-proof symbols as constants --- roseus/roseus.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index 490d31b64..ac3d9a126 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(); @@ -2011,7 +2011,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()); From 1bf4246dcdfe3cb35879e59590609868c8168a57 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 28 May 2021 13:34:17 +0900 Subject: [PATCH 013/176] Fix possible memory faults when registering subscribers --- roseus/roseus.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index ac3d9a126..243e2b1b8 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -888,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); From 009bc73353fa2e4d7841efb82019863308ac07d0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 16 Aug 2021 16:14:54 +0900 Subject: [PATCH 014/176] Shutdown ros before clearing static data on exit to avoid eventual locks --- roseus/roseus.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index 243e2b1b8..daf911c16 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -837,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])); From 3a8db4951123ded0ebde9b83ef9fadc4ee3c8b20 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 13 Apr 2022 09:25:01 +0900 Subject: [PATCH 015/176] Use ros::get-topic-subscriber instead of ros::get-num-publishers in one-shot-subscribe --- roseus/euslisp/roseus-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus/euslisp/roseus-utils.l b/roseus/euslisp/roseus-utils.l index d5e023f3a..27eb62ef3 100644 --- a/roseus/euslisp/roseus-utils.l +++ b/roseus/euslisp/roseus-utils.l @@ -1142,7 +1142,7 @@ (defun one-shot-subscribe (topic-name mclass &key (timeout) (after-stamp) (unsubscribe t)) "Subscribe message, just for once" (let ((lmsg (instance one-shot-subscribe-msg :init))) - (if (ros::get-num-publishers topic-name) + (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)) From d51c1bdb47dac0701dd1309fb69f527f1e27b93c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 13 Apr 2022 09:39:24 +0900 Subject: [PATCH 016/176] Add quotations to topic name in one-shot-subscribe error message --- roseus/euslisp/roseus-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus/euslisp/roseus-utils.l b/roseus/euslisp/roseus-utils.l index 27eb62ef3..7f6511665 100644 --- a/roseus/euslisp/roseus-utils.l +++ b/roseus/euslisp/roseus-utils.l @@ -1144,7 +1144,7 @@ (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)) + (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 From 68b5ead4c675c5301ecc39320cc570960b0d338b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 15:40:25 +0900 Subject: [PATCH 017/176] Add roseus_bt package --- roseus_bt/CMakeLists.txt | 25 ++++++ roseus_bt/include/roseus_bt/xml_parser.h | 97 ++++++++++++++++++++++++ roseus_bt/package.xml | 22 ++++++ roseus_bt/src/test_parse.cpp | 14 ++++ 4 files changed, 158 insertions(+) create mode 100644 roseus_bt/CMakeLists.txt create mode 100644 roseus_bt/include/roseus_bt/xml_parser.h create mode 100644 roseus_bt/package.xml create mode 100644 roseus_bt/src/test_parse.cpp diff --git a/roseus_bt/CMakeLists.txt b/roseus_bt/CMakeLists.txt new file mode 100644 index 000000000..81503f97d --- /dev/null +++ b/roseus_bt/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.0.2) +project(roseus_bt) + +find_package(catkin REQUIRED COMPONENTS + cmake_modules + roscpp + behaviortree_ros +) + +find_package(TinyXML2 REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES + CATKIN_DEPENDS + DEPENDS TinyXML2 +) + + +include_directories( include ${catkin_INCLUDE_DIRS} ${TinyXML2_INCUDE_DIRS}) + + +add_executable(test_parse src/test_parse.cpp) +add_dependencies(test_parse ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_parse ${catkin_LIBRARIES} ${TinyXML2_LIBRARIES}) 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..5207791d5 --- /dev/null +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -0,0 +1,97 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ +#define BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ + +#include +#include +#include +#include + +namespace RoseusBT +{ + using namespace tinyxml2; + + +class XMLParser +{ + +public: + + XMLParser(const std::string &filename) { + doc.LoadFile(filename.c_str()); + } + + ~XMLParser() {}; + +protected: + + XMLDocument doc; + std::string port_node_to_message_description(const XMLElement* port_node); + std::string generate_action_file_contents(const XMLElement* node); + +public: + + std::string test_all_actions(); + +}; + + +std::string XMLParser::port_node_to_message_description(const XMLElement* port_node) { + if (!port_node->Attribute("type") || + !port_node->Attribute("name")) { + std::string error_str = "Illformed port in "; + error_str.append(port_node->Name()); + throw std::logic_error(error_str); + } + + std::string result; + result.append(port_node->Attribute("type")); + result.append(" "); + result.append(port_node->Attribute("name")); + return result; +} + +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 = port_node_to_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); + } + } + + std::string output; + output.append(boost::algorithm::join(goal, "\n")); + output.append("\n---\n"); + output.append("bool success"); + output.append("\n---\n"); + output.append(boost::algorithm::join(feedback, "\n")); + + return output; +} + +std::string XMLParser::test_all_actions() { + std::string 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.append(generate_action_file_contents(action_node)); + result.append("\n\n"); + } + return result; +} + +} // namespace BT + +#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..55ed42ebf --- /dev/null +++ b/roseus_bt/package.xml @@ -0,0 +1,22 @@ + + + roseus_bt + 0.0.1 + The roseus_bt package + + affonso + BSD + + catkin + + roscpp + tinyxml2 + behaviortree_ros + + roscpp + tinyxml2 + behaviortree_ros + + + + diff --git a/roseus_bt/src/test_parse.cpp b/roseus_bt/src/test_parse.cpp new file mode 100644 index 000000000..9327e93cb --- /dev/null +++ b/roseus_bt/src/test_parse.cpp @@ -0,0 +1,14 @@ +#include +#include + + +int main(int argc, char **argv) +{ + std::cout << "Start..." << std::endl; + RoseusBT::XMLParser parser(argv[1]); + + std::cout << parser.test_all_actions() << std::endl; + std::cout << "...End" << std::endl; + + return 0; +} From 26a8f08cb48424375029b9bd4bb3e248d9a59873 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 15:55:32 +0900 Subject: [PATCH 018/176] (roseus_bt) Parse to service files --- roseus_bt/include/roseus_bt/xml_parser.h | 46 ++++++++++++++++++++++++ roseus_bt/src/test_parse.cpp | 6 ++-- 2 files changed, 50 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 5207791d5..8c6d07d03 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -27,10 +27,12 @@ class XMLParser XMLDocument doc; std::string port_node_to_message_description(const XMLElement* port_node); std::string generate_action_file_contents(const XMLElement* node); + std::string generate_service_file_contents(const XMLElement* node); public: std::string test_all_actions(); + std::string test_all_conditions(); }; @@ -78,6 +80,32 @@ std::string XMLParser::generate_action_file_contents(const XMLElement* node) { return output; } +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 = port_node_to_message_description(port_node); + + if (name == "input_port") { + request.push_back(text); + } + else { + throw std::logic_error("Condition Node only accepts input ports!"); + } + } + + std::string output; + output.append(boost::algorithm::join(request, "\n")); + output.append("\n---\n"); + output.append("bool success"); + + return output; +} + std::string XMLParser::test_all_actions() { std::string result; const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); @@ -86,12 +114,30 @@ std::string XMLParser::test_all_actions() { action_node != nullptr; action_node = action_node->NextSiblingElement("Action")) { + result.append(action_node->Attribute("ID")); + result.append(":\n"); result.append(generate_action_file_contents(action_node)); result.append("\n\n"); } return result; } +std::string XMLParser::test_all_conditions() { + std::string 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.append(condition_node->Attribute("ID")); + result.append(":\n"); + result.append(generate_service_file_contents(condition_node)); + result.append("\n\n"); + } + return result; +} + } // namespace BT #endif // BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ diff --git a/roseus_bt/src/test_parse.cpp b/roseus_bt/src/test_parse.cpp index 9327e93cb..0b4d7227e 100644 --- a/roseus_bt/src/test_parse.cpp +++ b/roseus_bt/src/test_parse.cpp @@ -4,11 +4,13 @@ int main(int argc, char **argv) { - std::cout << "Start..." << std::endl; RoseusBT::XMLParser parser(argv[1]); + std::cout << "Actions:" << std::endl; std::cout << parser.test_all_actions() << std::endl; - std::cout << "...End" << std::endl; + + std::cout << "Conditions:" << std::endl; + std::cout << parser.test_all_conditions() << std::endl; return 0; } From bff6c54e9054f8d0c8e9ee815aaa9130fffc5282 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 15:56:42 +0900 Subject: [PATCH 019/176] (roseus_bt) Add EusActionNode and EusConditionNode --- roseus_bt/include/roseus_bt/eus_action_node.h | 103 ++++++++++++++++++ .../include/roseus_bt/eus_condition_node.h | 22 ++++ 2 files changed, 125 insertions(+) create mode 100644 roseus_bt/include/roseus_bt/eus_action_node.h create mode 100644 roseus_bt/include/roseus_bt/eus_condition_node.h 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..0ec620eeb --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_action_node.h @@ -0,0 +1,103 @@ +#ifndef BEHAVIOR_TREE_EUS_ACTION_NODE_HPP_ +#define BEHAVIOR_TREE_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) {}; + +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_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..52ed5aab1 --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_condition_node.h @@ -0,0 +1,22 @@ +#ifndef BEHAVIOR_TREE_EUS_CONDITION_NODE_HPP_ +#define BEHAVIOR_TREE_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) {}; + +public: + EusConditionNode() = delete; + virtual ~EusConditionNode() = default; +}; + +} // namespace BT + +#endif // BEHAVIOR_TREE_EUS_CONDITION_NODE_HPP_ From 4152fe576471769ba577e1f9e0db5461c0b704a5 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 15:57:40 +0900 Subject: [PATCH 020/176] (roseus_bt) Add euslisp roseus_bt package and utilities --- roseus_bt/euslisp/nodes.l | 114 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 114 insertions(+) create mode 100644 roseus_bt/euslisp/nodes.l diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l new file mode 100644 index 000000000..3271bc189 --- /dev/null +++ b/roseus_bt/euslisp/nodes.l @@ -0,0 +1,114 @@ +(unless (find-package "ROSEUS_BT") + (make-package "ROSEUS_BT")) + +(in-package "ROSEUS_BT") + +(export '(define-action-callback define-condition-callback + action-node condition-node spin-once set-output ok)) + +(defvar *action-list*) +(defvar *condition-list*) + + +;; utility +(defun get-fn-sym (fn) + (when (functionp fn) + (cond + ((symbolp fn) fn) + ((and (listp fn) (memq (car fn) '(lambda lambda-closure))) + (cadr fn)) + ((derivedp fn compiled-code) + (send fn :name)) + (t nil)))) + +;; macros +(defmacro define-action-callback (name arg-list &rest body) + (user::with-gensyms (server goal get-result) + `(prog1 + (defun ,name (,server ,goal) + (labels ((set-output (name val) + (let ((msg (send ,server :feedback + (intern (string-upcase name) *keyword-package*) val))) + (send ,server :publish-feedback msg))) + (ok () + (send ,server :spin-once) + (not (send ,server :is-preempt-requested))) + (,get-result ,arg-list + (block ,name + ,@body))) + (let (,@(mapcar + #'(lambda (k) `(,k (send ,goal :goal + ,(intern (symbol-pname k) *keyword-package*)))) + arg-list)) + (send ,server :set-succeeded + (send ,server :result :success + (,get-result ,@arg-list)))))) + (setf (get ',name :function-type) :action-node-callback) + (setf (get ',name :function-documentation) ,(format nil "~S" arg-list))))) + + +(defmacro define-condition-callback (name arg-list &rest body) + (user::with-gensyms (request get-result) + `(prog1 + (defun ,name (,request) + (labels ((,get-result ,arg-list + (block ,name + ,@body))) + (let ((response (send ,request :response)) + ,@(mapcar + #'(lambda (k) `(,k (send ,request ,(intern (symbol-pname k) *keyword-package*)))) + arg-list)) + (send response :success (,get-result ,@arg-list)) + response))) + (setf (get ',name :function-type) :condition-node-callback) + (setf (get ',name :function-documentation) ,(format nil "~S" arg-list))))) + + +;; classes +(defclass action-node :super ros::simple-action-server) +(defmethod action-node + (:init (ns spec &key execute-cb preempt-cb accept-cb groupname) + (unless (eq (get (get-fn-sym execute-cb) :function-type) :action-node-callback) + (ros::ros-error + (concatenate-string + "Wrong function type detected! " + "Make sure to define execution callbacks using the define-action-callback macro")) + (error type-error "action-node-callback function expected for execute-cb")) + + (send-super :init ns spec + :execute-cb execute-cb + :preempt-cb preempt-cb + :accept-cb accept-cb + :groupname groupname) + (push self *action-list*) + (if *condition-list* + (ros::ros-warn "Actions and Conditions detected in the same node! Start two separate nodes when reacitivity is required.")) + self)) + +(defclass condition-node :super propertied-object) +(defmethod condition-node + (:init (ns spec &key execute-cb) + (unless (eq (get (get-fn-sym execute-cb) :function-type) :condition-node-callback) + (ros::ros-error + (concatenate-string + "Wrong function type detected! " + "Make sure to define execution callbacks using the define-condition-callback macro")) + (error type-error "condition-node-callback function expected for execute-cb")) + + (ros::advertise-service ns spec execute-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)) + + +;; functions +(defun spin-once () + (ros::spin-once) + (dolist (ac *action-list*) + (send ac :worker))) + +(defun set-output (name val) + (error "not defined in global scope")) + +(defun ok () t) From f199f52d16cb65e8b874da39e2a3c8959bb5a674 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 18:02:46 +0900 Subject: [PATCH 021/176] (roseus_bt) Parse header --- roseus_bt/CMakeLists.txt | 5 +- roseus_bt/include/roseus_bt/xml_parser.h | 60 ++++++++++++++++++++++-- roseus_bt/package.xml | 2 + roseus_bt/src/test_parse.cpp | 10 ++-- 4 files changed, 66 insertions(+), 11 deletions(-) diff --git a/roseus_bt/CMakeLists.txt b/roseus_bt/CMakeLists.txt index 81503f97d..d9d7e2ec0 100644 --- a/roseus_bt/CMakeLists.txt +++ b/roseus_bt/CMakeLists.txt @@ -8,12 +8,13 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(TinyXML2 REQUIRED) +find_package(fmt) catkin_package( INCLUDE_DIRS include LIBRARIES CATKIN_DEPENDS - DEPENDS TinyXML2 + DEPENDS TinyXML2 fmt ) @@ -22,4 +23,4 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${TinyXML2_INCUDE_DIRS}) add_executable(test_parse src/test_parse.cpp) add_dependencies(test_parse ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(test_parse ${catkin_LIBRARIES} ${TinyXML2_LIBRARIES}) +target_link_libraries(test_parse ${catkin_LIBRARIES} ${TinyXML2_LIBRARIES} fmt::fmt) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 8c6d07d03..aa5fa7793 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -4,6 +4,7 @@ #include #include #include +#include #include namespace RoseusBT @@ -33,6 +34,7 @@ class XMLParser std::string test_all_actions(); std::string test_all_conditions(); + std::string generate_headers(const char* package_name); }; @@ -45,11 +47,10 @@ std::string XMLParser::port_node_to_message_description(const XMLElement* port_n throw std::logic_error(error_str); } - std::string result; - result.append(port_node->Attribute("type")); - result.append(" "); - result.append(port_node->Attribute("name")); - return result; + std::string output = fmt::format("{} {}", + port_node->Attribute("type"), + port_node->Attribute("name")); + return output; } std::string XMLParser::generate_action_file_contents(const XMLElement* node) { @@ -106,6 +107,55 @@ std::string XMLParser::generate_service_file_contents(const XMLElement* node) { return output; } +std::string XMLParser::generate_headers(const char* package_name) { + auto format_action_node = [](const XMLElement* node, const char* package_name) { + return fmt::format("#include <{}/{}Action.h>", + package_name, + node->Attribute("ID")); + }; + + auto format_condition_node = [](const XMLElement* node, const char* package_name) { + return fmt::format("#include <{}/{}.h>", + package_name, + node->Attribute("ID")); + }; + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + std::vector headers; + + std::string common_headers = 1 + R"( +#include + +#include +#include + +#include +#include + +)"; + + for (auto action_node = root->FirstChildElement("Action"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("Action")) + { + headers.push_back(format_action_node(action_node, package_name)); + } + + for (auto condition_node = root->FirstChildElement("Condition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("Condition")) + { + headers.push_back(format_condition_node(condition_node, package_name)); + } + + std::string output; + output.append(common_headers); + output.append(boost::algorithm::join(headers, "\n")); + + return output; +} + + std::string XMLParser::test_all_actions() { std::string result; const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); diff --git a/roseus_bt/package.xml b/roseus_bt/package.xml index 55ed42ebf..ce017bf03 100644 --- a/roseus_bt/package.xml +++ b/roseus_bt/package.xml @@ -11,10 +11,12 @@ roscpp tinyxml2 + fmt behaviortree_ros roscpp tinyxml2 + fmt behaviortree_ros diff --git a/roseus_bt/src/test_parse.cpp b/roseus_bt/src/test_parse.cpp index 0b4d7227e..037d41121 100644 --- a/roseus_bt/src/test_parse.cpp +++ b/roseus_bt/src/test_parse.cpp @@ -6,11 +6,13 @@ int main(int argc, char **argv) { RoseusBT::XMLParser parser(argv[1]); - std::cout << "Actions:" << std::endl; - std::cout << parser.test_all_actions() << std::endl; + std::cout << parser.generate_headers("my_package") << std::endl; - std::cout << "Conditions:" << std::endl; - std::cout << parser.test_all_conditions() << std::endl; + // std::cout << "Actions:" << std::endl; + // std::cout << parser.test_all_actions() << std::endl; + + // std::cout << "Conditions:" << std::endl; + // std::cout << parser.test_all_conditions() << std::endl; return 0; } From f66d59b15fce5d99a99f98615ba5b65a134d448a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 19:13:31 +0900 Subject: [PATCH 022/176] (roseus_bt) Parse action classes --- roseus_bt/include/roseus_bt/xml_parser.h | 116 ++++++++++++++++++++++- roseus_bt/src/test_parse.cpp | 2 + 2 files changed, 117 insertions(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index aa5fa7793..2ca080e65 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -5,6 +5,7 @@ #include #include #include +#include #include namespace RoseusBT @@ -29,12 +30,14 @@ class XMLParser std::string port_node_to_message_description(const XMLElement* port_node); std::string generate_action_file_contents(const XMLElement* node); std::string generate_service_file_contents(const XMLElement* node); + std::string generate_action_class(const XMLElement* node, const char* package_name); public: std::string test_all_actions(); std::string test_all_conditions(); std::string generate_headers(const char* package_name); + std::string test_all_action_classes(const char* package_name); }; @@ -155,11 +158,108 @@ std::string XMLParser::generate_headers(const char* package_name) { return output; } +std::string XMLParser::generate_action_class(const XMLElement* node, const char* package_name) { + auto format_input_port = [](const XMLElement* node) { + return fmt::format(" InputPortAttribute("name")); + }; + auto format_output_port = [](const XMLElement* node) { + return fmt::format(" OutputPortAttribute("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(" 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; + + 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()); + + + std::string fmt_string = 1 + R"( +class %2%: public EusActionNode<%1%::%2%Action> +{ + +public: + %2%Action(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 % + node->Attribute("ID") % + boost::algorithm::join(provided_ports, ",\n") % + boost::algorithm::join(get_inputs, "\n") % + boost::algorithm::join(set_outputs, "\n"); + + return bfmt.str(); +} + std::string XMLParser::test_all_actions() { std::string result; const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); - for (auto action_node = root->FirstChildElement("Action"); action_node != nullptr; action_node = action_node->NextSiblingElement("Action")) @@ -188,6 +288,20 @@ std::string XMLParser::test_all_conditions() { return result; } +std::string XMLParser::test_all_action_classes(const char* package_name) { + std::string 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.append(generate_action_class(action_node, package_name)); + result.append("\n\n"); + } + return result; +} + } // namespace BT #endif // BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ diff --git a/roseus_bt/src/test_parse.cpp b/roseus_bt/src/test_parse.cpp index 037d41121..3ee239489 100644 --- a/roseus_bt/src/test_parse.cpp +++ b/roseus_bt/src/test_parse.cpp @@ -7,6 +7,8 @@ int main(int argc, char **argv) RoseusBT::XMLParser parser(argv[1]); std::cout << parser.generate_headers("my_package") << std::endl; + std::cout << std::endl << std::endl; + std::cout << parser.test_all_action_classes("my_package") << std::endl; // std::cout << "Actions:" << std::endl; // std::cout << parser.test_all_actions() << std::endl; From fe0bc07c1c840610bb3a1f7a2ca6406bca68d1ed Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 20:55:11 +0900 Subject: [PATCH 023/176] (roseus_bt) Parse condition classes --- roseus_bt/include/roseus_bt/xml_parser.h | 80 ++++++++++++++++++++++++ roseus_bt/src/test_parse.cpp | 2 + 2 files changed, 82 insertions(+) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 2ca080e65..6e6740966 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -31,6 +31,7 @@ class XMLParser std::string generate_action_file_contents(const XMLElement* node); std::string generate_service_file_contents(const XMLElement* node); std::string generate_action_class(const XMLElement* node, const char* package_name); + std::string generate_condition_class(const XMLElement* node, const char* package_name); public: @@ -38,6 +39,7 @@ class XMLParser std::string test_all_conditions(); std::string generate_headers(const char* package_name); std::string test_all_action_classes(const char* package_name); + std::string test_all_condition_classes(const char* package_name); }; @@ -256,6 +258,70 @@ EusActionNode<%1%::%2%Action>(handle, name, conf) {} return bfmt.str(); } +std::string XMLParser::generate_condition_class(const XMLElement* node, const char* package_name) { + auto format_input_port = [](const XMLElement* node) { + return fmt::format(" InputPortAttribute("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; + + 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)); + } + + std::string fmt_string = 1 + R"( +class %2%: public EusConditionNode<%1%::%2%> +{ + +public: + %2%(ros::NodeHandle& handle, const std::string& node_name, const NodeConfiguration& conf): + EusConditionNode<%1%::%2%>(handle, node_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(EusConditionNode::FailureCause failure) override + { + return NodeStatus::FAILURE; + } + +}; +)"; + + boost::format bfmt = boost::format(fmt_string) % + package_name % + node->Attribute("ID") % + boost::algorithm::join(provided_ports, ",\n") % + boost::algorithm::join(get_inputs, "\n"); + + return bfmt.str(); +} + std::string XMLParser::test_all_actions() { std::string result; @@ -302,6 +368,20 @@ std::string XMLParser::test_all_action_classes(const char* package_name) { return result; } +std::string XMLParser::test_all_condition_classes(const char* package_name) { + std::string 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.append(generate_condition_class(condition_node, package_name)); + result.append("\n\n"); + } + return result; +} + } // namespace BT #endif // BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ diff --git a/roseus_bt/src/test_parse.cpp b/roseus_bt/src/test_parse.cpp index 3ee239489..81bc559be 100644 --- a/roseus_bt/src/test_parse.cpp +++ b/roseus_bt/src/test_parse.cpp @@ -9,6 +9,8 @@ int main(int argc, char **argv) std::cout << parser.generate_headers("my_package") << std::endl; std::cout << std::endl << std::endl; std::cout << parser.test_all_action_classes("my_package") << std::endl; + std::cout << std::endl; + std::cout << parser.test_all_condition_classes("my_package") << std::endl; // std::cout << "Actions:" << std::endl; // std::cout << parser.test_all_actions() << std::endl; From aee6eae9a81a08e2a7377684febf8eeee0ea6cd0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 20:57:34 +0900 Subject: [PATCH 024/176] (roseus_bt) Parse main function --- roseus_bt/include/roseus_bt/xml_parser.h | 76 ++++++++++++++++++++++++ roseus_bt/src/test_parse.cpp | 2 + 2 files changed, 78 insertions(+) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 6e6740966..58004d4f3 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -38,6 +38,7 @@ class XMLParser std::string test_all_actions(); std::string test_all_conditions(); std::string generate_headers(const char* package_name); + std::string generate_main_function(const char* roscpp_node_name, const char* xml_filename); std::string test_all_action_classes(const char* package_name); std::string test_all_condition_classes(const char* package_name); @@ -322,6 +323,81 @@ class %2%: public EusConditionNode<%1%::%2%> return bfmt.str(); } +std::string XMLParser::generate_main_function(const char* roscpp_node_name, + const char* xml_filename) { + + auto format_ros_init = [](const char* roscpp_node_name) { + return fmt::format(" ros::init(argc, argv, \"{}\");", roscpp_node_name); + }; + auto format_create_tree = [](const char* xml_filename) { + return fmt::format(" auto tree = factory.createTreeFromFile(\"{}\");", xml_filename); + }; + auto format_action_node = [](const XMLElement* node) { + return fmt::format(" RegisterRosAction<{0}Action>(factory, \"{0}\", nh);", + node->Attribute("ID")); + }; + auto format_condition_node = [](const XMLElement* node) { + return fmt::format(" RegisterRosService<{0}>(factory, \"{0}\", nh);", + node->Attribute("ID")); + }; + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + std::vector register_actions; + std::vector register_conditions; + + 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 condition_node = root->FirstChildElement("Condition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("Condition")) + { + register_conditions.push_back(format_condition_node(condition_node)); + } + + std::string fmt_string = 1 + R"( +int main(int argc, char **argv) +{ +%1% + ros::NodeHandle nh; + + BehaviorTreeFactory factory; + +%3% +%4% + +%2% + + StdCoutLogger logger_cout(tree); + 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.05); + sleep_time.sleep(); + } + + return 0; +} +)"; + + boost::format bfmt = boost::format(fmt_string) % + format_ros_init(roscpp_node_name) % + format_create_tree(xml_filename) % + boost::algorithm::join(register_actions, ",\n") % + boost::algorithm::join(register_conditions, ",\n"); + + return bfmt.str(); +} + std::string XMLParser::test_all_actions() { std::string result; diff --git a/roseus_bt/src/test_parse.cpp b/roseus_bt/src/test_parse.cpp index 81bc559be..01a418d1a 100644 --- a/roseus_bt/src/test_parse.cpp +++ b/roseus_bt/src/test_parse.cpp @@ -11,6 +11,8 @@ int main(int argc, char **argv) std::cout << parser.test_all_action_classes("my_package") << std::endl; std::cout << std::endl; std::cout << parser.test_all_condition_classes("my_package") << std::endl; + std::cout << std::endl << std::endl; + std::cout << parser.generate_main_function("test", argv[1]) << std::endl; // std::cout << "Actions:" << std::endl; // std::cout << parser.test_all_actions() << std::endl; From 6cb4a61a020981e0a46c5ecbffeaf85b5cd1989d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 21:04:55 +0900 Subject: [PATCH 025/176] (roseus_bt) Fix namespace of eus nodes --- roseus_bt/include/roseus_bt/xml_parser.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 58004d4f3..186d409bf 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -210,12 +210,12 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const char* std::string fmt_string = 1 + R"( -class %2%: public EusActionNode<%1%::%2%Action> +class %2%: public BT::EusActionNode<%1%::%2%Action> { public: %2%Action(ros::NodeHandle& handle, const std::string& name, const NodeConfiguration& conf): -EusActionNode<%1%::%2%Action>(handle, name, conf) {} +BT::EusActionNode<%1%::%2%Action>(handle, name, conf) {} static PortsList providedPorts() { @@ -281,12 +281,12 @@ std::string XMLParser::generate_condition_class(const XMLElement* node, const ch } std::string fmt_string = 1 + R"( -class %2%: public EusConditionNode<%1%::%2%> +class %2%: public BT::EusConditionNode<%1%::%2%> { public: %2%(ros::NodeHandle& handle, const std::string& node_name, const NodeConfiguration& conf): - EusConditionNode<%1%::%2%>(handle, node_name, conf) {} + BT::EusConditionNode<%1%::%2%>(handle, node_name, conf) {} static PortsList providedPorts() { @@ -306,7 +306,7 @@ class %2%: public EusConditionNode<%1%::%2%> return NodeStatus::FAILURE; } - virtual NodeStatus onFailedRequest(EusConditionNode::FailureCause failure) override + virtual NodeStatus onFailedRequest(FailureCause failure) override { return NodeStatus::FAILURE; } From ff365ca26e3d7573e49f3a6c1bf4d7fdd49382ed Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 21:14:51 +0900 Subject: [PATCH 026/176] (roseus_bt) Add generate_cpp_file --- roseus_bt/include/roseus_bt/xml_parser.h | 37 +++++++++++++----------- roseus_bt/src/test_parse.cpp | 8 +---- 2 files changed, 21 insertions(+), 24 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 186d409bf..4955d7e65 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -30,17 +30,18 @@ class XMLParser std::string port_node_to_message_description(const XMLElement* port_node); std::string generate_action_file_contents(const XMLElement* node); std::string generate_service_file_contents(const XMLElement* node); + std::string generate_headers(const char* package_name); std::string generate_action_class(const XMLElement* node, const char* package_name); std::string generate_condition_class(const XMLElement* node, const char* package_name); + std::string generate_main_function(const char* roscpp_node_name, const char* xml_filename); public: std::string test_all_actions(); std::string test_all_conditions(); - std::string generate_headers(const char* package_name); - std::string generate_main_function(const char* roscpp_node_name, const char* xml_filename); - std::string test_all_action_classes(const char* package_name); - std::string test_all_condition_classes(const char* package_name); + std::string generate_cpp_file(const char* package_name, + const char* roscpp_node_name, + const char* xml_filename); }; @@ -430,32 +431,34 @@ std::string XMLParser::test_all_conditions() { return result; } -std::string XMLParser::test_all_action_classes(const char* package_name) { - std::string result; +std::string XMLParser::generate_cpp_file(const char* package_name, + const char* roscpp_node_name, + const char* 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")) { - result.append(generate_action_class(action_node, package_name)); - result.append("\n\n"); + output.append(generate_action_class(action_node, package_name)); + output.append("\n\n"); } - return result; -} - -std::string XMLParser::test_all_condition_classes(const char* package_name) { - std::string 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.append(generate_condition_class(condition_node, package_name)); - result.append("\n\n"); + output.append(generate_condition_class(condition_node, package_name)); + output.append("\n\n"); } - return result; + + output.append(generate_main_function(roscpp_node_name, xml_filename)); + + return output; } } // namespace BT diff --git a/roseus_bt/src/test_parse.cpp b/roseus_bt/src/test_parse.cpp index 01a418d1a..a1e194ccd 100644 --- a/roseus_bt/src/test_parse.cpp +++ b/roseus_bt/src/test_parse.cpp @@ -6,13 +6,7 @@ int main(int argc, char **argv) { RoseusBT::XMLParser parser(argv[1]); - std::cout << parser.generate_headers("my_package") << std::endl; - std::cout << std::endl << std::endl; - std::cout << parser.test_all_action_classes("my_package") << std::endl; - std::cout << std::endl; - std::cout << parser.test_all_condition_classes("my_package") << std::endl; - std::cout << std::endl << std::endl; - std::cout << parser.generate_main_function("test", argv[1]) << std::endl; + std::cout << parser.generate_cpp_file("my_package", "test", argv[1]) << std::endl; // std::cout << "Actions:" << std::endl; // std::cout << parser.test_all_actions() << std::endl; From 1e0e155a890683a0859f8b4ff61e36e936c042ff Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 11:11:07 +0900 Subject: [PATCH 027/176] (roseus_bt) Add generate_cmake_lists --- roseus_bt/include/roseus_bt/xml_parser.h | 109 +++++++++++++++++++++++ roseus_bt/src/test_parse.cpp | 3 +- 2 files changed, 111 insertions(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 4955d7e65..f7d4c8c3c 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -42,6 +42,8 @@ class XMLParser std::string generate_cpp_file(const char* package_name, const char* roscpp_node_name, const char* xml_filename); + std::string generate_cmake_lists(const char* package_name, + const char* target_filename); }; @@ -461,6 +463,113 @@ std::string XMLParser::generate_cpp_file(const char* package_name, return output; } +std::string XMLParser::generate_cmake_lists(const char* package_name, const char* target_name) { + auto format_pkg = [](const char* pkg) { + return fmt::format(" {}", pkg); + }; + 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")); + }; + auto maybe_push_message_package = [format_pkg](const XMLElement* node, std::vector* message_packages) { + std::string msg_type = node->Attribute("type"); + std::size_t pos = msg_type.find('/'); + if (pos != std::string::npos) { + std::string pkg = format_pkg(msg_type.substr(0, pos).c_str()); + if (std::find(message_packages->begin(), message_packages->end(), pkg) == + message_packages->end()) { + message_packages->push_back(pkg); + } + } + }; + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + std::vector message_packages; + std::vector action_files; + std::vector service_files; + + message_packages.push_back(format_pkg("std_msgs")); + message_packages.push_back(format_pkg("actionlib_msgs")); + for (auto action_node = root->FirstChildElement("Action"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("Action")) + { + action_files.push_back(format_action_file(action_node)); + for (auto port_node = action_node->FirstChildElement(); + port_node != nullptr; + port_node = port_node->NextSiblingElement()) + { + maybe_push_message_package(port_node, &message_packages); + } + } + + for (auto condition_node = root->FirstChildElement("Condition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("Condition")) + { + service_files.push_back(format_service_file(condition_node)); + for (auto port_node = condition_node->FirstChildElement(); + port_node != nullptr; + port_node = port_node->NextSiblingElement()) + { + maybe_push_message_package(port_node, &message_packages); + } + } + + std::string fmt_string = 1+ R"( +cmake_minimum_required(VERSION 3.0.2) +project(%1%) + +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + roseus_bt +%3% +) + +add_service_files( + FILES +%4% +) + +add_action_files( + FILES +%5% +) + +generate_messages( + DEPENDENCIES +%3% +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES + CATKIN_DEPENDS + message_runtime +%3% +) + + +include_directories(include ${catkin_INCLUDE_DIRS}) + +add_executable(%2% src/%2%.cpp) +add_dependencies(%2% ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(%2% ${catkin_LIBRARIES}) +)"; + + boost::format bfmt = boost::format(fmt_string) % + package_name % + target_name % + boost::algorithm::join(message_packages, "\n") % + boost::algorithm::join(service_files, "\n") % + boost::algorithm::join(action_files, "\n"); + + return bfmt.str(); +} + } // namespace BT #endif // BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ diff --git a/roseus_bt/src/test_parse.cpp b/roseus_bt/src/test_parse.cpp index a1e194ccd..ff816deb2 100644 --- a/roseus_bt/src/test_parse.cpp +++ b/roseus_bt/src/test_parse.cpp @@ -6,7 +6,8 @@ int main(int argc, char **argv) { RoseusBT::XMLParser parser(argv[1]); - std::cout << parser.generate_cpp_file("my_package", "test", argv[1]) << std::endl; + // std::cout << parser.generate_cpp_file("my_package", "test", argv[1]) << std::endl; + std::cout << parser.generate_cmake_lists("my_package", "test") << std::endl; // std::cout << "Actions:" << std::endl; // std::cout << parser.test_all_actions() << std::endl; From 96a844a1830f5f68d8778d14aaf6699a438df274 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 12:56:52 +0900 Subject: [PATCH 028/176] (roseus_bt) Add generate_package_xml --- roseus_bt/include/roseus_bt/xml_parser.h | 109 +++++++++++++++++++++++ roseus_bt/src/test_parse.cpp | 3 +- 2 files changed, 111 insertions(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index f7d4c8c3c..e2c663b74 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -44,6 +44,8 @@ class XMLParser const char* xml_filename); std::string generate_cmake_lists(const char* package_name, const char* target_filename); + std::string generate_package_xml(const char* package_name, + const char* author_name); }; @@ -570,6 +572,113 @@ target_link_libraries(%2% ${catkin_LIBRARIES}) return bfmt.str(); } +std::string XMLParser::generate_package_xml(const char* package_name, const char* author_name) { + auto format_build_depend = [](std::string pkg) { + return fmt::format(" {}", pkg); + }; + auto format_exec_depend = [](std::string pkg) { + return fmt::format(" {}", pkg); + }; + auto maybe_push_message_package = [](const XMLElement* node, std::vector* message_packages) { + std::string msg_type = node->Attribute("type"); + std::size_t pos = msg_type.find('/'); + if (pos != std::string::npos) { + std::string pkg = msg_type.substr(0, pos); + if (std::find(message_packages->begin(), message_packages->end(), pkg) == + message_packages->end()) { + message_packages->push_back(pkg); + } + } + }; + + 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(), ' ', '_'); + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + std::vector message_packages; + + message_packages.push_back("std_msgs"); + message_packages.push_back("actionlib_msgs"); + + for (auto node = root->FirstChildElement(); + node != nullptr; + node = node->NextSiblingElement()) + { + for (auto port_node = node->FirstChildElement(); + port_node != nullptr; + port_node = port_node->NextSiblingElement()) + { + maybe_push_message_package(port_node, &message_packages); + } + } + + 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); + + std::string fmt_string = 1 + R"( + + + %1% + 0.0.0 + The %1% package + + + %2% + + + + + + TODO + + + + + + + + + + + + + + + catkin + message_generation + roscpp + roseus_bt +%4% + + message_runtime + roscpp + 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(); +} + } // namespace BT #endif // BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ diff --git a/roseus_bt/src/test_parse.cpp b/roseus_bt/src/test_parse.cpp index ff816deb2..b2dd19318 100644 --- a/roseus_bt/src/test_parse.cpp +++ b/roseus_bt/src/test_parse.cpp @@ -7,7 +7,8 @@ int main(int argc, char **argv) RoseusBT::XMLParser parser(argv[1]); // std::cout << parser.generate_cpp_file("my_package", "test", argv[1]) << std::endl; - std::cout << parser.generate_cmake_lists("my_package", "test") << std::endl; + // std::cout << parser.generate_cmake_lists("my_package", "test") << std::endl; + std::cout << parser.generate_package_xml("my_package", "Guilherme Affonso") << std::endl; // std::cout << "Actions:" << std::endl; // std::cout << parser.test_all_actions() << std::endl; From 8db457d62c3d013414256315fcc6364d1e926e73 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 16:44:03 +0900 Subject: [PATCH 029/176] (roseus_bt) Change action/service file generator --- roseus_bt/include/roseus_bt/xml_parser.h | 32 +++++++++++++----------- roseus_bt/src/test_parse.cpp | 21 +++++++++++----- 2 files changed, 32 insertions(+), 21 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index e2c663b74..87fc1c169 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -1,6 +1,7 @@ #ifndef BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ #define BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ +#include #include #include #include @@ -37,8 +38,8 @@ class XMLParser public: - std::string test_all_actions(); - std::string test_all_conditions(); + std::map generate_all_action_files(); + std::map generate_all_service_files(); std::string generate_cpp_file(const char* package_name, const char* roscpp_node_name, const char* xml_filename); @@ -404,33 +405,34 @@ int main(int argc, char **argv) } -std::string XMLParser::test_all_actions() { - std::string result; +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.append(action_node->Attribute("ID")); - result.append(":\n"); - result.append(generate_action_file_contents(action_node)); - result.append("\n\n"); + result[format_filename(action_node)] = generate_action_file_contents(action_node); } return result; } -std::string XMLParser::test_all_conditions() { - std::string result; - const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); +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.append(condition_node->Attribute("ID")); - result.append(":\n"); - result.append(generate_service_file_contents(condition_node)); - result.append("\n\n"); + result[format_filename(condition_node)] = generate_service_file_contents(condition_node); } return result; } diff --git a/roseus_bt/src/test_parse.cpp b/roseus_bt/src/test_parse.cpp index b2dd19318..86d72b30d 100644 --- a/roseus_bt/src/test_parse.cpp +++ b/roseus_bt/src/test_parse.cpp @@ -1,20 +1,29 @@ #include #include - int main(int argc, char **argv) { RoseusBT::XMLParser parser(argv[1]); // std::cout << parser.generate_cpp_file("my_package", "test", argv[1]) << std::endl; // std::cout << parser.generate_cmake_lists("my_package", "test") << std::endl; - std::cout << parser.generate_package_xml("my_package", "Guilherme Affonso") << std::endl; + // std::cout << parser.generate_package_xml("my_package", "Guilherme Affonso") << std::endl; + + std::map action_files, service_files; + std::map::iterator it; + + action_files = parser.generate_all_action_files(); + service_files = parser.generate_all_service_files(); - // std::cout << "Actions:" << std::endl; - // std::cout << parser.test_all_actions() << std::endl; + for (it=action_files.begin(); it!=action_files.end(); ++it) { + std::cout << it->first << std::endl; + std::cout << it->second << std::endl << std::endl; + } - // std::cout << "Conditions:" << std::endl; - // std::cout << parser.test_all_conditions() << std::endl; + for (it=service_files.begin(); it!=service_files.end(); ++it) { + std::cout << it->first << std::endl; + std::cout << it->second << std::endl << std::endl; + } return 0; } From 170e17086e6c8142ca67453e22ef9502f6b0b82b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 17:02:44 +0900 Subject: [PATCH 030/176] (roseus_bt) Keep xml_filename in class variable --- roseus_bt/include/roseus_bt/xml_parser.h | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 87fc1c169..822ee804a 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -19,8 +19,8 @@ class XMLParser public: - XMLParser(const std::string &filename) { - doc.LoadFile(filename.c_str()); + XMLParser(const char* filename) : xml_filename(filename) { + doc.LoadFile(filename); } ~XMLParser() {}; @@ -28,21 +28,21 @@ class XMLParser protected: XMLDocument doc; + const char* xml_filename; std::string port_node_to_message_description(const XMLElement* port_node); std::string generate_action_file_contents(const XMLElement* node); std::string generate_service_file_contents(const XMLElement* node); std::string generate_headers(const char* package_name); std::string generate_action_class(const XMLElement* node, const char* package_name); std::string generate_condition_class(const XMLElement* node, const char* package_name); - std::string generate_main_function(const char* roscpp_node_name, const char* xml_filename); + std::string generate_main_function(const char* roscpp_node_name); public: std::map generate_all_action_files(); std::map generate_all_service_files(); std::string generate_cpp_file(const char* package_name, - const char* roscpp_node_name, - const char* xml_filename); + const char* roscpp_node_name); std::string generate_cmake_lists(const char* package_name, const char* target_filename); std::string generate_package_xml(const char* package_name, @@ -329,9 +329,7 @@ class %2%: public BT::EusConditionNode<%1%::%2%> return bfmt.str(); } -std::string XMLParser::generate_main_function(const char* roscpp_node_name, - const char* xml_filename) { - +std::string XMLParser::generate_main_function(const char* roscpp_node_name) { auto format_ros_init = [](const char* roscpp_node_name) { return fmt::format(" ros::init(argc, argv, \"{}\");", roscpp_node_name); }; @@ -438,8 +436,7 @@ std::map XMLParser::generate_all_service_files() { } std::string XMLParser::generate_cpp_file(const char* package_name, - const char* roscpp_node_name, - const char* xml_filename) { + const char* roscpp_node_name) { const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); std::string output; @@ -462,7 +459,7 @@ std::string XMLParser::generate_cpp_file(const char* package_name, output.append("\n\n"); } - output.append(generate_main_function(roscpp_node_name, xml_filename)); + output.append(generate_main_function(roscpp_node_name)); return output; } From 47f6ef0b0fbd245abfe3195d80ca877e2f8d896f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 17:48:30 +0900 Subject: [PATCH 031/176] (roseus_bt) Fix comment in xml_parser.h --- roseus_bt/include/roseus_bt/xml_parser.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 822ee804a..33630940f 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -678,6 +678,6 @@ std::string XMLParser::generate_package_xml(const char* package_name, const char return bfmt.str(); } -} // namespace BT +} // namespace RoseusBT #endif // BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ From 45e5ca623c7e911eb2b7c2b525d5b7e8cde0fb5a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 17:49:17 +0900 Subject: [PATCH 032/176] (roseus_bt) Add create_bt_package executable --- roseus_bt/CMakeLists.txt | 4 + .../include/roseus_bt/package_generator.h | 111 ++++++++++++++++++ roseus_bt/src/create_bt_package.cpp | 10 ++ 3 files changed, 125 insertions(+) create mode 100644 roseus_bt/include/roseus_bt/package_generator.h create mode 100644 roseus_bt/src/create_bt_package.cpp diff --git a/roseus_bt/CMakeLists.txt b/roseus_bt/CMakeLists.txt index d9d7e2ec0..66c027bf7 100644 --- a/roseus_bt/CMakeLists.txt +++ b/roseus_bt/CMakeLists.txt @@ -24,3 +24,7 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${TinyXML2_INCUDE_DIRS}) add_executable(test_parse src/test_parse.cpp) add_dependencies(test_parse ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(test_parse ${catkin_LIBRARIES} ${TinyXML2_LIBRARIES} fmt::fmt) + +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} fmt::fmt) 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..1a7e75de8 --- /dev/null +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -0,0 +1,111 @@ +#include +#include +#include +#include +#include +#include +#include + + +namespace RoseusBT +{ + +class PackageGenerator +{ +public: + PackageGenerator(const char* package_name, + const char* xml_filename, + const char* roscpp_node_name, + const char* target_filename, + const char* author_name) : + parser(xml_filename), + package_name(package_name), + roscpp_node_name(roscpp_node_name), + target_filename(target_filename), + author_name(author_name) {}; + + ~PackageGenerator() {}; + +private: + XMLParser parser; + const char* package_name; + const char* roscpp_node_name; + const char* target_filename; + const char* author_name; + +public: + void write_action_files(); + void write_service_files(); + void write_cpp_file(); + void write_cmake_lists(); + void write_package_xml(); + void write_all_files(); + +}; + +void PackageGenerator::write_action_files() { + std::string base_dir = fmt::format("{}/action", package_name); + boost::filesystem::create_directories(base_dir); + + std::map action_files; + std::map::iterator it; + action_files = parser.generate_all_action_files(); + + for (it=action_files.begin(); it!=action_files.end(); ++it) { + std::ofstream output_file(fmt::format("{}/{}", base_dir, it->first)); + output_file << it->second; + output_file.close(); + } +} + +void PackageGenerator::write_service_files() { + std::string base_dir = fmt::format("{}/srv", package_name); + boost::filesystem::create_directories(base_dir); + + std::map action_files; + std::map::iterator it; + action_files = parser.generate_all_service_files(); + + for (it=action_files.begin(); it!=action_files.end(); ++it) { + std::ofstream output_file(fmt::format("{}/{}", base_dir, it->first)); + output_file << it->second; + output_file.close(); + } +} + +void PackageGenerator::write_cpp_file() { + std::string base_dir = fmt::format("{}/src", package_name); + boost::filesystem::create_directories(base_dir); + + std::ofstream output_file(fmt::format("{}/{}.cpp", base_dir, target_filename)); + output_file << parser.generate_cpp_file(package_name, roscpp_node_name); + output_file.close(); +} + +void PackageGenerator::write_cmake_lists() { + std::string base_dir = package_name; + boost::filesystem::create_directories(base_dir); + + std::ofstream output_file(fmt::format("{}/CMakeLists.txt", base_dir)); + output_file << parser.generate_cmake_lists(package_name, target_filename); + output_file.close(); +} + +void PackageGenerator::write_package_xml() { + std::string base_dir = package_name; + boost::filesystem::create_directories(base_dir); + + std::ofstream output_file(fmt::format("{}/package.xml", base_dir)); + output_file << parser.generate_package_xml(package_name, author_name); + output_file.close(); +} + +void PackageGenerator::write_all_files() { + write_action_files(); + write_service_files(); + write_cpp_file(); + write_cmake_lists(); + write_package_xml(); +} + +} // namespaceRoseusBT diff --git a/roseus_bt/src/create_bt_package.cpp b/roseus_bt/src/create_bt_package.cpp new file mode 100644 index 000000000..9eec8b508 --- /dev/null +++ b/roseus_bt/src/create_bt_package.cpp @@ -0,0 +1,10 @@ +#include + +int main(int argc, char** argv) +{ + RoseusBT::PackageGenerator pg(argv[1], argv[2], "test_node", "mynode", "Guilherme Affonso"); + pg.write_all_files(); + + return 0; +} + From c650e21e32cbf16279869145ab9b12509f738114 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 20:09:16 +0900 Subject: [PATCH 033/176] (roseus_bt) Fix compilation errors --- roseus_bt/include/roseus_bt/xml_parser.h | 32 +++++++++++++----------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 33630940f..94e2124eb 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -163,17 +163,18 @@ std::string XMLParser::generate_headers(const char* package_name) { std::string output; output.append(common_headers); output.append(boost::algorithm::join(headers, "\n")); - + output.append("\n\n"); + output.append("using namespace BT;"); return output; } std::string XMLParser::generate_action_class(const XMLElement* node, const char* 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) { @@ -216,12 +217,12 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const char* std::string fmt_string = 1 + R"( -class %2%: public BT::EusActionNode<%1%::%2%Action> +class %2%: public EusActionNode<%1%::%2%Action> { public: - %2%Action(ros::NodeHandle& handle, const std::string& name, const NodeConfiguration& conf): -BT::EusActionNode<%1%::%2%Action>(handle, name, conf) {} + %2%(ros::NodeHandle& handle, const std::string& name, const NodeConfiguration& conf): +EusActionNode<%1%::%2%Action>(handle, name, conf) {} static PortsList providedPorts() { @@ -267,7 +268,7 @@ BT::EusActionNode<%1%::%2%Action>(handle, name, conf) {} std::string XMLParser::generate_condition_class(const XMLElement* node, const char* package_name) { auto format_input_port = [](const XMLElement* node) { - return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); }; auto format_get_input = [](const XMLElement* node) { @@ -287,12 +288,12 @@ std::string XMLParser::generate_condition_class(const XMLElement* node, const ch } std::string fmt_string = 1 + R"( -class %2%: public BT::EusConditionNode<%1%::%2%> +class %2%: public EusConditionNode<%1%::%2%> { public: %2%(ros::NodeHandle& handle, const std::string& node_name, const NodeConfiguration& conf): - BT::EusConditionNode<%1%::%2%>(handle, node_name, conf) {} + EusConditionNode<%1%::%2%>(handle, node_name, conf) {} static PortsList providedPorts() { @@ -337,7 +338,7 @@ std::string XMLParser::generate_main_function(const char* roscpp_node_name) { return fmt::format(" auto tree = factory.createTreeFromFile(\"{}\");", xml_filename); }; auto format_action_node = [](const XMLElement* node) { - return fmt::format(" RegisterRosAction<{0}Action>(factory, \"{0}\", nh);", + return fmt::format(" RegisterRosAction<{0}>(factory, \"{0}\", nh);", node->Attribute("ID")); }; auto format_condition_node = [](const XMLElement* node) { @@ -396,8 +397,8 @@ int main(int argc, char **argv) boost::format bfmt = boost::format(fmt_string) % format_ros_init(roscpp_node_name) % format_create_tree(xml_filename) % - boost::algorithm::join(register_actions, ",\n") % - boost::algorithm::join(register_conditions, ",\n"); + boost::algorithm::join(register_actions, "\n") % + boost::algorithm::join(register_conditions, "\n"); return bfmt.str(); } @@ -526,6 +527,7 @@ project(%1%) find_package(catkin REQUIRED COMPONENTS message_generation roscpp + behaviortree_ros roseus_bt %3% ) @@ -546,7 +548,7 @@ generate_messages( ) catkin_package( - INCLUDE_DIRS include + INCLUDE_DIRS LIBRARIES CATKIN_DEPENDS message_runtime @@ -554,7 +556,7 @@ catkin_package( ) -include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(${catkin_INCLUDE_DIRS}) add_executable(%2% src/%2%.cpp) add_dependencies(%2% ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -655,11 +657,13 @@ std::string XMLParser::generate_package_xml(const char* package_name, const char catkin message_generation roscpp + behaviortree_ros roseus_bt %4% message_runtime roscpp + behaviortree_ros roseus_bt %5% From a1636f01aff8c7127b7a0daef698f7e7258acfc3 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 18 Jun 2021 13:20:23 +0900 Subject: [PATCH 034/176] (roseus_bt) Remove test executable --- roseus_bt/CMakeLists.txt | 4 ---- roseus_bt/src/test_parse.cpp | 29 ----------------------------- 2 files changed, 33 deletions(-) delete mode 100644 roseus_bt/src/test_parse.cpp diff --git a/roseus_bt/CMakeLists.txt b/roseus_bt/CMakeLists.txt index 66c027bf7..b5a1e4389 100644 --- a/roseus_bt/CMakeLists.txt +++ b/roseus_bt/CMakeLists.txt @@ -21,10 +21,6 @@ catkin_package( include_directories( include ${catkin_INCLUDE_DIRS} ${TinyXML2_INCUDE_DIRS}) -add_executable(test_parse src/test_parse.cpp) -add_dependencies(test_parse ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(test_parse ${catkin_LIBRARIES} ${TinyXML2_LIBRARIES} fmt::fmt) - 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} fmt::fmt) diff --git a/roseus_bt/src/test_parse.cpp b/roseus_bt/src/test_parse.cpp deleted file mode 100644 index 86d72b30d..000000000 --- a/roseus_bt/src/test_parse.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include -#include - -int main(int argc, char **argv) -{ - RoseusBT::XMLParser parser(argv[1]); - - // std::cout << parser.generate_cpp_file("my_package", "test", argv[1]) << std::endl; - // std::cout << parser.generate_cmake_lists("my_package", "test") << std::endl; - // std::cout << parser.generate_package_xml("my_package", "Guilherme Affonso") << std::endl; - - std::map action_files, service_files; - std::map::iterator it; - - action_files = parser.generate_all_action_files(); - service_files = parser.generate_all_service_files(); - - for (it=action_files.begin(); it!=action_files.end(); ++it) { - std::cout << it->first << std::endl; - std::cout << it->second << std::endl << std::endl; - } - - for (it=service_files.begin(); it!=service_files.end(); ++it) { - std::cout << it->first << std::endl; - std::cout << it->second << std::endl << std::endl; - } - - return 0; -} From bdebe189c7914ba6a6e7f5864c4cedf9e888f249 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 18 Jun 2021 13:44:37 +0900 Subject: [PATCH 035/176] (roseus_bt) Print warnings on non-global lambda-closures --- roseus_bt/euslisp/nodes.l | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 3271bc189..72d29068a 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -21,6 +21,17 @@ (send fn :name)) (t nil)))) +(defun check-fn-closure (fn) + (when (and (consp fn) + (eql (car fn) 'lambda-closure) + (not (zerop (third fn)))) + (let ((name (get-fn-sym fn))) + (ros::ros-warn + (format nil "Possibly harmful lambda-closure in #'~A! Try using '~A instead." + name + name))))) + + ;; macros (defmacro define-action-callback (name arg-list &rest body) (user::with-gensyms (server goal get-result) @@ -73,7 +84,9 @@ (concatenate-string "Wrong function type detected! " "Make sure to define execution callbacks using the define-action-callback macro")) - (error type-error "action-node-callback function expected for execute-cb")) + (error "action-node-callback function expected for execute-cb")) + (check-fn-closure execute-cb) + (check-fn-closure preempt-cb) (send-super :init ns spec :execute-cb execute-cb @@ -93,7 +106,7 @@ (concatenate-string "Wrong function type detected! " "Make sure to define execution callbacks using the define-condition-callback macro")) - (error type-error "condition-node-callback function expected for execute-cb")) + (error "condition-node-callback function expected for execute-cb")) (ros::advertise-service ns spec execute-cb) (push self *condition-list*) From ee068c3cba0e71a8d0b79d48ecaf64e72455b1d7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 18 Jun 2021 15:56:38 +0900 Subject: [PATCH 036/176] (roseus_bt) Copy model file and use absolute path --- .../include/roseus_bt/package_generator.h | 21 +++++++++++++++- roseus_bt/include/roseus_bt/xml_parser.h | 25 ++++++++++--------- 2 files changed, 33 insertions(+), 13 deletions(-) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 1a7e75de8..6234f2da9 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -19,6 +19,7 @@ class PackageGenerator const char* target_filename, const char* author_name) : parser(xml_filename), + xml_filename(xml_filename), package_name(package_name), roscpp_node_name(roscpp_node_name), target_filename(target_filename), @@ -29,11 +30,13 @@ class PackageGenerator private: XMLParser parser; const char* package_name; + std::string xml_filename; const char* roscpp_node_name; const char* target_filename; const char* author_name; public: + void copy_xml_file(); void write_action_files(); void write_service_files(); void write_cpp_file(); @@ -43,6 +46,19 @@ class PackageGenerator }; +void PackageGenerator::copy_xml_file() { + 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()); + + 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; +} + void PackageGenerator::write_action_files() { std::string base_dir = fmt::format("{}/action", package_name); boost::filesystem::create_directories(base_dir); @@ -78,7 +94,9 @@ void PackageGenerator::write_cpp_file() { boost::filesystem::create_directories(base_dir); std::ofstream output_file(fmt::format("{}/{}.cpp", base_dir, target_filename)); - output_file << parser.generate_cpp_file(package_name, roscpp_node_name); + + output_file << parser.generate_cpp_file(package_name, roscpp_node_name, + boost::filesystem::absolute(xml_filename).c_str()); output_file.close(); } @@ -101,6 +119,7 @@ void PackageGenerator::write_package_xml() { } void PackageGenerator::write_all_files() { + copy_xml_file(); write_action_files(); write_service_files(); write_cpp_file(); diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 94e2124eb..821569321 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -19,7 +19,7 @@ class XMLParser public: - XMLParser(const char* filename) : xml_filename(filename) { + XMLParser(const char* filename) { doc.LoadFile(filename); } @@ -28,21 +28,21 @@ class XMLParser protected: XMLDocument doc; - const char* xml_filename; std::string port_node_to_message_description(const XMLElement* port_node); std::string generate_action_file_contents(const XMLElement* node); std::string generate_service_file_contents(const XMLElement* node); std::string generate_headers(const char* package_name); std::string generate_action_class(const XMLElement* node, const char* package_name); std::string generate_condition_class(const XMLElement* node, const char* package_name); - std::string generate_main_function(const char* roscpp_node_name); + std::string generate_main_function(const char* roscpp_node_name, const char* xml_filename); public: std::map generate_all_action_files(); std::map generate_all_service_files(); std::string generate_cpp_file(const char* package_name, - const char* roscpp_node_name); + const char* roscpp_node_name, + const char* xml_filename); std::string generate_cmake_lists(const char* package_name, const char* target_filename); std::string generate_package_xml(const char* package_name, @@ -330,11 +330,12 @@ class %2%: public EusConditionNode<%1%::%2%> return bfmt.str(); } -std::string XMLParser::generate_main_function(const char* roscpp_node_name) { - auto format_ros_init = [](const char* roscpp_node_name) { +std::string XMLParser::generate_main_function(const char* roscpp_node_name, + const char* xml_filename) { + auto format_ros_init = [roscpp_node_name]() { return fmt::format(" ros::init(argc, argv, \"{}\");", roscpp_node_name); }; - auto format_create_tree = [](const char* xml_filename) { + auto format_create_tree = [xml_filename]() { return fmt::format(" auto tree = factory.createTreeFromFile(\"{}\");", xml_filename); }; auto format_action_node = [](const XMLElement* node) { @@ -395,8 +396,8 @@ int main(int argc, char **argv) )"; boost::format bfmt = boost::format(fmt_string) % - format_ros_init(roscpp_node_name) % - format_create_tree(xml_filename) % + format_ros_init() % + format_create_tree() % boost::algorithm::join(register_actions, "\n") % boost::algorithm::join(register_conditions, "\n"); @@ -437,8 +438,8 @@ std::map XMLParser::generate_all_service_files() { } std::string XMLParser::generate_cpp_file(const char* package_name, - const char* roscpp_node_name) { - + const char* roscpp_node_name, + const char* xml_filename) { const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); std::string output; output.append(generate_headers(package_name)); @@ -460,7 +461,7 @@ std::string XMLParser::generate_cpp_file(const char* package_name, output.append("\n\n"); } - output.append(generate_main_function(roscpp_node_name)); + output.append(generate_main_function(roscpp_node_name, xml_filename)); return output; } From 5dfda3f63182bd5fbe62e5ec8307761191eb56b1 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 11:11:26 +0900 Subject: [PATCH 037/176] (roseus_bt) Add roseus_bt:spin --- roseus_bt/euslisp/nodes.l | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 72d29068a..31ece86dd 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -4,7 +4,8 @@ (in-package "ROSEUS_BT") (export '(define-action-callback define-condition-callback - action-node condition-node spin-once set-output ok)) + action-node condition-node spin-once spin + set-output ok)) (defvar *action-list*) (defvar *condition-list*) @@ -121,6 +122,11 @@ (dolist (ac *action-list*) (send ac :worker))) +(defun spin () + (while t + (spin-once) + (ros::sleep))) + (defun set-output (name val) (error "not defined in global scope")) From e7e1b55c295b00ff69dc00b2ab65327fefe6cc17 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 11:12:37 +0900 Subject: [PATCH 038/176] (roseus_bt) Add generate_eus_action_server --- .../include/roseus_bt/package_generator.h | 12 ++ roseus_bt/include/roseus_bt/xml_parser.h | 114 ++++++++++++++++++ 2 files changed, 126 insertions(+) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 6234f2da9..fe04444a4 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -40,6 +40,7 @@ class PackageGenerator void write_action_files(); void write_service_files(); void write_cpp_file(); + void write_eus_action_server(); void write_cmake_lists(); void write_package_xml(); void write_all_files(); @@ -100,6 +101,16 @@ void PackageGenerator::write_cpp_file() { output_file.close(); } +void PackageGenerator::write_eus_action_server() { + std::string base_dir = fmt::format("{}/euslisp", package_name); + boost::filesystem::create_directories(base_dir); + + std::ofstream output_file(fmt::format("{}/action-server.l", base_dir)); + + output_file << parser.generate_eus_action_server(package_name); + output_file.close(); +} + void PackageGenerator::write_cmake_lists() { std::string base_dir = package_name; boost::filesystem::create_directories(base_dir); @@ -123,6 +134,7 @@ void PackageGenerator::write_all_files() { write_action_files(); write_service_files(); write_cpp_file(); + write_eus_action_server(); write_cmake_lists(); write_package_xml(); } diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 821569321..ad9a2f462 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -28,6 +28,8 @@ class XMLParser protected: XMLDocument doc; + void collect_node_attribute(const XMLElement* node, const XMLElement* ref_node, + const char* attribute, std::vector* node_attributes); std::string port_node_to_message_description(const XMLElement* port_node); std::string generate_action_file_contents(const XMLElement* node); std::string generate_service_file_contents(const XMLElement* node); @@ -43,6 +45,7 @@ class XMLParser std::string generate_cpp_file(const char* package_name, const char* roscpp_node_name, const char* xml_filename); + std::string generate_eus_action_server(const char* action_name); std::string generate_cmake_lists(const char* package_name, const char* target_filename); std::string generate_package_xml(const char* package_name, @@ -51,6 +54,29 @@ class XMLParser }; +void XMLParser::collect_node_attribute(const XMLElement* node, + const XMLElement* ref_node, + const char* attribute, + std::vector* node_attributes) { + + if (!std::strcmp(node->Name(), ref_node->Name()) && + !std::strcmp(node->Attribute("ID"), ref_node->Attribute("ID"))) { + if (node->Attribute(attribute) && + std::find(node_attributes->begin(), node_attributes->end(), + node->Attribute(attribute)) == node_attributes->end()) { + node_attributes->push_back(node->Attribute(attribute)); + } + return; + } + + for (auto child_node = node->FirstChildElement(); + child_node != nullptr; + child_node = child_node->NextSiblingElement()) + { + collect_node_attribute(child_node, ref_node, attribute, node_attributes); + } +} + std::string XMLParser::port_node_to_message_description(const XMLElement* port_node) { if (!port_node->Attribute("type") || !port_node->Attribute("name")) { @@ -404,6 +430,94 @@ int main(int argc, char **argv) return bfmt.str(); } +std::string XMLParser::generate_eus_action_server(const char* package_name) { + auto format_callback = [](const XMLElement* node, const char* suffix) { + std::string fmt_string = 1 + R"( +(roseus_bt:define-action-callback {0}-execute-cb{1} ({2}) + ;; do something + t) +)"; + std::vector param_list; + 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") { + param_list.push_back(port_node->Attribute("name")); + } + } + + return fmt::format(fmt_string, + node->Attribute("ID"), + suffix, + boost::algorithm::join(param_list, " ")); + }; + + auto format_instance = [package_name](const XMLElement* node, const char* suffix, + std::string server_name) { + std::string fmt_string = 1 + R"( +(instance roseus_bt:action-node :init + "{3}" {0}::{1}Action + :execute-cb '{1}-execute-cb{2}) +)"; + return fmt::format(fmt_string, + package_name, + node->Attribute("ID"), + suffix, + server_name); + }; + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); + std::vector callback_definition; + std::vector instance_creation; + + callback_definition.push_back(std::string(";; define action callbacks")); + instance_creation.push_back(std::string(";; create server instances")); + + for (auto action_node = root->FirstChildElement("Action"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("Action")) + { + std::vector server_names; + collect_node_attribute(bt_root, action_node, "server_name", &server_names); + + if (server_names.empty()) { + callback_definition.push_back(format_callback(action_node, "")); + instance_creation.push_back(format_instance(action_node, "", + action_node->Attribute("ID"))); + } + else if (server_names.size() == 1) { + callback_definition.push_back(format_callback(action_node, "")); + instance_creation.push_back(format_instance(action_node, "", + server_names.at(0).c_str())); + } + else { + int suffix_num = 1; + for (std::vector::const_iterator it = server_names.begin(); + it != server_names.end(); ++it, suffix_num++) { + std::string suffix = fmt::format("-{}", suffix_num); + callback_definition.push_back(format_callback(action_node, suffix.c_str())); + instance_creation.push_back(format_instance(action_node, suffix.c_str(), *it)); + } + } + } + + std::string output; + output.append("(ros::roseus \"action_server\")\n"); + output.append(fmt::format("(ros::load-ros-package \"{}\")\n", package_name)); + output.append("(load \"package://roseus_bt/euslisp/nodes.l\")\n"); + output.append("\n\n"); + output.append(boost::algorithm::join(callback_definition, "\n")); + output.append("\n\n"); + output.append(boost::algorithm::join(instance_creation, "\n")); + output.append("\n\n"); + output.append(";; spin\n"); + output.append("(roseus_bt:spin)"); + + return output; +} std::map XMLParser::generate_all_action_files() { auto format_filename = [](const XMLElement* node) { From c19a0d7436d5c6bc77389fd8d238df2c47eeb00e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 13:16:53 +0900 Subject: [PATCH 039/176] (roseus_bt) Add generate_eus_condition_server --- .../include/roseus_bt/package_generator.h | 12 +++ roseus_bt/include/roseus_bt/xml_parser.h | 89 +++++++++++++++++++ 2 files changed, 101 insertions(+) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index fe04444a4..12f7e3731 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -41,6 +41,7 @@ class PackageGenerator void write_service_files(); void write_cpp_file(); void write_eus_action_server(); + void write_eus_condition_server(); void write_cmake_lists(); void write_package_xml(); void write_all_files(); @@ -111,6 +112,16 @@ void PackageGenerator::write_eus_action_server() { output_file.close(); } +void PackageGenerator::write_eus_condition_server() { + std::string base_dir = fmt::format("{}/euslisp", package_name); + boost::filesystem::create_directories(base_dir); + + std::ofstream output_file(fmt::format("{}/condition-server.l", base_dir)); + + output_file << parser.generate_eus_condition_server(package_name); + output_file.close(); +} + void PackageGenerator::write_cmake_lists() { std::string base_dir = package_name; boost::filesystem::create_directories(base_dir); @@ -135,6 +146,7 @@ void PackageGenerator::write_all_files() { write_service_files(); write_cpp_file(); write_eus_action_server(); + write_eus_condition_server(); write_cmake_lists(); write_package_xml(); } diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index ad9a2f462..06b5b7a85 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -46,6 +46,7 @@ class XMLParser const char* roscpp_node_name, const char* xml_filename); std::string generate_eus_action_server(const char* action_name); + std::string generate_eus_condition_server(const char* action_name); std::string generate_cmake_lists(const char* package_name, const char* target_filename); std::string generate_package_xml(const char* package_name, @@ -519,6 +520,94 @@ std::string XMLParser::generate_eus_action_server(const char* package_name) { return output; } +std::string XMLParser::generate_eus_condition_server(const char* package_name) { + auto format_callback = [](const XMLElement* node, const char* suffix) { + std::string fmt_string = 1 + R"( +(roseus_bt:define-condition-callback {0}-cb{1} ({2}) + ;; do something + t) +)"; + std::vector param_list; + 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") { + param_list.push_back(port_node->Attribute("name")); + } + } + + return fmt::format(fmt_string, + node->Attribute("ID"), + suffix, + boost::algorithm::join(param_list, " ")); + }; + + auto format_instance = [package_name](const XMLElement* node, const char* suffix, + std::string service_name) { + std::string fmt_string = 1 + R"( +(instance roseus_bt:condition-node :init + "{3}" {0}::{1} #'{1}-execute-cb{2}) +)"; + return fmt::format(fmt_string, + package_name, + node->Attribute("ID"), + suffix, + service_name); + }; + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); + std::vector callback_definition; + std::vector instance_creation; + + callback_definition.push_back(std::string(";; define condition callbacks")); + instance_creation.push_back(std::string(";; create server instances")); + + for (auto condition_node = root->FirstChildElement("Condition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("Condition")) + { + std::vector server_names; + collect_node_attribute(bt_root, condition_node, "service_name", &server_names); + + if (server_names.empty()) { + callback_definition.push_back(format_callback(condition_node, "")); + instance_creation.push_back(format_instance(condition_node, "", + condition_node->Attribute("ID"))); + } + else if (server_names.size() == 1) { + callback_definition.push_back(format_callback(condition_node, "")); + instance_creation.push_back(format_instance(condition_node, "", + server_names.at(0).c_str())); + } + else { + int suffix_num = 1; + for (std::vector::const_iterator it = server_names.begin(); + it != server_names.end(); ++it, suffix_num++) { + std::string suffix = fmt::format("-{}", suffix_num); + callback_definition.push_back(format_callback(condition_node, suffix.c_str())); + instance_creation.push_back(format_instance(condition_node, suffix.c_str(), *it)); + } + } + } + + std::string output; + output.append("(ros::roseus \"condition_server\")\n"); + output.append(fmt::format("(ros::load-ros-package \"{}\")\n", package_name)); + output.append("(load \"package://roseus_bt/euslisp/nodes.l\")\n"); + output.append("\n\n"); + output.append(boost::algorithm::join(callback_definition, "\n")); + output.append("\n\n"); + output.append(boost::algorithm::join(instance_creation, "\n")); + output.append("\n\n"); + output.append(";; spin\n"); + output.append("(roseus_bt:spin)"); + + return output; +} + std::map XMLParser::generate_all_action_files() { auto format_filename = [](const XMLElement* node) { return fmt::format("{}.action", node->Attribute("ID")); From 8d0fbdff3197323e790d057dda7bfdae48dd307f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 14:37:40 +0900 Subject: [PATCH 040/176] (roseus_bt) Add separate template generator class --- roseus_bt/include/roseus_bt/gen_template.h | 423 +++++++++++++++++++++ roseus_bt/include/roseus_bt/xml_parser.h | 336 ++-------------- 2 files changed, 446 insertions(+), 313 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/gen_template.h 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..4119b2d41 --- /dev/null +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -0,0 +1,423 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_GEN_TEMPLATE_ +#define BEHAVIOR_TREE_ROSEUS_BT_GEN_TEMPLATE_ + +#include +#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 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 condition_class_template(std::string package_name, std::string nodeID, + std::vector provided_ports, + std::vector get_inputs); + std::string main_function_template(std::string roscpp_node_name, + std::string xml_filename, + std::vector register_actions, + std::vector register_conditions); + std::string eus_server_template(std::string server_type, + std::string package_name, + std::vector callbacks, + std::vector instances); + std::string cmake_lists_template(std::string package_name, std::string target_name, + std::vector message_packages, + std::vector service_files, + std::vector action_files); + std::string package_xml_template(std::string package_name, + std::string author_name, + std::vector build_dependencies, + std::vector exec_dependencies); +}; + + +std::string GenTemplate::action_file_template(std::vector goal, + std::vector feedback) { + std::string fmt_string = 1 + R"( +%1% +--- +bool success +--- +%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::headers_template(std::vector headers) { + std::string fmt_string = 1 + R"( +#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::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& node_name, const NodeConfiguration& conf): + EusConditionNode<%1%::%2%>(handle, node_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::main_function_template(std::string roscpp_node_name, + std::string xml_filename, + std::vector register_actions, + std::vector register_conditions) { + 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) +{ +%1% + ros::NodeHandle nh; + + BehaviorTreeFactory factory; + +%3% +%4% + +%2% + + StdCoutLogger logger_cout(tree); + 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.05); + sleep_time.sleep(); + } + + return 0; +} +)"; + + boost::format bfmt = boost::format(fmt_string) % + format_ros_init() % + format_create_tree() % + boost::algorithm::join(register_actions, "\n") % + boost::algorithm::join(register_conditions, "\n"); + + return bfmt.str(); +} + + +std::string GenTemplate::eus_server_template(std::string server_type, + std::string package_name, + std::vector callbacks, + std::vector instances) { + 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 \"{}\")\n", package_name); + }; + + std::string fmt_string = 1 + R"( +%2% +%3% +%4% + +;; define %1% callbacks +%5% + +;; create server instances +%6% + +;; spin +(roseus_bt::spin) +)"; + + boost::format bfmt = boost::format(fmt_string) % + server_type % + format_ros_roseus() % + format_load_ros_package() % + "(load \"package://roseus_bt/euslisp/nodes.l\")" % + boost::algorithm::join(callbacks, "\n") % + boost::algorithm::join(instances, "\n"); + + return bfmt.str(); +} + + +std::string GenTemplate::cmake_lists_template(std::string package_name, std::string target_name, + std::vector message_packages, + std::vector service_files, + std::vector action_files) { + std::string fmt_string = 1+ R"( +cmake_minimum_required(VERSION 3.0.2) +project(%1%) + +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + behaviortree_ros + roseus_bt +%3% +) + +add_service_files( + FILES +%4% +) + +add_action_files( + FILES +%5% +) + +generate_messages( + DEPENDENCIES +%3% +) + +catkin_package( + INCLUDE_DIRS + LIBRARIES + CATKIN_DEPENDS + message_runtime +%3% +) + + +include_directories(${catkin_INCLUDE_DIRS}) + +add_executable(%2% src/%2%.cpp) +add_dependencies(%2% ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(%2% ${catkin_LIBRARIES}) +)"; + + boost::format bfmt = boost::format(fmt_string) % + package_name % + target_name % + boost::algorithm::join(message_packages, "\n") % + boost::algorithm::join(service_files, "\n") % + boost::algorithm::join(action_files, "\n"); + + return bfmt.str(); +} + + +std::string GenTemplate::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(); +} + + +} // namespace RoseusBT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_GEN_TEMPLATE_ diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 06b5b7a85..274cd6f7b 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -1,13 +1,8 @@ #ifndef BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ #define BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ -#include -#include -#include #include -#include -#include -#include +#include "gen_template.h" namespace RoseusBT { @@ -19,7 +14,7 @@ class XMLParser public: - XMLParser(const char* filename) { + XMLParser(const char* filename) : gen_template() { doc.LoadFile(filename); } @@ -28,6 +23,7 @@ class XMLParser protected: XMLDocument doc; + GenTemplate gen_template; void collect_node_attribute(const XMLElement* node, const XMLElement* ref_node, const char* attribute, std::vector* node_attributes); std::string port_node_to_message_description(const XMLElement* port_node); @@ -110,14 +106,7 @@ std::string XMLParser::generate_action_file_contents(const XMLElement* node) { } } - std::string output; - output.append(boost::algorithm::join(goal, "\n")); - output.append("\n---\n"); - output.append("bool success"); - output.append("\n---\n"); - output.append(boost::algorithm::join(feedback, "\n")); - - return output; + return gen_template.action_file_template(goal, feedback); } std::string XMLParser::generate_service_file_contents(const XMLElement* node) { @@ -138,12 +127,7 @@ std::string XMLParser::generate_service_file_contents(const XMLElement* node) { } } - std::string output; - output.append(boost::algorithm::join(request, "\n")); - output.append("\n---\n"); - output.append("bool success"); - - return output; + return gen_template.service_file_template(request); } std::string XMLParser::generate_headers(const char* package_name) { @@ -162,17 +146,6 @@ std::string XMLParser::generate_headers(const char* package_name) { const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); std::vector headers; - std::string common_headers = 1 + R"( -#include - -#include -#include - -#include -#include - -)"; - for (auto action_node = root->FirstChildElement("Action"); action_node != nullptr; action_node = action_node->NextSiblingElement("Action")) @@ -187,12 +160,7 @@ std::string XMLParser::generate_headers(const char* package_name) { headers.push_back(format_condition_node(condition_node, package_name)); } - std::string output; - output.append(common_headers); - output.append(boost::algorithm::join(headers, "\n")); - output.append("\n\n"); - output.append("using namespace BT;"); - return output; + return gen_template.headers_template(headers); } std::string XMLParser::generate_action_class(const XMLElement* node, const char* package_name) { @@ -243,54 +211,8 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const char* provided_output_ports.end()); - 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 % - node->Attribute("ID") % - boost::algorithm::join(provided_ports, ",\n") % - boost::algorithm::join(get_inputs, "\n") % - boost::algorithm::join(set_outputs, "\n"); - - return bfmt.str(); + return gen_template.action_class_template(package_name, node->Attribute("ID"), + provided_ports, get_inputs, set_outputs); } std::string XMLParser::generate_condition_class(const XMLElement* node, const char* package_name) { @@ -314,57 +236,12 @@ std::string XMLParser::generate_condition_class(const XMLElement* node, const ch get_inputs.push_back(format_get_input(port_node)); } - std::string fmt_string = 1 + R"( -class %2%: public EusConditionNode<%1%::%2%> -{ - -public: - %2%(ros::NodeHandle& handle, const std::string& node_name, const NodeConfiguration& conf): - EusConditionNode<%1%::%2%>(handle, node_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 % - node->Attribute("ID") % - boost::algorithm::join(provided_ports, ",\n") % - boost::algorithm::join(get_inputs, "\n"); - - return bfmt.str(); + return gen_template.condition_class_template(package_name, node->Attribute("ID"), + provided_ports, get_inputs); } std::string XMLParser::generate_main_function(const char* roscpp_node_name, const char* xml_filename) { - 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); - }; auto format_action_node = [](const XMLElement* node) { return fmt::format(" RegisterRosAction<{0}>(factory, \"{0}\", nh);", node->Attribute("ID")); @@ -392,43 +269,8 @@ std::string XMLParser::generate_main_function(const char* roscpp_node_name, register_conditions.push_back(format_condition_node(condition_node)); } - std::string fmt_string = 1 + R"( -int main(int argc, char **argv) -{ -%1% - ros::NodeHandle nh; - - BehaviorTreeFactory factory; - -%3% -%4% - -%2% - - StdCoutLogger logger_cout(tree); - 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.05); - sleep_time.sleep(); - } - - return 0; -} -)"; - - boost::format bfmt = boost::format(fmt_string) % - format_ros_init() % - format_create_tree() % - boost::algorithm::join(register_actions, "\n") % - boost::algorithm::join(register_conditions, "\n"); - - return bfmt.str(); + return gen_template.main_function_template(roscpp_node_name, xml_filename, + register_actions, register_conditions); } std::string XMLParser::generate_eus_action_server(const char* package_name) { @@ -474,9 +316,6 @@ std::string XMLParser::generate_eus_action_server(const char* package_name) { std::vector callback_definition; std::vector instance_creation; - callback_definition.push_back(std::string(";; define action callbacks")); - instance_creation.push_back(std::string(";; create server instances")); - for (auto action_node = root->FirstChildElement("Action"); action_node != nullptr; action_node = action_node->NextSiblingElement("Action")) @@ -505,19 +344,8 @@ std::string XMLParser::generate_eus_action_server(const char* package_name) { } } - std::string output; - output.append("(ros::roseus \"action_server\")\n"); - output.append(fmt::format("(ros::load-ros-package \"{}\")\n", package_name)); - output.append("(load \"package://roseus_bt/euslisp/nodes.l\")\n"); - output.append("\n\n"); - output.append(boost::algorithm::join(callback_definition, "\n")); - output.append("\n\n"); - output.append(boost::algorithm::join(instance_creation, "\n")); - output.append("\n\n"); - output.append(";; spin\n"); - output.append("(roseus_bt:spin)"); - - return output; + return gen_template.eus_server_template("action", package_name, + callback_definition, instance_creation); } std::string XMLParser::generate_eus_condition_server(const char* package_name) { @@ -562,9 +390,6 @@ std::string XMLParser::generate_eus_condition_server(const char* package_name) { std::vector callback_definition; std::vector instance_creation; - callback_definition.push_back(std::string(";; define condition callbacks")); - instance_creation.push_back(std::string(";; create server instances")); - for (auto condition_node = root->FirstChildElement("Condition"); condition_node != nullptr; condition_node = condition_node->NextSiblingElement("Condition")) @@ -593,19 +418,8 @@ std::string XMLParser::generate_eus_condition_server(const char* package_name) { } } - std::string output; - output.append("(ros::roseus \"condition_server\")\n"); - output.append(fmt::format("(ros::load-ros-package \"{}\")\n", package_name)); - output.append("(load \"package://roseus_bt/euslisp/nodes.l\")\n"); - output.append("\n\n"); - output.append(boost::algorithm::join(callback_definition, "\n")); - output.append("\n\n"); - output.append(boost::algorithm::join(instance_creation, "\n")); - output.append("\n\n"); - output.append(";; spin\n"); - output.append("(roseus_bt:spin)"); - - return output; + return gen_template.eus_server_template("condition", package_name, + callback_definition, instance_creation); } std::map XMLParser::generate_all_action_files() { @@ -724,57 +538,10 @@ std::string XMLParser::generate_cmake_lists(const char* package_name, const char } } - std::string fmt_string = 1+ R"( -cmake_minimum_required(VERSION 3.0.2) -project(%1%) - -find_package(catkin REQUIRED COMPONENTS - message_generation - roscpp - behaviortree_ros - roseus_bt -%3% -) - -add_service_files( - FILES -%4% -) - -add_action_files( - FILES -%5% -) - -generate_messages( - DEPENDENCIES -%3% -) - -catkin_package( - INCLUDE_DIRS - LIBRARIES - CATKIN_DEPENDS - message_runtime -%3% -) - - -include_directories(${catkin_INCLUDE_DIRS}) - -add_executable(%2% src/%2%.cpp) -add_dependencies(%2% ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(%2% ${catkin_LIBRARIES}) -)"; - - boost::format bfmt = boost::format(fmt_string) % - package_name % - target_name % - boost::algorithm::join(message_packages, "\n") % - boost::algorithm::join(service_files, "\n") % - boost::algorithm::join(action_files, "\n"); - - return bfmt.str(); + return gen_template.cmake_lists_template(package_name, target_name, + message_packages, + service_files, + action_files); } std::string XMLParser::generate_package_xml(const char* package_name, const char* author_name) { @@ -796,11 +563,6 @@ std::string XMLParser::generate_package_xml(const char* package_name, const char } }; - 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(), ' ', '_'); - const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); std::vector message_packages; @@ -829,61 +591,9 @@ std::string XMLParser::generate_package_xml(const char* package_name, const char std::transform(message_packages.begin(), message_packages.end(), exec_dependencies.begin(), format_exec_depend); - 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(); + return gen_template.package_xml_template(package_name, author_name, + build_dependencies, + exec_dependencies); } } // namespace RoseusBT From f31e111e74bdb0bb2032f7308b2edc5fbdbbdc06 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 14:44:08 +0900 Subject: [PATCH 041/176] (roseus_bt) Replace const char* with std::string --- roseus_bt/include/roseus_bt/xml_parser.h | 82 ++++++++++++------------ 1 file changed, 41 insertions(+), 41 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 274cd6f7b..db5eb1579 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -14,8 +14,8 @@ class XMLParser public: - XMLParser(const char* filename) : gen_template() { - doc.LoadFile(filename); + XMLParser(std::string filename) : gen_template() { + doc.LoadFile(filename.c_str()); } ~XMLParser() {}; @@ -29,24 +29,24 @@ class XMLParser std::string port_node_to_message_description(const XMLElement* port_node); std::string generate_action_file_contents(const XMLElement* node); std::string generate_service_file_contents(const XMLElement* node); - std::string generate_headers(const char* package_name); - std::string generate_action_class(const XMLElement* node, const char* package_name); - std::string generate_condition_class(const XMLElement* node, const char* package_name); - std::string generate_main_function(const char* roscpp_node_name, const char* xml_filename); + std::string generate_headers(std::string package_name); + std::string generate_action_class(const XMLElement* node, std::string package_name); + std::string generate_condition_class(const XMLElement* node, std::string package_name); + std::string generate_main_function(std::string roscpp_node_name, std::string xml_filename); public: std::map generate_all_action_files(); std::map generate_all_service_files(); - std::string generate_cpp_file(const char* package_name, - const char* roscpp_node_name, - const char* xml_filename); - std::string generate_eus_action_server(const char* action_name); - std::string generate_eus_condition_server(const char* action_name); - std::string generate_cmake_lists(const char* package_name, - const char* target_filename); - std::string generate_package_xml(const char* package_name, - const char* author_name); + std::string generate_cpp_file(std::string package_name, + std::string roscpp_node_name, + std::string xml_filename); + std::string generate_eus_action_server(std::string action_name); + std::string generate_eus_condition_server(std::string action_name); + std::string generate_cmake_lists(std::string package_name, + std::string target_filename); + std::string generate_package_xml(std::string package_name, + std::string author_name); }; @@ -130,14 +130,14 @@ std::string XMLParser::generate_service_file_contents(const XMLElement* node) { return gen_template.service_file_template(request); } -std::string XMLParser::generate_headers(const char* package_name) { - auto format_action_node = [](const XMLElement* node, const char* package_name) { +std::string XMLParser::generate_headers(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, const char* package_name) { + auto format_condition_node = [](const XMLElement* node, std::string package_name) { return fmt::format("#include <{}/{}.h>", package_name, node->Attribute("ID")); @@ -163,7 +163,7 @@ std::string XMLParser::generate_headers(const char* package_name) { return gen_template.headers_template(headers); } -std::string XMLParser::generate_action_class(const XMLElement* node, const char* package_name) { +std::string XMLParser::generate_action_class(const XMLElement* node, std::string package_name) { auto format_input_port = [](const XMLElement* node) { return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); @@ -215,7 +215,7 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const char* provided_ports, get_inputs, set_outputs); } -std::string XMLParser::generate_condition_class(const XMLElement* node, const char* package_name) { +std::string XMLParser::generate_condition_class(const XMLElement* node, std::string package_name) { auto format_input_port = [](const XMLElement* node) { return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); @@ -240,8 +240,8 @@ std::string XMLParser::generate_condition_class(const XMLElement* node, const ch provided_ports, get_inputs); } -std::string XMLParser::generate_main_function(const char* roscpp_node_name, - const char* xml_filename) { +std::string XMLParser::generate_main_function(std::string roscpp_node_name, + std::string xml_filename) { auto format_action_node = [](const XMLElement* node) { return fmt::format(" RegisterRosAction<{0}>(factory, \"{0}\", nh);", node->Attribute("ID")); @@ -273,8 +273,8 @@ std::string XMLParser::generate_main_function(const char* roscpp_node_name, register_actions, register_conditions); } -std::string XMLParser::generate_eus_action_server(const char* package_name) { - auto format_callback = [](const XMLElement* node, const char* suffix) { +std::string XMLParser::generate_eus_action_server(std::string package_name) { + auto format_callback = [](const XMLElement* node, std::string suffix) { std::string fmt_string = 1 + R"( (roseus_bt:define-action-callback {0}-execute-cb{1} ({2}) ;; do something @@ -297,7 +297,7 @@ std::string XMLParser::generate_eus_action_server(const char* package_name) { boost::algorithm::join(param_list, " ")); }; - auto format_instance = [package_name](const XMLElement* node, const char* suffix, + auto format_instance = [package_name](const XMLElement* node, std::string suffix, std::string server_name) { std::string fmt_string = 1 + R"( (instance roseus_bt:action-node :init @@ -331,15 +331,15 @@ std::string XMLParser::generate_eus_action_server(const char* package_name) { else if (server_names.size() == 1) { callback_definition.push_back(format_callback(action_node, "")); instance_creation.push_back(format_instance(action_node, "", - server_names.at(0).c_str())); + server_names.at(0))); } else { int suffix_num = 1; for (std::vector::const_iterator it = server_names.begin(); it != server_names.end(); ++it, suffix_num++) { std::string suffix = fmt::format("-{}", suffix_num); - callback_definition.push_back(format_callback(action_node, suffix.c_str())); - instance_creation.push_back(format_instance(action_node, suffix.c_str(), *it)); + callback_definition.push_back(format_callback(action_node, suffix)); + instance_creation.push_back(format_instance(action_node, suffix, *it)); } } } @@ -348,8 +348,8 @@ std::string XMLParser::generate_eus_action_server(const char* package_name) { callback_definition, instance_creation); } -std::string XMLParser::generate_eus_condition_server(const char* package_name) { - auto format_callback = [](const XMLElement* node, const char* suffix) { +std::string XMLParser::generate_eus_condition_server(std::string package_name) { + auto format_callback = [](const XMLElement* node, std::string suffix) { std::string fmt_string = 1 + R"( (roseus_bt:define-condition-callback {0}-cb{1} ({2}) ;; do something @@ -372,7 +372,7 @@ std::string XMLParser::generate_eus_condition_server(const char* package_name) { boost::algorithm::join(param_list, " ")); }; - auto format_instance = [package_name](const XMLElement* node, const char* suffix, + auto format_instance = [package_name](const XMLElement* node, std::string suffix, std::string service_name) { std::string fmt_string = 1 + R"( (instance roseus_bt:condition-node :init @@ -405,15 +405,15 @@ std::string XMLParser::generate_eus_condition_server(const char* package_name) { else if (server_names.size() == 1) { callback_definition.push_back(format_callback(condition_node, "")); instance_creation.push_back(format_instance(condition_node, "", - server_names.at(0).c_str())); + server_names.at(0))); } else { int suffix_num = 1; for (std::vector::const_iterator it = server_names.begin(); it != server_names.end(); ++it, suffix_num++) { std::string suffix = fmt::format("-{}", suffix_num); - callback_definition.push_back(format_callback(condition_node, suffix.c_str())); - instance_creation.push_back(format_instance(condition_node, suffix.c_str(), *it)); + callback_definition.push_back(format_callback(condition_node, suffix)); + instance_creation.push_back(format_instance(condition_node, suffix, *it)); } } } @@ -454,9 +454,9 @@ std::map XMLParser::generate_all_service_files() { return result; } -std::string XMLParser::generate_cpp_file(const char* package_name, - const char* roscpp_node_name, - const char* xml_filename) { +std::string XMLParser::generate_cpp_file(std::string package_name, + std::string roscpp_node_name, + std::string xml_filename) { const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); std::string output; output.append(generate_headers(package_name)); @@ -483,8 +483,8 @@ std::string XMLParser::generate_cpp_file(const char* package_name, return output; } -std::string XMLParser::generate_cmake_lists(const char* package_name, const char* target_name) { - auto format_pkg = [](const char* pkg) { +std::string XMLParser::generate_cmake_lists(std::string package_name, std::string target_name) { + auto format_pkg = [](std::string pkg) { return fmt::format(" {}", pkg); }; auto format_action_file = [](const XMLElement* node) { @@ -497,7 +497,7 @@ std::string XMLParser::generate_cmake_lists(const char* package_name, const char std::string msg_type = node->Attribute("type"); std::size_t pos = msg_type.find('/'); if (pos != std::string::npos) { - std::string pkg = format_pkg(msg_type.substr(0, pos).c_str()); + std::string pkg = format_pkg(msg_type.substr(0, pos)); if (std::find(message_packages->begin(), message_packages->end(), pkg) == message_packages->end()) { message_packages->push_back(pkg); @@ -544,7 +544,7 @@ std::string XMLParser::generate_cmake_lists(const char* package_name, const char action_files); } -std::string XMLParser::generate_package_xml(const char* package_name, const char* author_name) { +std::string XMLParser::generate_package_xml(std::string package_name, std::string author_name) { auto format_build_depend = [](std::string pkg) { return fmt::format(" {}", pkg); }; From eb44cfe2e5dcd6ee44cc947497f201a9ebf0c173 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 14:47:52 +0900 Subject: [PATCH 042/176] (roseus_bt) Use external symbol roseus_bt:spin in template --- roseus_bt/include/roseus_bt/gen_template.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 4119b2d41..90f16f3ea 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -277,7 +277,7 @@ std::string GenTemplate::eus_server_template(std::string server_type, %6% ;; spin -(roseus_bt::spin) +(roseus_bt:spin) )"; boost::format bfmt = boost::format(fmt_string) % From 7c7ca2f9c06a46a952d80bc66c8c8794a4f2e2ba Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 17:28:15 +0900 Subject: [PATCH 043/176] (roseus_bt) Add command line arguments --- .../include/roseus_bt/package_generator.h | 18 ++++----- roseus_bt/src/create_bt_package.cpp | 40 ++++++++++++++++++- 2 files changed, 46 insertions(+), 12 deletions(-) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 12f7e3731..f7bf6c6f7 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -13,15 +13,13 @@ namespace RoseusBT class PackageGenerator { public: - PackageGenerator(const char* package_name, - const char* xml_filename, - const char* roscpp_node_name, - const char* target_filename, - const char* author_name) : + PackageGenerator(std::string package_name, + std::string xml_filename, + std::string target_filename, + std::string author_name) : parser(xml_filename), xml_filename(xml_filename), package_name(package_name), - roscpp_node_name(roscpp_node_name), target_filename(target_filename), author_name(author_name) {}; @@ -29,11 +27,10 @@ class PackageGenerator private: XMLParser parser; - const char* package_name; + std::string package_name; std::string xml_filename; - const char* roscpp_node_name; - const char* target_filename; - const char* author_name; + std::string target_filename; + std::string author_name; public: void copy_xml_file(); @@ -95,6 +92,7 @@ void PackageGenerator::write_cpp_file() { 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::ofstream output_file(fmt::format("{}/{}.cpp", base_dir, target_filename)); output_file << parser.generate_cpp_file(package_name, roscpp_node_name, diff --git a/roseus_bt/src/create_bt_package.cpp b/roseus_bt/src/create_bt_package.cpp index 9eec8b508..a8a4f43c1 100644 --- a/roseus_bt/src/create_bt_package.cpp +++ b/roseus_bt/src/create_bt_package.cpp @@ -1,10 +1,46 @@ +#include #include +#include + +namespace po = boost::program_options; int main(int argc, char** argv) { - RoseusBT::PackageGenerator pg(argv[1], argv[2], "test_node", "mynode", "Guilherme Affonso"); + std::string package_name, model_file, author, executable, node_name; + + 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_file), "model file") + ("executable,e", po::value(&executable)->default_value("mynode"), + "executable name") + ("author,a", po::value(&author)->default_value("The Author"), + "author name"); + + 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); + + // 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() || model_file.empty()) { + std::cout << desc << std::endl; + return 1; + } + + RoseusBT::PackageGenerator pg(package_name, model_file, executable, author); pg.write_all_files(); return 0; } - From f7c353b3b5440ec7818015c3752b6cf596380f34 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 17:47:39 +0900 Subject: [PATCH 044/176] (roseus_bt) Fix bugs in euslisp file generation --- roseus_bt/include/roseus_bt/gen_template.h | 3 ++- roseus_bt/include/roseus_bt/xml_parser.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 90f16f3ea..b2d9b4be7 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -262,7 +262,7 @@ std::string GenTemplate::eus_server_template(std::string server_type, return fmt::format("(ros::roseus \"{}_server\")", server_type); }; auto format_load_ros_package = [package_name]() { - return fmt::format("(ros::load-ros-package \"{}\")\n", package_name); + return fmt::format("(ros::load-ros-package \"{}\")", package_name); }; std::string fmt_string = 1 + R"( @@ -270,6 +270,7 @@ std::string GenTemplate::eus_server_template(std::string server_type, %3% %4% + ;; define %1% callbacks %5% diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index db5eb1579..9e604d092 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -376,7 +376,8 @@ std::string XMLParser::generate_eus_condition_server(std::string package_name) { std::string service_name) { std::string fmt_string = 1 + R"( (instance roseus_bt:condition-node :init - "{3}" {0}::{1} #'{1}-execute-cb{2}) + "{3}" {0}::{1} + :execute-cb #'{1}-cb{2}) )"; return fmt::format(fmt_string, package_name, From ac7b5a7f868d2aa17b5cdde4d5791d002001da10 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 19:05:11 +0900 Subject: [PATCH 045/176] (roseus_bt) Query before overwriting files --- .../include/roseus_bt/package_generator.h | 80 +++++++++++++++++-- roseus_bt/src/create_bt_package.cpp | 6 +- 2 files changed, 76 insertions(+), 10 deletions(-) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index f7bf6c6f7..99c59f04b 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -1,7 +1,9 @@ #include #include +#include #include #include +#include #include #include #include @@ -10,27 +12,50 @@ 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); +}; + class PackageGenerator { public: PackageGenerator(std::string package_name, std::string xml_filename, std::string target_filename, - std::string author_name) : + std::string author_name, + bool force_overwrite) : + query(), parser(xml_filename), xml_filename(xml_filename), package_name(package_name), target_filename(target_filename), - author_name(author_name) {}; + author_name(author_name), + force_overwrite(force_overwrite) + {}; ~PackageGenerator() {}; private: + Query query; XMLParser parser; std::string package_name; std::string xml_filename; std::string target_filename; std::string author_name; + bool force_overwrite; + +protected: + bool overwrite(const std::string filename); public: void copy_xml_file(); @@ -45,16 +70,37 @@ class PackageGenerator }; +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 std::logic_error("Invalid input"); +} + +bool PackageGenerator::overwrite(std::string filename) { + return force_overwrite || query.yn(fmt::format("Overwrite {}?", filename)); +} + void PackageGenerator::copy_xml_file() { 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 (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) + return; + 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; } @@ -93,8 +139,12 @@ void PackageGenerator::write_cpp_file() { boost::filesystem::create_directories(base_dir); std::string roscpp_node_name = fmt::format("{}_engine", target_filename); - std::ofstream output_file(fmt::format("{}/{}.cpp", base_dir, target_filename)); + std::string dest_file = fmt::format("{}/{}.cpp", base_dir, target_filename); + + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) + return; + std::ofstream output_file(dest_file); output_file << parser.generate_cpp_file(package_name, roscpp_node_name, boost::filesystem::absolute(xml_filename).c_str()); output_file.close(); @@ -104,8 +154,11 @@ void PackageGenerator::write_eus_action_server() { std::string base_dir = fmt::format("{}/euslisp", package_name); boost::filesystem::create_directories(base_dir); - std::ofstream output_file(fmt::format("{}/action-server.l", base_dir)); + std::string dest_file = fmt::format("{}/action-server.l", base_dir); + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) + return; + std::ofstream output_file(dest_file); output_file << parser.generate_eus_action_server(package_name); output_file.close(); } @@ -114,8 +167,11 @@ void PackageGenerator::write_eus_condition_server() { std::string base_dir = fmt::format("{}/euslisp", package_name); boost::filesystem::create_directories(base_dir); - std::ofstream output_file(fmt::format("{}/condition-server.l", base_dir)); + std::string dest_file = fmt::format("{}/condition-server.l", base_dir); + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) + return; + std::ofstream output_file(dest_file); output_file << parser.generate_eus_condition_server(package_name); output_file.close(); } @@ -124,7 +180,11 @@ void PackageGenerator::write_cmake_lists() { std::string base_dir = package_name; boost::filesystem::create_directories(base_dir); - std::ofstream output_file(fmt::format("{}/CMakeLists.txt", base_dir)); + std::string dest_file = fmt::format("{}/CMakeLists.txt", base_dir); + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) + return; + + std::ofstream output_file(dest_file); output_file << parser.generate_cmake_lists(package_name, target_filename); output_file.close(); } @@ -133,7 +193,11 @@ void PackageGenerator::write_package_xml() { std::string base_dir = package_name; boost::filesystem::create_directories(base_dir); - std::ofstream output_file(fmt::format("{}/package.xml", base_dir)); + std::string dest_file = fmt::format("{}/package.xml", base_dir); + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) + return; + + std::ofstream output_file(dest_file); output_file << parser.generate_package_xml(package_name, author_name); output_file.close(); } diff --git a/roseus_bt/src/create_bt_package.cpp b/roseus_bt/src/create_bt_package.cpp index a8a4f43c1..90e89a40e 100644 --- a/roseus_bt/src/create_bt_package.cpp +++ b/roseus_bt/src/create_bt_package.cpp @@ -16,7 +16,8 @@ int main(int argc, char** argv) ("executable,e", po::value(&executable)->default_value("mynode"), "executable name") ("author,a", po::value(&author)->default_value("The Author"), - "author name"); + "author name") + ("overwrite,y", "overwrite all existing files"); po::positional_options_description positional_arguments; positional_arguments.add("package_name", 1); @@ -39,7 +40,8 @@ int main(int argc, char** argv) return 1; } - RoseusBT::PackageGenerator pg(package_name, model_file, executable, author); + RoseusBT::PackageGenerator pg(package_name, model_file, executable, author, + args.count("overwrite")); pg.write_all_files(); return 0; From 8feb2c058d07c1be898ca470fa7c8c2f7cc3ee25 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 19:11:56 +0900 Subject: [PATCH 046/176] (roseus_bt) Use const strings on function arguments --- .../include/roseus_bt/package_generator.h | 12 ++--- roseus_bt/include/roseus_bt/xml_parser.h | 52 ++++++++++--------- 2 files changed, 33 insertions(+), 31 deletions(-) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 99c59f04b..e01106080 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -29,10 +29,10 @@ class Query class PackageGenerator { public: - PackageGenerator(std::string package_name, - std::string xml_filename, - std::string target_filename, - std::string author_name, + PackageGenerator(const std::string package_name, + const std::string xml_filename, + const std::string target_filename, + const std::string author_name, bool force_overwrite) : query(), parser(xml_filename), @@ -67,9 +67,9 @@ class PackageGenerator void write_cmake_lists(); void write_package_xml(); void write_all_files(); - }; + bool Query::yn(const std::string message) { auto prompt = [message]() { std::cout << message << " [Y/n] "; @@ -85,7 +85,7 @@ bool Query::yn(const std::string message) { throw std::logic_error("Invalid input"); } -bool PackageGenerator::overwrite(std::string filename) { +bool PackageGenerator::overwrite(const std::string filename) { return force_overwrite || query.yn(fmt::format("Overwrite {}?", filename)); } diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 9e604d092..beba52df6 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -29,24 +29,24 @@ class XMLParser std::string port_node_to_message_description(const XMLElement* port_node); std::string generate_action_file_contents(const XMLElement* node); std::string generate_service_file_contents(const XMLElement* node); - std::string generate_headers(std::string package_name); - std::string generate_action_class(const XMLElement* node, std::string package_name); - std::string generate_condition_class(const XMLElement* node, std::string package_name); - std::string generate_main_function(std::string roscpp_node_name, std::string xml_filename); + 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_condition_class(const XMLElement* node, const std::string package_name); + std::string generate_main_function(const std::string roscpp_node_name, const std::string xml_filename); public: std::map generate_all_action_files(); std::map generate_all_service_files(); - std::string generate_cpp_file(std::string package_name, - std::string roscpp_node_name, - std::string xml_filename); - std::string generate_eus_action_server(std::string action_name); - std::string generate_eus_condition_server(std::string action_name); - std::string generate_cmake_lists(std::string package_name, - std::string target_filename); - std::string generate_package_xml(std::string package_name, - std::string author_name); + std::string generate_cpp_file(const std::string package_name, + const std::string roscpp_node_name, + const std::string xml_filename); + std::string generate_eus_action_server(const std::string action_name); + std::string generate_eus_condition_server(const std::string action_name); + std::string generate_cmake_lists(const std::string package_name, + const std::string target_filename); + std::string generate_package_xml(const std::string package_name, + const std::string author_name); }; @@ -130,7 +130,7 @@ std::string XMLParser::generate_service_file_contents(const XMLElement* node) { return gen_template.service_file_template(request); } -std::string XMLParser::generate_headers(std::string package_name) { +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, @@ -163,7 +163,7 @@ std::string XMLParser::generate_headers(std::string package_name) { return gen_template.headers_template(headers); } -std::string XMLParser::generate_action_class(const XMLElement* node, std::string package_name) { +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")); @@ -215,7 +215,7 @@ std::string XMLParser::generate_action_class(const XMLElement* node, std::string provided_ports, get_inputs, set_outputs); } -std::string XMLParser::generate_condition_class(const XMLElement* node, std::string package_name) { +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")); @@ -240,8 +240,8 @@ std::string XMLParser::generate_condition_class(const XMLElement* node, std::str provided_ports, get_inputs); } -std::string XMLParser::generate_main_function(std::string roscpp_node_name, - std::string xml_filename) { +std::string XMLParser::generate_main_function(const std::string roscpp_node_name, + const std::string xml_filename) { auto format_action_node = [](const XMLElement* node) { return fmt::format(" RegisterRosAction<{0}>(factory, \"{0}\", nh);", node->Attribute("ID")); @@ -273,7 +273,7 @@ std::string XMLParser::generate_main_function(std::string roscpp_node_name, register_actions, register_conditions); } -std::string XMLParser::generate_eus_action_server(std::string package_name) { +std::string XMLParser::generate_eus_action_server(const std::string package_name) { auto format_callback = [](const XMLElement* node, std::string suffix) { std::string fmt_string = 1 + R"( (roseus_bt:define-action-callback {0}-execute-cb{1} ({2}) @@ -348,7 +348,7 @@ std::string XMLParser::generate_eus_action_server(std::string package_name) { callback_definition, instance_creation); } -std::string XMLParser::generate_eus_condition_server(std::string package_name) { +std::string XMLParser::generate_eus_condition_server(const std::string package_name) { auto format_callback = [](const XMLElement* node, std::string suffix) { std::string fmt_string = 1 + R"( (roseus_bt:define-condition-callback {0}-cb{1} ({2}) @@ -455,9 +455,9 @@ std::map XMLParser::generate_all_service_files() { return result; } -std::string XMLParser::generate_cpp_file(std::string package_name, - std::string roscpp_node_name, - std::string xml_filename) { +std::string XMLParser::generate_cpp_file(const std::string package_name, + const std::string roscpp_node_name, + const std::string xml_filename) { const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); std::string output; output.append(generate_headers(package_name)); @@ -484,7 +484,8 @@ std::string XMLParser::generate_cpp_file(std::string package_name, return output; } -std::string XMLParser::generate_cmake_lists(std::string package_name, std::string target_name) { +std::string XMLParser::generate_cmake_lists(const std::string package_name, + const std::string target_name) { auto format_pkg = [](std::string pkg) { return fmt::format(" {}", pkg); }; @@ -545,7 +546,8 @@ std::string XMLParser::generate_cmake_lists(std::string package_name, std::strin action_files); } -std::string XMLParser::generate_package_xml(std::string package_name, std::string author_name) { +std::string XMLParser::generate_package_xml(const std::string package_name, + const std::string author_name) { auto format_build_depend = [](std::string pkg) { return fmt::format(" {}", pkg); }; From 9b5ba8399f8ce2bc9f1f1def375356fe648dc405 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Jun 2021 13:49:24 +0900 Subject: [PATCH 047/176] (roseus_bt) Add subscriber nodes --- .../include/roseus_bt/eus_subscriber_node.h | 72 +++++++++++++++++++ roseus_bt/include/roseus_bt/gen_template.h | 46 +++++++++++- roseus_bt/include/roseus_bt/xml_parser.h | 64 ++++++++++++++++- 3 files changed, 178 insertions(+), 4 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/eus_subscriber_node.h 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..689e2bef7 --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_subscriber_node.h @@ -0,0 +1,72 @@ +#ifndef BEHAVIOR_TREE_EUS_SUBSCRIBER_NODE_HPP_ +#define BEHAVIOR_TREE_EUS_SUBSCRIBER_NODE_HPP_ + +#include + +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); + } + +private: + ros::NodeHandle& node_; + ros::Subscriber sub_; + +public: + EusSubscriberNode() = delete; + virtual ~EusSubscriberNode() = default; + + static PortsList providedPorts() { + return { + InputPort("topic_name", "name of the subscribed topic"), + OutputPort("to", "port to where messages are redirected") + }; + } + + virtual void callback(MessageT msg) { + setOutput("to", msg); + } + + virtual BT::NodeStatus tick() override final + { + return NodeStatus::SUCCESS; + } + + virtual void halt() override final + { + setStatus(NodeStatus::IDLE); + } +}; + + +/// 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; + factory.registerBuilder( manifest, builder ); +} + +} // namespace BT + +#endif // BEHAVIOR_TREE_EUS_SUBSCRIBER_NODE_HPP_ + + diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index b2d9b4be7..fe7bee020 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -29,10 +29,13 @@ class GenTemplate std::string 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::string message_field); std::string main_function_template(std::string roscpp_node_name, std::string xml_filename, std::vector register_actions, - std::vector register_conditions); + std::vector register_conditions, + std::vector register_subscribers); std::string eus_server_template(std::string server_type, std::string package_name, std::vector callbacks, @@ -86,6 +89,7 @@ std::string GenTemplate::headers_template(std::vector headers) { #include #include +#include #include #include @@ -203,10 +207,44 @@ class %2%: public EusConditionNode<%1%::%2%> } +std::string GenTemplate::subscriber_class_template(std::string nodeID, std::string message_type, + std::string message_field) { + if (!message_field.empty()) { + std::string fmt_string = R"( + virtual void callback(%1% msg) { + setOutput("to", msg.%2%); + } +)"; + + boost::format bfmt = boost::format(fmt_string) % + message_type % + message_field; + + message_field = bfmt.str(); + } + + std::string fmt_string = 1 + R"( +class %1%: public EusSubscriberNode<%2%> +{ +public: + %1%(ros::NodeHandle& handle, const std::string& node_name, const NodeConfiguration& conf) : + EusSubscriberNode<%2%>(handle, node_name, conf) {} +%3%}; +)"; + boost::format bfmt = boost::format(fmt_string) % + nodeID % + message_type % + message_field; + + return bfmt.str(); +} + + std::string GenTemplate::main_function_template(std::string roscpp_node_name, std::string xml_filename, std::vector register_actions, - std::vector register_conditions) { + 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); }; @@ -224,6 +262,7 @@ int main(int argc, char **argv) %3% %4% +%5% %2% @@ -248,7 +287,8 @@ int main(int argc, char **argv) format_ros_init() % format_create_tree() % boost::algorithm::join(register_actions, "\n") % - boost::algorithm::join(register_conditions, "\n"); + boost::algorithm::join(register_conditions, "\n") % + boost::algorithm::join(register_subscribers, "\n"); return bfmt.str(); } diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index beba52df6..fadf21ba8 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -32,6 +32,7 @@ class XMLParser 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_condition_class(const XMLElement* node, const std::string package_name); + std::string generate_subscriber_class(const XMLElement* node); std::string generate_main_function(const std::string roscpp_node_name, const std::string xml_filename); public: @@ -143,6 +144,10 @@ std::string XMLParser::generate_headers(const std::string package_name) { node->Attribute("ID")); }; + auto format_subscriber_node = [](const XMLElement* node) { + return fmt::format("#include <{}.h>", node->Attribute("type")); + }; + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); std::vector headers; @@ -160,6 +165,13 @@ std::string XMLParser::generate_headers(const std::string package_name) { headers.push_back(format_condition_node(condition_node, package_name)); } + for (auto subscriber_node = root->FirstChildElement("Subscriber"); + subscriber_node != nullptr; + subscriber_node = subscriber_node->NextSiblingElement("Subscriber")) + { + headers.push_back(format_subscriber_node(subscriber_node)); + } + return gen_template.headers_template(headers); } @@ -240,6 +252,26 @@ std::string XMLParser::generate_condition_class(const XMLElement* node, const st provided_ports, get_inputs); } +std::string XMLParser::generate_subscriber_class(const XMLElement* node) { + auto format_type = [](const XMLElement* node) { + std::string type = node->Attribute("type"); + std::size_t pos = type.find('/'); + if (pos == std::string::npos) { + throw std::logic_error(fmt::format("Invalid topic type in: {}", type)); + } + return fmt::format("{}::{}", type.substr(0, pos), type.substr(1+pos)); + }; + auto format_field = [](const XMLElement* node) { + if (!node->Attribute("field")) return ""; + return node->Attribute("field"); + }; + + return gen_template.subscriber_class_template(node->Attribute("ID"), + format_type(node), + format_field(node)); +} + + std::string XMLParser::generate_main_function(const std::string roscpp_node_name, const std::string xml_filename) { auto format_action_node = [](const XMLElement* node) { @@ -250,10 +282,15 @@ std::string XMLParser::generate_main_function(const std::string roscpp_node_name return fmt::format(" RegisterRosService<{0}>(factory, \"{0}\", nh);", node->Attribute("ID")); }; + auto format_subscriber_node = [](const XMLElement* node) { + return fmt::format(" RegisterSubscriberNode<{0}>(factory, \"{0}\", nh);", + 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; @@ -269,8 +306,17 @@ std::string XMLParser::generate_main_function(const std::string roscpp_node_name register_conditions.push_back(format_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)); + } + return gen_template.main_function_template(roscpp_node_name, xml_filename, - register_actions, register_conditions); + register_actions, + register_conditions, + register_subscribers); } std::string XMLParser::generate_eus_action_server(const std::string package_name) { @@ -479,6 +525,14 @@ std::string XMLParser::generate_cpp_file(const std::string 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"); + } + output.append(generate_main_function(roscpp_node_name, xml_filename)); return output; @@ -514,6 +568,7 @@ std::string XMLParser::generate_cmake_lists(const std::string package_name, message_packages.push_back(format_pkg("std_msgs")); message_packages.push_back(format_pkg("actionlib_msgs")); + for (auto action_node = root->FirstChildElement("Action"); action_node != nullptr; action_node = action_node->NextSiblingElement("Action")) @@ -540,6 +595,13 @@ std::string XMLParser::generate_cmake_lists(const std::string package_name, } } + for (auto subscriber_node = root->FirstChildElement("Subscriber"); + subscriber_node != nullptr; + subscriber_node = subscriber_node->NextSiblingElement("Subscriber")) + { + maybe_push_message_package(subscriber_node, &message_packages); + } + return gen_template.cmake_lists_template(package_name, target_name, message_packages, service_files, From 9bb4e6abfc93d8304fb26e8d06454d45d12c41db Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Jun 2021 13:50:22 +0900 Subject: [PATCH 048/176] (roseus_bt) Add roseus_bt/eus_nodes.h header file --- roseus_bt/include/roseus_bt/eus_nodes.h | 9 +++++++++ roseus_bt/include/roseus_bt/gen_template.h | 5 +---- 2 files changed, 10 insertions(+), 4 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/eus_nodes.h 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..a331478ba --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_nodes.h @@ -0,0 +1,9 @@ +#ifndef BEHAVIOR_TREE_EUS_NODES_HPP_ +#define BEHAVIOR_TREE_EUS_NODES_HPP_ + +#include "eus_action_node.h" +#include "eus_condition_node.h" +#include "eus_subscriber_node.h" + + +#endif // BEHAVIOR_TREE_EUS_NODES_HPP_ diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index fe7bee020..7d2dd0845 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -87,10 +87,7 @@ std::string GenTemplate::headers_template(std::vector headers) { std::string fmt_string = 1 + R"( #include -#include -#include -#include - +#include #include #include From f11665a0fbd2ce49493e8108e5bbbed04b0a7b2a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 14:17:03 +0900 Subject: [PATCH 049/176] (roseus_bt) Add eus-like function names --- roseus_bt/include/roseus_bt/xml_parser.h | 31 ++++++++++++++++-------- 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index fadf21ba8..1e605a4c4 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -26,6 +26,7 @@ class XMLParser GenTemplate gen_template; void collect_node_attribute(const XMLElement* node, const XMLElement* ref_node, const char* attribute, std::vector* node_attributes); + std::string format_eus_name(const std::string input); std::string port_node_to_message_description(const XMLElement* port_node); std::string generate_action_file_contents(const XMLElement* node); std::string generate_service_file_contents(const XMLElement* node); @@ -75,6 +76,14 @@ void XMLParser::collect_node_attribute(const XMLElement* node, } } +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::port_node_to_message_description(const XMLElement* port_node) { if (!port_node->Attribute("type") || !port_node->Attribute("name")) { @@ -320,7 +329,7 @@ std::string XMLParser::generate_main_function(const std::string roscpp_node_name } std::string XMLParser::generate_eus_action_server(const std::string package_name) { - auto format_callback = [](const XMLElement* node, std::string suffix) { + auto format_callback = [this](const XMLElement* node, std::string suffix) { std::string fmt_string = 1 + R"( (roseus_bt:define-action-callback {0}-execute-cb{1} ({2}) ;; do something @@ -338,21 +347,22 @@ std::string XMLParser::generate_eus_action_server(const std::string package_name } return fmt::format(fmt_string, - node->Attribute("ID"), + format_eus_name(node->Attribute("ID")), suffix, boost::algorithm::join(param_list, " ")); }; - auto format_instance = [package_name](const XMLElement* node, std::string suffix, - std::string server_name) { + auto format_instance = [this, package_name](const XMLElement* node, std::string suffix, + std::string server_name) { std::string fmt_string = 1 + R"( (instance roseus_bt:action-node :init "{3}" {0}::{1}Action - :execute-cb '{1}-execute-cb{2}) + :execute-cb '{2}-execute-cb{3}) )"; return fmt::format(fmt_string, package_name, node->Attribute("ID"), + format_eus_name(node->Attribute("ID")), suffix, server_name); }; @@ -395,7 +405,7 @@ std::string XMLParser::generate_eus_action_server(const std::string package_name } std::string XMLParser::generate_eus_condition_server(const std::string package_name) { - auto format_callback = [](const XMLElement* node, std::string suffix) { + auto format_callback = [this](const XMLElement* node, std::string suffix) { std::string fmt_string = 1 + R"( (roseus_bt:define-condition-callback {0}-cb{1} ({2}) ;; do something @@ -413,21 +423,22 @@ std::string XMLParser::generate_eus_condition_server(const std::string package_n } return fmt::format(fmt_string, - node->Attribute("ID"), + format_eus_name(node->Attribute("ID")), suffix, boost::algorithm::join(param_list, " ")); }; - auto format_instance = [package_name](const XMLElement* node, std::string suffix, - std::string service_name) { + auto format_instance = [this, package_name](const XMLElement* node, std::string suffix, + std::string service_name) { std::string fmt_string = 1 + R"( (instance roseus_bt:condition-node :init "{3}" {0}::{1} - :execute-cb #'{1}-cb{2}) + :execute-cb #'{2}-cb{3}) )"; return fmt::format(fmt_string, package_name, node->Attribute("ID"), + format_eus_name(node->Attribute("ID")), suffix, service_name); }; From 5ad47053747698d313d6fd992e116880b33c376d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 14:29:27 +0900 Subject: [PATCH 050/176] (roseus_bt) Add comments on roseus_bt:set-output usage --- roseus_bt/include/roseus_bt/xml_parser.h | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 1e605a4c4..c6fd31141 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -329,13 +329,21 @@ std::string XMLParser::generate_main_function(const std::string roscpp_node_name } std::string XMLParser::generate_eus_action_server(const std::string package_name) { - auto format_callback = [this](const XMLElement* node, std::string suffix) { + auto format_setoutput = [](const XMLElement* node) { + return fmt::format(" ;; (roseus_bt:set-output \"{0}\" <{1}>)", + node->Attribute("name"), + node->Attribute("type")); + }; + + auto format_callback = [this, format_setoutput](const XMLElement* node, std::string suffix) { std::string fmt_string = 1 + R"( (roseus_bt:define-action-callback {0}-execute-cb{1} ({2}) ;; do something +{3} t) )"; std::vector param_list; + std::vector output_list; for (auto port_node = node->FirstChildElement(); port_node != nullptr; port_node = port_node->NextSiblingElement()) @@ -344,12 +352,16 @@ std::string XMLParser::generate_eus_action_server(const std::string package_name if (name == "input_port" || name == "inout_port") { param_list.push_back(port_node->Attribute("name")); } + if (name == "output_port" || name == "inout_port") { + output_list.push_back(format_setoutput(port_node)); + } } return fmt::format(fmt_string, format_eus_name(node->Attribute("ID")), suffix, - boost::algorithm::join(param_list, " ")); + boost::algorithm::join(param_list, " "), + boost::algorithm::join(output_list, "\n")); }; auto format_instance = [this, package_name](const XMLElement* node, std::string suffix, From 0394133ff4c714d122821aa6cb703516a439b9d2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 14:31:25 +0900 Subject: [PATCH 051/176] (roseus_bt) Add hyphen-spelled package nickname --- roseus_bt/euslisp/nodes.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 31ece86dd..a78f9a3c4 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -1,5 +1,5 @@ (unless (find-package "ROSEUS_BT") - (make-package "ROSEUS_BT")) + (make-package "ROSEUS_BT" :nicknames (list "ROSEUS-BT"))) (in-package "ROSEUS_BT") From 1a67a2ce4cbb119480acb7fde80b808dc5eca521 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 14:46:47 +0900 Subject: [PATCH 052/176] (roseus_bt) Add maybe_push_message_package as common utility --- roseus_bt/include/roseus_bt/xml_parser.h | 44 +++++++++++------------- 1 file changed, 20 insertions(+), 24 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index c6fd31141..6fc943c1f 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -26,6 +26,8 @@ class XMLParser GenTemplate gen_template; void collect_node_attribute(const XMLElement* node, const XMLElement* ref_node, const char* attribute, std::vector* node_attributes); + void maybe_push_message_package(const XMLElement* node, + std::vector* message_packages); std::string format_eus_name(const std::string input); std::string port_node_to_message_description(const XMLElement* port_node); std::string generate_action_file_contents(const XMLElement* node); @@ -76,6 +78,19 @@ void XMLParser::collect_node_attribute(const XMLElement* node, } } +void XMLParser::maybe_push_message_package(const XMLElement* node, + std::vector* message_packages) { + std::string msg_type = node->Attribute("type"); + std::size_t pos = msg_type.find('/'); + if (pos != std::string::npos) { + std::string pkg = msg_type.substr(0, pos); + if (std::find(message_packages->begin(), message_packages->end(), pkg) == + message_packages->end()) { + message_packages->push_back(pkg); + } + } +}; + 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"); @@ -572,25 +587,14 @@ std::string XMLParser::generate_cmake_lists(const std::string package_name, auto format_service_file = [](const XMLElement* node) { return fmt::format(" {}.srv", node->Attribute("ID")); }; - auto maybe_push_message_package = [format_pkg](const XMLElement* node, std::vector* message_packages) { - std::string msg_type = node->Attribute("type"); - std::size_t pos = msg_type.find('/'); - if (pos != std::string::npos) { - std::string pkg = format_pkg(msg_type.substr(0, pos)); - if (std::find(message_packages->begin(), message_packages->end(), pkg) == - message_packages->end()) { - message_packages->push_back(pkg); - } - } - }; const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); std::vector message_packages; std::vector action_files; std::vector service_files; - message_packages.push_back(format_pkg("std_msgs")); - message_packages.push_back(format_pkg("actionlib_msgs")); + message_packages.push_back("std_msgs"); + message_packages.push_back("actionlib_msgs"); for (auto action_node = root->FirstChildElement("Action"); action_node != nullptr; @@ -625,6 +629,9 @@ std::string XMLParser::generate_cmake_lists(const std::string package_name, maybe_push_message_package(subscriber_node, &message_packages); } + std::transform(message_packages.begin(), message_packages.end(), + message_packages.begin(), format_pkg); + return gen_template.cmake_lists_template(package_name, target_name, message_packages, service_files, @@ -639,17 +646,6 @@ std::string XMLParser::generate_package_xml(const std::string package_name, auto format_exec_depend = [](std::string pkg) { return fmt::format(" {}", pkg); }; - auto maybe_push_message_package = [](const XMLElement* node, std::vector* message_packages) { - std::string msg_type = node->Attribute("type"); - std::size_t pos = msg_type.find('/'); - if (pos != std::string::npos) { - std::string pkg = msg_type.substr(0, pos); - if (std::find(message_packages->begin(), message_packages->end(), pkg) == - message_packages->end()) { - message_packages->push_back(pkg); - } - } - }; const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); std::vector message_packages; From c114318a103c6fdb0006b9e55fed75a1e411c867 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 15:43:39 +0900 Subject: [PATCH 053/176] (roseus_bt) Don't write eus files if not needed --- roseus_bt/include/roseus_bt/package_generator.h | 10 ++++++++-- roseus_bt/include/roseus_bt/xml_parser.h | 4 ++++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index e01106080..759b2e2d5 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -158,8 +158,11 @@ void PackageGenerator::write_eus_action_server() { if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) return; + std::string body = parser.generate_eus_action_server(package_name); + if (body.empty()) return; + std::ofstream output_file(dest_file); - output_file << parser.generate_eus_action_server(package_name); + output_file << body; output_file.close(); } @@ -171,8 +174,11 @@ void PackageGenerator::write_eus_condition_server() { if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) return; + std::string body = parser.generate_eus_condition_server(package_name); + if (body.empty()) return; + std::ofstream output_file(dest_file); - output_file << parser.generate_eus_condition_server(package_name); + output_file << body; output_file.close(); } diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 6fc943c1f..cf1073be8 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -427,6 +427,8 @@ std::string XMLParser::generate_eus_action_server(const std::string package_name } } + if (callback_definition.empty()) return ""; + return gen_template.eus_server_template("action", package_name, callback_definition, instance_creation); } @@ -503,6 +505,8 @@ std::string XMLParser::generate_eus_condition_server(const std::string package_n } } + if (callback_definition.empty()) return ""; + return gen_template.eus_server_template("condition", package_name, callback_definition, instance_creation); } From 008b58d5a5c93b3dd34f5fcbaa48147b4b462966 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 15:50:07 +0900 Subject: [PATCH 054/176] (roseus_bt) Skip copy_xml_file if source is equal to destination --- roseus_bt/include/roseus_bt/package_generator.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 759b2e2d5..b7238df26 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -95,6 +95,9 @@ void PackageGenerator::copy_xml_file() { 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; From 89eb54cba3dd0aae4f30c6a6ab521c50ca4a5f5b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 6 Jul 2021 16:29:41 +0900 Subject: [PATCH 055/176] (roseus_bt) Use model filename as default executable name --- roseus_bt/include/roseus_bt/package_generator.h | 4 ++-- roseus_bt/src/create_bt_package.cpp | 9 +++++++-- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index b7238df26..cb87d1091 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -157,7 +157,7 @@ void PackageGenerator::write_eus_action_server() { std::string base_dir = fmt::format("{}/euslisp", package_name); boost::filesystem::create_directories(base_dir); - std::string dest_file = fmt::format("{}/action-server.l", base_dir); + std::string dest_file = fmt::format("{}/{}-action-server.l", base_dir, target_filename); if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) return; @@ -173,7 +173,7 @@ void PackageGenerator::write_eus_condition_server() { std::string base_dir = fmt::format("{}/euslisp", package_name); boost::filesystem::create_directories(base_dir); - std::string dest_file = fmt::format("{}/condition-server.l", base_dir); + std::string dest_file = fmt::format("{}/{}-condition-server.l", base_dir, target_filename); if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) return; diff --git a/roseus_bt/src/create_bt_package.cpp b/roseus_bt/src/create_bt_package.cpp index 90e89a40e..ecccdfa83 100644 --- a/roseus_bt/src/create_bt_package.cpp +++ b/roseus_bt/src/create_bt_package.cpp @@ -1,6 +1,7 @@ #include #include #include +#include namespace po = boost::program_options; @@ -13,8 +14,7 @@ int main(int argc, char** argv) ("help,h", "show this help message and exit") ("package_name", po::value(&package_name), "package name") ("model_file", po::value(&model_file), "model file") - ("executable,e", po::value(&executable)->default_value("mynode"), - "executable name") + ("executable,e", po::value(&executable), "executable name (defaults to model filename)") ("author,a", po::value(&author)->default_value("The Author"), "author name") ("overwrite,y", "overwrite all existing files"); @@ -40,6 +40,11 @@ int main(int argc, char** argv) return 1; } + // Fill default executable name + if (executable.empty()) { + executable = boost::filesystem::path(model_file).stem().string(); + } + RoseusBT::PackageGenerator pg(package_name, model_file, executable, author, args.count("overwrite")); pg.write_all_files(); From 2f44250d4ee69a72e420a3468853877e26cafc08 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 6 Jul 2021 20:03:21 +0900 Subject: [PATCH 056/176] (roseus_bt) Allow to assign multiple model files --- roseus_bt/include/roseus_bt/gen_template.h | 135 ----------- .../include/roseus_bt/package_generator.h | 127 ++++++---- roseus_bt/include/roseus_bt/pkg_template.h | 227 ++++++++++++++++++ roseus_bt/include/roseus_bt/xml_parser.h | 97 ++------ roseus_bt/src/create_bt_package.cpp | 29 ++- 5 files changed, 344 insertions(+), 271 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/pkg_template.h diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 7d2dd0845..17ece8b1d 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -1,7 +1,6 @@ #ifndef BEHAVIOR_TREE_ROSEUS_BT_GEN_TEMPLATE_ #define BEHAVIOR_TREE_ROSEUS_BT_GEN_TEMPLATE_ -#include #include #include #include @@ -40,14 +39,6 @@ class GenTemplate std::string package_name, std::vector callbacks, std::vector instances); - std::string cmake_lists_template(std::string package_name, std::string target_name, - std::vector message_packages, - std::vector service_files, - std::vector action_files); - std::string package_xml_template(std::string package_name, - std::string author_name, - std::vector build_dependencies, - std::vector exec_dependencies); }; @@ -330,132 +321,6 @@ std::string GenTemplate::eus_server_template(std::string server_type, } -std::string GenTemplate::cmake_lists_template(std::string package_name, std::string target_name, - std::vector message_packages, - std::vector service_files, - std::vector action_files) { - std::string fmt_string = 1+ R"( -cmake_minimum_required(VERSION 3.0.2) -project(%1%) - -find_package(catkin REQUIRED COMPONENTS - message_generation - roscpp - behaviortree_ros - roseus_bt -%3% -) - -add_service_files( - FILES -%4% -) - -add_action_files( - FILES -%5% -) - -generate_messages( - DEPENDENCIES -%3% -) - -catkin_package( - INCLUDE_DIRS - LIBRARIES - CATKIN_DEPENDS - message_runtime -%3% -) - - -include_directories(${catkin_INCLUDE_DIRS}) - -add_executable(%2% src/%2%.cpp) -add_dependencies(%2% ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(%2% ${catkin_LIBRARIES}) -)"; - - boost::format bfmt = boost::format(fmt_string) % - package_name % - target_name % - boost::algorithm::join(message_packages, "\n") % - boost::algorithm::join(service_files, "\n") % - boost::algorithm::join(action_files, "\n"); - - return bfmt.str(); -} - - -std::string GenTemplate::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(); -} - - } // 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 index cb87d1091..5e4d4ed10 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -7,6 +7,7 @@ #include #include #include +#include namespace RoseusBT @@ -30,15 +31,16 @@ class PackageGenerator { public: PackageGenerator(const std::string package_name, - const std::string xml_filename, - const std::string target_filename, + const std::vector xml_filenames, + const std::vector target_filenames, const std::string author_name, bool force_overwrite) : query(), - parser(xml_filename), - xml_filename(xml_filename), + pkg_template(), + parser_vector(xml_filenames.begin(), xml_filenames.end()), package_name(package_name), - target_filename(target_filename), + xml_filenames(xml_filenames), + target_filenames(target_filenames), author_name(author_name), force_overwrite(force_overwrite) {}; @@ -47,10 +49,11 @@ class PackageGenerator private: Query query; - XMLParser parser; + PkgTemplate pkg_template; + std::vector parser_vector; std::string package_name; - std::string xml_filename; - std::string target_filename; + std::vector xml_filenames; + std::vector target_filenames; std::string author_name; bool force_overwrite; @@ -58,14 +61,17 @@ class PackageGenerator bool overwrite(const std::string filename); public: - void copy_xml_file(); - void write_action_files(); - void write_service_files(); - void write_cpp_file(); - void write_eus_action_server(); - void write_eus_condition_server(); - void write_cmake_lists(); - void write_package_xml(); + void copy_xml_file(std::string* xml_filename); + void write_action_files(XMLParser* parser); + void write_service_files(XMLParser* parser); + void write_cpp_file(XMLParser* parser, + const std::string target_filename, const std::string xml_filename); + void write_eus_action_server(XMLParser* parser, const std::string target_filename); + void write_eus_condition_server(XMLParser* 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(); }; @@ -89,55 +95,53 @@ bool PackageGenerator::overwrite(const std::string filename) { return force_overwrite || query.yn(fmt::format("Overwrite {}?", filename)); } -void PackageGenerator::copy_xml_file() { +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()); + boost::filesystem::path(*xml_filename).filename().c_str()); - if (dest_file == xml_filename) + if (dest_file == *xml_filename) return; if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) return; boost::filesystem::create_directories(base_dir); - boost::filesystem::copy_file(xml_filename, dest_file, + boost::filesystem::copy_file(*xml_filename, dest_file, boost::filesystem::copy_option::overwrite_if_exists); - xml_filename = dest_file; + xml_filename = &dest_file; } -void PackageGenerator::write_action_files() { +void PackageGenerator::write_action_files(XMLParser* parser) { std::string base_dir = fmt::format("{}/action", package_name); boost::filesystem::create_directories(base_dir); - std::map action_files; - std::map::iterator it; - action_files = parser.generate_all_action_files(); + std::map action_files = parser->generate_all_action_files(); - for (it=action_files.begin(); it!=action_files.end(); ++it) { + for (auto it=action_files.begin(); it!=action_files.end(); ++it) { std::ofstream output_file(fmt::format("{}/{}", base_dir, it->first)); output_file << it->second; output_file.close(); } } -void PackageGenerator::write_service_files() { +void PackageGenerator::write_service_files(XMLParser* parser) { std::string base_dir = fmt::format("{}/srv", package_name); boost::filesystem::create_directories(base_dir); - std::map action_files; - std::map::iterator it; - action_files = parser.generate_all_service_files(); + std::map service_files = parser->generate_all_service_files(); - for (it=action_files.begin(); it!=action_files.end(); ++it) { + for (auto it=service_files.begin(); it!=service_files.end(); ++it) { std::ofstream output_file(fmt::format("{}/{}", base_dir, it->first)); output_file << it->second; output_file.close(); } } -void PackageGenerator::write_cpp_file() { +void PackageGenerator::write_cpp_file(XMLParser* 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); @@ -148,12 +152,13 @@ void PackageGenerator::write_cpp_file() { return; std::ofstream output_file(dest_file); - output_file << parser.generate_cpp_file(package_name, roscpp_node_name, - boost::filesystem::absolute(xml_filename).c_str()); + output_file << parser->generate_cpp_file(package_name, roscpp_node_name, + boost::filesystem::absolute(xml_filename).c_str()); output_file.close(); } -void PackageGenerator::write_eus_action_server() { +void PackageGenerator::write_eus_action_server(XMLParser* parser, + const std::string target_filename) { std::string base_dir = fmt::format("{}/euslisp", package_name); boost::filesystem::create_directories(base_dir); @@ -161,7 +166,7 @@ void PackageGenerator::write_eus_action_server() { if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) return; - std::string body = parser.generate_eus_action_server(package_name); + std::string body = parser->generate_eus_action_server(package_name); if (body.empty()) return; std::ofstream output_file(dest_file); @@ -169,7 +174,8 @@ void PackageGenerator::write_eus_action_server() { output_file.close(); } -void PackageGenerator::write_eus_condition_server() { +void PackageGenerator::write_eus_condition_server(XMLParser* parser, + const std::string target_filename) { std::string base_dir = fmt::format("{}/euslisp", package_name); boost::filesystem::create_directories(base_dir); @@ -177,7 +183,7 @@ void PackageGenerator::write_eus_condition_server() { if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) return; - std::string body = parser.generate_eus_condition_server(package_name); + std::string body = parser->generate_eus_condition_server(package_name); if (body.empty()) return; std::ofstream output_file(dest_file); @@ -185,7 +191,9 @@ void PackageGenerator::write_eus_condition_server() { output_file.close(); } -void PackageGenerator::write_cmake_lists() { +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); @@ -194,11 +202,14 @@ void PackageGenerator::write_cmake_lists() { return; std::ofstream output_file(dest_file); - output_file << parser.generate_cmake_lists(package_name, target_filename); + output_file << pkg_template.generate_cmake_lists(package_name, target_filenames, + message_packages, + service_files, + action_files); output_file.close(); } -void PackageGenerator::write_package_xml() { +void PackageGenerator::write_package_xml(const std::vector message_packages) { std::string base_dir = package_name; boost::filesystem::create_directories(base_dir); @@ -207,19 +218,35 @@ void PackageGenerator::write_package_xml() { return; std::ofstream output_file(dest_file); - output_file << parser.generate_package_xml(package_name, author_name); + output_file << pkg_template.generate_package_xml(package_name, author_name, + message_packages); output_file.close(); } void PackageGenerator::write_all_files() { - copy_xml_file(); - write_action_files(); - write_service_files(); - write_cpp_file(); - write_eus_action_server(); - write_eus_condition_server(); - write_cmake_lists(); - write_package_xml(); + 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_cmake_lists(message_packages, service_files, action_files); + write_package_xml(message_packages); } } // namespaceRoseusBT 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..82479c42f --- /dev/null +++ b/roseus_bt/include/roseus_bt/pkg_template.h @@ -0,0 +1,227 @@ +#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 3.0.2) +project(%1%) + +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + behaviortree_ros + roseus_bt +%2% +) + +add_service_files( + FILES +%3% +) + +add_action_files( + FILES +%4% +) + +generate_messages( + DEPENDENCIES +%2% +) + +catkin_package( + INCLUDE_DIRS + LIBRARIES + CATKIN_DEPENDS + message_runtime +%2% +) + + +include_directories(${catkin_INCLUDE_DIRS}) + +%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}) + +)"; + + 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/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index cf1073be8..9a81cf842 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -4,11 +4,16 @@ #include #include "gen_template.h" +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 { @@ -45,13 +50,11 @@ class XMLParser std::string generate_cpp_file(const std::string package_name, const std::string roscpp_node_name, const std::string xml_filename); - std::string generate_eus_action_server(const std::string action_name); - std::string generate_eus_condition_server(const std::string action_name); - std::string generate_cmake_lists(const std::string package_name, - const std::string target_filename); - std::string generate_package_xml(const std::string package_name, - const std::string author_name); - + std::string generate_eus_action_server(const std::string package_name); + std::string generate_eus_condition_server(const std::string package_name); + void push_dependencies(std::vector* message_packages, + std::vector* action_files, + std::vector* service_files); }; @@ -84,10 +87,7 @@ void XMLParser::maybe_push_message_package(const XMLElement* node, std::size_t pos = msg_type.find('/'); if (pos != std::string::npos) { std::string pkg = msg_type.substr(0, pos); - if (std::find(message_packages->begin(), message_packages->end(), pkg) == - message_packages->end()) { - message_packages->push_back(pkg); - } + push_new(pkg, message_packages); } }; @@ -580,11 +580,9 @@ std::string XMLParser::generate_cpp_file(const std::string package_name, return output; } -std::string XMLParser::generate_cmake_lists(const std::string package_name, - const std::string target_name) { - auto format_pkg = [](std::string pkg) { - return fmt::format(" {}", pkg); - }; +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")); }; @@ -593,23 +591,17 @@ std::string XMLParser::generate_cmake_lists(const std::string package_name, }; const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); - 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 (auto action_node = root->FirstChildElement("Action"); action_node != nullptr; action_node = action_node->NextSiblingElement("Action")) { - action_files.push_back(format_action_file(action_node)); + push_new(format_action_file(action_node), action_files); for (auto port_node = action_node->FirstChildElement(); port_node != nullptr; port_node = port_node->NextSiblingElement()) { - maybe_push_message_package(port_node, &message_packages); + maybe_push_message_package(port_node, message_packages); } } @@ -617,12 +609,12 @@ std::string XMLParser::generate_cmake_lists(const std::string package_name, condition_node != nullptr; condition_node = condition_node->NextSiblingElement("Condition")) { - service_files.push_back(format_service_file(condition_node)); + push_new(format_service_file(condition_node), service_files); for (auto port_node = condition_node->FirstChildElement(); port_node != nullptr; port_node = port_node->NextSiblingElement()) { - maybe_push_message_package(port_node, &message_packages); + maybe_push_message_package(port_node, message_packages); } } @@ -630,59 +622,10 @@ std::string XMLParser::generate_cmake_lists(const std::string package_name, subscriber_node != nullptr; subscriber_node = subscriber_node->NextSiblingElement("Subscriber")) { - maybe_push_message_package(subscriber_node, &message_packages); + maybe_push_message_package(subscriber_node, message_packages); } - - std::transform(message_packages.begin(), message_packages.end(), - message_packages.begin(), format_pkg); - - return gen_template.cmake_lists_template(package_name, target_name, - message_packages, - service_files, - action_files); } -std::string XMLParser::generate_package_xml(const std::string package_name, - const std::string author_name) { - auto format_build_depend = [](std::string pkg) { - return fmt::format(" {}", pkg); - }; - auto format_exec_depend = [](std::string pkg) { - return fmt::format(" {}", pkg); - }; - - const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); - std::vector message_packages; - - message_packages.push_back("std_msgs"); - message_packages.push_back("actionlib_msgs"); - - for (auto node = root->FirstChildElement(); - node != nullptr; - node = node->NextSiblingElement()) - { - for (auto port_node = node->FirstChildElement(); - port_node != nullptr; - port_node = port_node->NextSiblingElement()) - { - maybe_push_message_package(port_node, &message_packages); - } - } - - 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 gen_template.package_xml_template(package_name, author_name, - build_dependencies, - exec_dependencies); -} } // namespace RoseusBT diff --git a/roseus_bt/src/create_bt_package.cpp b/roseus_bt/src/create_bt_package.cpp index ecccdfa83..8f3346ef3 100644 --- a/roseus_bt/src/create_bt_package.cpp +++ b/roseus_bt/src/create_bt_package.cpp @@ -7,21 +7,23 @@ namespace po = boost::program_options; int main(int argc, char** argv) { - std::string package_name, model_file, author, executable, node_name; + 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_file), "model file") - ("executable,e", po::value(&executable), "executable name (defaults to model filename)") + ("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"); po::positional_options_description positional_arguments; positional_arguments.add("package_name", 1); - positional_arguments.add("model_file", 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); @@ -35,17 +37,26 @@ int main(int argc, char** argv) } // Wrong usage - if (package_name.empty() || model_file.empty()) { + if (package_name.empty()) { std::cout << desc << std::endl; return 1; } - // Fill default executable name - if (executable.empty()) { - executable = boost::filesystem::path(model_file).stem().string(); + // 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_file, executable, author, + RoseusBT::PackageGenerator pg(package_name, model_files, executable_names, author, args.count("overwrite")); pg.write_all_files(); From 9d37b221e155073db3e2cbbf43577c5d580be213 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 6 Jul 2021 20:04:19 +0900 Subject: [PATCH 057/176] (roseus_bt) Add euslisp sample files --- roseus_bt/sample/sample-robot-utils.l | 36 ++++++++++++ roseus_bt/sample/sample-room.l | 32 ++++++++++ roseus_bt/sample/sample-task.l | 84 +++++++++++++++++++++++++++ 3 files changed, 152 insertions(+) create mode 100644 roseus_bt/sample/sample-robot-utils.l create mode 100644 roseus_bt/sample/sample-room.l create mode 100644 roseus_bt/sample/sample-task.l 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..ce90417a9 --- /dev/null +++ b/roseus_bt/sample/sample-task.l @@ -0,0 +1,84 @@ +(load "sample-robot-utils.l") +(load "sample-room.l") + + +;; Initialize *room* and *robot* +(defun init (&optional (broom t) (start-viewer t)) + (defparameter *room* (instance sample-room-scene :init :broom broom)) + (defparameter *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)) + + +;; Pour task +(defun pick-sushi-bottle () + (let ((petbottle-obj (send *room* :object "petbottle")) + (sushi-cup-obj (send *room* :object "sushi-cup"))) + (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"))) + (send *robot* :dual-arm-ik (send broom-obj :handle) :debug-view t) + + (dotimes (i 125) + (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)))) From 41fe61b6bc01f5f725fb00ab533d91d18e011dca Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 19:44:04 +0900 Subject: [PATCH 058/176] (roseus_bt) Update euslisp sample task --- roseus_bt/sample/sample-task.l | 31 +++++++++++++++++++++---------- 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/roseus_bt/sample/sample-task.l b/roseus_bt/sample/sample-task.l index ce90417a9..39d2ad013 100644 --- a/roseus_bt/sample/sample-task.l +++ b/roseus_bt/sample/sample-task.l @@ -4,11 +4,12 @@ ;; Initialize *room* and *robot* (defun init (&optional (broom t) (start-viewer t)) - (defparameter *room* (instance sample-room-scene :init :broom broom)) - (defparameter *robot* (instance sample-robot :init)) - (send *robot* :reset-pose) - (send *robot* :fix-leg-to-coords (make-coords)) - (send *robot* :update-descendants) + (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*)))) @@ -36,11 +37,19 @@ (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 () +(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 @@ -73,12 +82,14 @@ ;; Sweep task -(defun sweep-floor () - (let ((broom-obj (send *room* :object "broom"))) +(defun sweep-floor (&optional (check-fn #'(lambda () t))) + (let ((broom-obj (send *room* :object "broom")) + (i 0)) (send *robot* :dual-arm-ik (send broom-obj :handle) :debug-view t) - (dotimes (i 125) + (while (funcall check-fn) (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)))) + (send *irtviewer* :draw-objects) + (incf i)))) From fda9fe26edb40071e56e55d208ab8e91d98ccd20 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 19:44:27 +0900 Subject: [PATCH 059/176] (roseus_bt) Add sample model files --- roseus_bt/sample/models/t01_simple_tree.xml | 23 +++++++ roseus_bt/sample/models/t02_conditions.xml | 27 ++++++++ roseus_bt/sample/models/t03_ports.xml | 42 ++++++++++++ roseus_bt/sample/models/t04_subscriber.xml | 39 +++++++++++ roseus_bt/sample/models/t05_subtrees.xml | 64 ++++++++++++++++++ roseus_bt/sample/models/t06_reactive.xml | 72 +++++++++++++++++++++ 6 files changed, 267 insertions(+) create mode 100644 roseus_bt/sample/models/t01_simple_tree.xml create mode 100644 roseus_bt/sample/models/t02_conditions.xml create mode 100644 roseus_bt/sample/models/t03_ports.xml create mode 100644 roseus_bt/sample/models/t04_subscriber.xml create mode 100644 roseus_bt/sample/models/t05_subtrees.xml create mode 100644 roseus_bt/sample/models/t06_reactive.xml 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..c82d734f0 --- /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..fb583ddcb --- /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..ab1fd8447 --- /dev/null +++ b/roseus_bt/sample/models/t03_ports.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/models/t04_subscriber.xml b/roseus_bt/sample/models/t04_subscriber.xml new file mode 100644 index 000000000..a51ae1449 --- /dev/null +++ b/roseus_bt/sample/models/t04_subscriber.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/models/t05_subtrees.xml b/roseus_bt/sample/models/t05_subtrees.xml new file mode 100644 index 000000000..85459abf2 --- /dev/null +++ b/roseus_bt/sample/models/t05_subtrees.xml @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/sample/models/t06_reactive.xml b/roseus_bt/sample/models/t06_reactive.xml new file mode 100644 index 000000000..0f8e42328 --- /dev/null +++ b/roseus_bt/sample/models/t06_reactive.xml @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 0de44689e4a6e1e016bfc65c036cee053df3b132 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 20:58:17 +0900 Subject: [PATCH 060/176] (roseus_bt) Suppress return value on eus_condition_server template --- roseus_bt/include/roseus_bt/xml_parser.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 9a81cf842..43b2ec23d 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -438,7 +438,7 @@ std::string XMLParser::generate_eus_condition_server(const std::string package_n std::string fmt_string = 1 + R"( (roseus_bt:define-condition-callback {0}-cb{1} ({2}) ;; do something - t) + ) )"; std::vector param_list; for (auto port_node = node->FirstChildElement(); From c7d896fe032eb591230c9214b4c7548754bca823 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:01:02 +0900 Subject: [PATCH 061/176] (roseus_bt) Avoid extra newline after comment on eus_action_server template --- roseus_bt/include/roseus_bt/xml_parser.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 43b2ec23d..aedc90930 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -354,8 +354,7 @@ std::string XMLParser::generate_eus_action_server(const std::string package_name std::string fmt_string = 1 + R"( (roseus_bt:define-action-callback {0}-execute-cb{1} ({2}) ;; do something -{3} - t) +{3} t) )"; std::vector param_list; std::vector output_list; @@ -372,6 +371,9 @@ std::string XMLParser::generate_eus_action_server(const std::string package_name } } + if (output_list.size() != 0) { + output_list.push_back(""); + } return fmt::format(fmt_string, format_eus_name(node->Attribute("ID")), suffix, From a4c147c0812927ba0215e06db119f63a60ee85fa Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:15:53 +0900 Subject: [PATCH 062/176] (roseus_bt) Fix bug on server_name/service_name formatting --- roseus_bt/include/roseus_bt/xml_parser.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index aedc90930..08532452a 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -385,7 +385,7 @@ std::string XMLParser::generate_eus_action_server(const std::string package_name std::string server_name) { std::string fmt_string = 1 + R"( (instance roseus_bt:action-node :init - "{3}" {0}::{1}Action + "{4}" {0}::{1}Action :execute-cb '{2}-execute-cb{3}) )"; return fmt::format(fmt_string, @@ -463,7 +463,7 @@ std::string XMLParser::generate_eus_condition_server(const std::string package_n std::string service_name) { std::string fmt_string = 1 + R"( (instance roseus_bt:condition-node :init - "{3}" {0}::{1} + "{4}" {0}::{1} :execute-cb #'{2}-cb{3}) )"; return fmt::format(fmt_string, From 68ec60f0481dfd1ef9bd20a3f93aa71c9e3f6d80 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:17:47 +0900 Subject: [PATCH 063/176] (roseus_bt) Search in all BehaviorTrees for server_name/service_name --- roseus_bt/include/roseus_bt/xml_parser.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 08532452a..020a6b57d 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -406,7 +406,11 @@ std::string XMLParser::generate_eus_action_server(const std::string package_name action_node = action_node->NextSiblingElement("Action")) { std::vector server_names; - collect_node_attribute(bt_root, action_node, "server_name", &server_names); + for (auto bt_node = bt_root; + bt_node != nullptr; + bt_node = bt_node->NextSiblingElement()) { + collect_node_attribute(bt_node, action_node, "server_name", &server_names); + } if (server_names.empty()) { callback_definition.push_back(format_callback(action_node, "")); @@ -484,7 +488,11 @@ std::string XMLParser::generate_eus_condition_server(const std::string package_n condition_node = condition_node->NextSiblingElement("Condition")) { std::vector server_names; - collect_node_attribute(bt_root, condition_node, "service_name", &server_names); + for (auto bt_node = bt_root; + bt_node != nullptr; + bt_node = bt_node->NextSiblingElement()) { + collect_node_attribute(bt_node, condition_node, "service_name", &server_names); + } if (server_names.empty()) { callback_definition.push_back(format_callback(condition_node, "")); From e0db84cf24a1444e7a09b63e5093b9c1d4c3e7fa Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:32:52 +0900 Subject: [PATCH 064/176] (roseus_bt) Update target xml_filename location --- roseus_bt/include/roseus_bt/package_generator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 5e4d4ed10..c55cd8ac3 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -110,7 +110,7 @@ void PackageGenerator::copy_xml_file(std::string* xml_filename) { 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; + *xml_filename = dest_file; } void PackageGenerator::write_action_files(XMLParser* parser) { From fa3ecd906ceda22f2497f7316e73e021c588bdaf Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:33:34 +0900 Subject: [PATCH 065/176] (roseus_bt) Avoid extra newlines after registering on cpp main function --- roseus_bt/include/roseus_bt/gen_template.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 17ece8b1d..9560fd1d0 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -248,10 +248,7 @@ int main(int argc, char **argv) BehaviorTreeFactory factory; -%3% -%4% -%5% - +%3%%4%%5% %2% StdCoutLogger logger_cout(tree); @@ -271,6 +268,10 @@ int main(int argc, char **argv) } )"; + 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 bfmt = boost::format(fmt_string) % format_ros_init() % format_create_tree() % From 0d648211cef7db20b6f6380596a6990ae91e4150 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:45:37 +0900 Subject: [PATCH 066/176] (roseus_bt) Ensure valid package name --- roseus_bt/include/roseus_bt/package_generator.h | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index c55cd8ac3..7348ee6b8 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -43,7 +43,14 @@ class PackageGenerator 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 std::logic_error( + fmt::format("Package name {} does not follow naming conventions", package_name)); + } + }; ~PackageGenerator() {}; From 08118164ab9f582bbc6bf6223e452018624d30d3 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 13:55:23 +0900 Subject: [PATCH 067/176] (roseus_bt) Use different IDs when initializing demo task with broom object --- roseus_bt/sample/models/t05_subtrees.xml | 4 ++-- roseus_bt/sample/models/t06_reactive.xml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/roseus_bt/sample/models/t05_subtrees.xml b/roseus_bt/sample/models/t05_subtrees.xml index 85459abf2..f77eb73a5 100644 --- a/roseus_bt/sample/models/t05_subtrees.xml +++ b/roseus_bt/sample/models/t05_subtrees.xml @@ -3,7 +3,7 @@ - + @@ -40,7 +40,7 @@ - + diff --git a/roseus_bt/sample/models/t06_reactive.xml b/roseus_bt/sample/models/t06_reactive.xml index 0f8e42328..a848a7fa0 100644 --- a/roseus_bt/sample/models/t06_reactive.xml +++ b/roseus_bt/sample/models/t06_reactive.xml @@ -3,7 +3,7 @@ - + @@ -44,7 +44,7 @@ - + From d40eae9b8cc38765c385145ae87603c971c18ffe Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 14:03:23 +0900 Subject: [PATCH 068/176] (roseus_bt) Add create_bt_tutorials executable --- roseus_bt/CMakeLists.txt | 4 + roseus_bt/include/roseus_bt/gen_template.h | 18 +- .../include/roseus_bt/package_generator.h | 58 +-- roseus_bt/include/roseus_bt/tutorial_parser.h | 100 +++++ roseus_bt/include/roseus_bt/xml_parser.h | 374 ++++++++++-------- roseus_bt/src/create_bt_package.cpp | 6 +- roseus_bt/src/create_bt_tutorials.cpp | 58 +++ 7 files changed, 427 insertions(+), 191 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/tutorial_parser.h create mode 100644 roseus_bt/src/create_bt_tutorials.cpp diff --git a/roseus_bt/CMakeLists.txt b/roseus_bt/CMakeLists.txt index b5a1e4389..88b3f445b 100644 --- a/roseus_bt/CMakeLists.txt +++ b/roseus_bt/CMakeLists.txt @@ -24,3 +24,7 @@ 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} 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} fmt::fmt) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 9560fd1d0..7e7f7a472 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -38,7 +38,8 @@ class GenTemplate std::string eus_server_template(std::string server_type, std::string package_name, std::vector callbacks, - std::vector instances); + std::vector instances, + std::vector load_files); }; @@ -286,25 +287,31 @@ int main(int argc, char **argv) std::string GenTemplate::eus_server_template(std::string server_type, std::string package_name, std::vector callbacks, - std::vector instances) { + 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); std::string fmt_string = 1 + R"( %2% %3% %4% - +%5% ;; define %1% callbacks -%5% +%6% ;; create server instances -%6% +%7% ;; spin (roseus_bt:spin) @@ -315,6 +322,7 @@ std::string GenTemplate::eus_server_template(std::string server_type, 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"); diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 7348ee6b8..f91592baf 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -6,7 +6,6 @@ #include #include #include -#include #include @@ -27,6 +26,7 @@ class Query bool yn(const std::string message); }; +template class PackageGenerator { public: @@ -57,7 +57,7 @@ class PackageGenerator private: Query query; PkgTemplate pkg_template; - std::vector parser_vector; + std::vector parser_vector; std::string package_name; std::vector xml_filenames; std::vector target_filenames; @@ -69,12 +69,12 @@ class PackageGenerator public: void copy_xml_file(std::string* xml_filename); - void write_action_files(XMLParser* parser); - void write_service_files(XMLParser* parser); - void write_cpp_file(XMLParser* parser, + void write_action_files(Parser* parser); + void write_service_files(Parser* parser); + void write_cpp_file(Parser* parser, const std::string target_filename, const std::string xml_filename); - void write_eus_action_server(XMLParser* parser, const std::string target_filename); - void write_eus_condition_server(XMLParser* parser, const std::string target_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); @@ -98,11 +98,13 @@ bool Query::yn(const std::string message) { throw std::logic_error("Invalid input"); } -bool PackageGenerator::overwrite(const std::string filename) { +template +bool PackageGenerator::overwrite(const std::string filename) { return force_overwrite || query.yn(fmt::format("Overwrite {}?", filename)); } -void PackageGenerator::copy_xml_file(std::string* xml_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, @@ -120,7 +122,8 @@ void PackageGenerator::copy_xml_file(std::string* xml_filename) { *xml_filename = dest_file; } -void PackageGenerator::write_action_files(XMLParser* parser) { +template +void PackageGenerator::write_action_files(Parser* parser) { std::string base_dir = fmt::format("{}/action", package_name); boost::filesystem::create_directories(base_dir); @@ -133,7 +136,8 @@ void PackageGenerator::write_action_files(XMLParser* parser) { } } -void PackageGenerator::write_service_files(XMLParser* parser) { +template +void PackageGenerator::write_service_files(Parser* parser) { std::string base_dir = fmt::format("{}/srv", package_name); boost::filesystem::create_directories(base_dir); @@ -146,9 +150,10 @@ void PackageGenerator::write_service_files(XMLParser* parser) { } } -void PackageGenerator::write_cpp_file(XMLParser* parser, - const std::string target_filename, - const std::string xml_filename) { +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); @@ -164,8 +169,9 @@ void PackageGenerator::write_cpp_file(XMLParser* parser, output_file.close(); } -void PackageGenerator::write_eus_action_server(XMLParser* parser, - const std::string target_filename) { +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); @@ -181,8 +187,9 @@ void PackageGenerator::write_eus_action_server(XMLParser* parser, output_file.close(); } -void PackageGenerator::write_eus_condition_server(XMLParser* parser, - const std::string target_filename) { +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); @@ -198,9 +205,10 @@ void PackageGenerator::write_eus_condition_server(XMLParser* parser, output_file.close(); } -void PackageGenerator::write_cmake_lists(const std::vector message_packages, - const std::vector service_files, - const std::vector action_files) { +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); @@ -216,7 +224,8 @@ void PackageGenerator::write_cmake_lists(const std::vector message_ output_file.close(); } -void PackageGenerator::write_package_xml(const std::vector message_packages) { +template +void PackageGenerator::write_package_xml(const std::vector message_packages) { std::string base_dir = package_name; boost::filesystem::create_directories(base_dir); @@ -230,7 +239,8 @@ void PackageGenerator::write_package_xml(const std::vector message_ output_file.close(); } -void PackageGenerator::write_all_files() { +template +void PackageGenerator::write_all_files() { std::vector message_packages; std::vector action_files; std::vector service_files; @@ -239,7 +249,7 @@ void PackageGenerator::write_all_files() { message_packages.push_back("actionlib_msgs"); for (int i=0; i +#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) override; + +public: + virtual std::string generate_eus_action_server(const std::string package_name) override; + virtual std::string generate_eus_condition_server(const std::string package_name) override; +}; + + +std::string TutorialParser::format_node_body(const XMLElement* node) { + 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 " (send value :data)"; + if (id == "atTable") return " (at-spot \"table-front\")"; + if (id == "atSpot") return fmt::format(" (at-spot {})", param_list.at(0)); + + // Actions + if (id == "Init") return " (init nil t)"; + if (id == "InitWithBroom") return " (init t t)"; + if (id == "MoveToTable") return " (go-to-spot \"table-front\")"; + if (id == "PickBottle") return " (pick-sushi-bottle)"; + if (id == "PourBottle") return " (pour-sushi-bottle)"; + if (id == "PlaceBottle") return " (place-sushi-bottle)\n (reset-pose)"; + if (id == "SweepFloor") return " (sweep-floor #'roseus_bt:ok)\n (reset-pose)"; + + if (id == "MoveTo") + return fmt::format(" (go-to-spot {})", param_list.at(0)); + if (id == "PickBottleAt") + return fmt::format(" (pick-sushi-bottle (ros::tf-pose->coords {}))", param_list.at(0)); + if (id == "setCoords") { + std::string fmt_string = 1 + R"( + (roseus_bt:set-output + "{}" (ros::coords->tf-pose (make-coords :pos #f(1850 400 700)))))"; + return fmt::format(fmt_string, output_list.at(0)); + } + + throw std::logic_error(fmt::format("Unrecognized {} node with ID {}", node->Name(), id)); +} + +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); + + 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, &callback_definition, &instance_creation); + + 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/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 020a6b57d..7f89c9a81 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -1,8 +1,10 @@ #ifndef BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ #define BEHAVIOR_TREE_ROSEUS_BT_XML_PARSER_ +#include +#include #include -#include "gen_template.h" +#include void push_new(std::string elem, std::vector* vec) { if (std::find(vec->begin(), vec->end(), elem) == vec->end()) { @@ -31,6 +33,17 @@ class XMLParser GenTemplate gen_template; void collect_node_attribute(const XMLElement* node, const XMLElement* ref_node, const char* attribute, std::vector* node_attributes); + void collect_param_list(const XMLElement* node, std::vector* param_list, + std::vector* output_list, + std::function fn); + void collect_param_list(const XMLElement* node, std::vector* param_list, + std::vector* output_list); + 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); void maybe_push_message_package(const XMLElement* node, std::vector* message_packages); std::string format_eus_name(const std::string input); @@ -43,6 +56,8 @@ class XMLParser std::string generate_subscriber_class(const XMLElement* node); std::string generate_main_function(const std::string roscpp_node_name, const std::string xml_filename); + virtual std::string format_node_body(const XMLElement* node); + public: std::map generate_all_action_files(); @@ -50,11 +65,13 @@ class XMLParser std::string generate_cpp_file(const std::string package_name, const std::string roscpp_node_name, const std::string xml_filename); - std::string generate_eus_action_server(const std::string package_name); - std::string generate_eus_condition_server(const std::string package_name); void push_dependencies(std::vector* message_packages, std::vector* action_files, std::vector* service_files); + + virtual std::string generate_eus_action_server(const std::string package_name); + virtual std::string generate_eus_condition_server(const std::string package_name); + }; @@ -81,6 +98,178 @@ void XMLParser::collect_node_attribute(const XMLElement* node, } } +void XMLParser::collect_param_list(const XMLElement* node, + std::vector* param_list, + std::vector* output_list, + std::function fn) { + + 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") { + if (param_list != NULL) + param_list->push_back(port_node->Attribute("name")); + } + if (name == "output_port" || name == "inout_port") { + if (output_list != NULL) + output_list->push_back(fn(port_node)); + } + } +} + +void XMLParser::collect_param_list(const XMLElement* node, + std::vector* param_list, + std::vector* output_list) { + auto format_output_port = [](const XMLElement* port_node) { + return port_node->Attribute("name"); + }; + collect_param_list(node, param_list, output_list, format_output_port); +} + +void XMLParser::collect_eus_actions(const std::string package_name, + std::vector* callback_definition, + std::vector* instance_creation) { + + auto format_callback = [this](const XMLElement* node, std::string suffix) { + std::string fmt_string = 1 + R"( +(roseus_bt:define-action-callback {0}-execute-cb{1} ({2}) +{3} + t) +)"; + std::vector param_list; + collect_param_list(node, ¶m_list, NULL); + + return fmt::format(fmt_string, + format_eus_name(node->Attribute("ID")), + suffix, + boost::algorithm::join(param_list, " "), + format_node_body(node)); + }; + + auto format_instance = [this, package_name](const XMLElement* node, std::string suffix, + std::string server_name) { + std::string fmt_string = 1 + R"( +(instance roseus_bt:action-node :init + "{4}" {0}::{1}Action + :execute-cb '{2}-execute-cb{3}) +)"; + return fmt::format(fmt_string, + package_name, + node->Attribute("ID"), + format_eus_name(node->Attribute("ID")), + suffix, + server_name); + }; + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); + + for (auto action_node = root->FirstChildElement("Action"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("Action")) + { + std::vector server_names; + for (auto bt_node = bt_root; + bt_node != nullptr; + bt_node = bt_node->NextSiblingElement()) { + collect_node_attribute(bt_node, action_node, "server_name", &server_names); + } + + if (server_names.empty()) { + callback_definition->push_back(format_callback(action_node, "")); + instance_creation->push_back(format_instance(action_node, "", + action_node->Attribute("ID"))); + } + else if (server_names.size() == 1) { + callback_definition->push_back(format_callback(action_node, "")); + instance_creation->push_back(format_instance(action_node, "", + server_names.at(0))); + } + else { + int suffix_num = 1; + for (std::vector::const_iterator it = server_names.begin(); + it != server_names.end(); ++it, suffix_num++) { + std::string suffix = fmt::format("-{}", suffix_num); + callback_definition->push_back(format_callback(action_node, suffix)); + instance_creation->push_back(format_instance(action_node, suffix, *it)); + } + } + } +} + +void XMLParser::collect_eus_conditions(const std::string package_name, + std::vector* callback_definition, + std::vector* instance_creation) { + + auto format_callback = [this](const XMLElement* node, std::string suffix) { + std::string fmt_string = 1 + R"( +(roseus_bt:define-condition-callback {0}-cb{1} ({2}) +{3} + ) +)"; + std::vector param_list; + collect_param_list(node, ¶m_list, NULL); + + return fmt::format(fmt_string, + format_eus_name(node->Attribute("ID")), + suffix, + boost::algorithm::join(param_list, " "), + format_node_body(node)); + }; + + auto format_instance = [this, package_name](const XMLElement* node, std::string suffix, + std::string service_name) { + std::string fmt_string = 1 + R"( +(instance roseus_bt:condition-node :init + "{4}" {0}::{1} + :execute-cb #'{2}-cb{3}) +)"; + return fmt::format(fmt_string, + package_name, + node->Attribute("ID"), + format_eus_name(node->Attribute("ID")), + suffix, + service_name); + }; + + const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); + + for (auto condition_node = root->FirstChildElement("Condition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("Condition")) + { + std::vector server_names; + for (auto bt_node = bt_root; + bt_node != nullptr; + bt_node = bt_node->NextSiblingElement()) { + collect_node_attribute(bt_node, condition_node, "service_name", &server_names); + } + + if (server_names.empty()) { + callback_definition->push_back(format_callback(condition_node, "")); + instance_creation->push_back(format_instance(condition_node, "", + condition_node->Attribute("ID"))); + } + else if (server_names.size() == 1) { + callback_definition->push_back(format_callback(condition_node, "")); + instance_creation->push_back(format_instance(condition_node, "", + server_names.at(0))); + } + else { + int suffix_num = 1; + for (std::vector::const_iterator it = server_names.begin(); + it != server_names.end(); ++it, suffix_num++) { + std::string suffix = fmt::format("-{}", suffix_num); + callback_definition->push_back(format_callback(condition_node, suffix)); + instance_creation->push_back(format_instance(condition_node, suffix, *it)); + } + } + } +} + void XMLParser::maybe_push_message_package(const XMLElement* node, std::vector* message_packages) { std::string msg_type = node->Attribute("type"); @@ -99,6 +288,20 @@ std::string XMLParser::format_eus_name(const std::string input) { return out; }; +std::string XMLParser::format_node_body(const XMLElement* node) { + auto format_setoutput = [](const XMLElement* port_node) { + return fmt::format(" ;; (roseus_bt:set-output \"{0}\" <{1}>)", + port_node->Attribute("name"), + port_node->Attribute("type")); + }; + + std::vector output_list; + output_list.push_back(" ;; do something"); + collect_param_list(node, NULL, &output_list, format_setoutput); + + return boost::algorithm::join(output_list, "\n"); +} + std::string XMLParser::port_node_to_message_description(const XMLElement* port_node) { if (!port_node->Attribute("type") || !port_node->Attribute("name")) { @@ -344,181 +547,32 @@ std::string XMLParser::generate_main_function(const std::string roscpp_node_name } std::string XMLParser::generate_eus_action_server(const std::string package_name) { - auto format_setoutput = [](const XMLElement* node) { - return fmt::format(" ;; (roseus_bt:set-output \"{0}\" <{1}>)", - node->Attribute("name"), - node->Attribute("type")); - }; - auto format_callback = [this, format_setoutput](const XMLElement* node, std::string suffix) { - std::string fmt_string = 1 + R"( -(roseus_bt:define-action-callback {0}-execute-cb{1} ({2}) - ;; do something -{3} t) -)"; - std::vector param_list; - std::vector output_list; - 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") { - param_list.push_back(port_node->Attribute("name")); - } - if (name == "output_port" || name == "inout_port") { - output_list.push_back(format_setoutput(port_node)); - } - } - - if (output_list.size() != 0) { - output_list.push_back(""); - } - return fmt::format(fmt_string, - format_eus_name(node->Attribute("ID")), - suffix, - boost::algorithm::join(param_list, " "), - boost::algorithm::join(output_list, "\n")); - }; - - auto format_instance = [this, package_name](const XMLElement* node, std::string suffix, - std::string server_name) { - std::string fmt_string = 1 + R"( -(instance roseus_bt:action-node :init - "{4}" {0}::{1}Action - :execute-cb '{2}-execute-cb{3}) -)"; - return fmt::format(fmt_string, - package_name, - node->Attribute("ID"), - format_eus_name(node->Attribute("ID")), - suffix, - server_name); - }; - - const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); - const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); std::vector callback_definition; std::vector instance_creation; + std::vector load_files; - for (auto action_node = root->FirstChildElement("Action"); - action_node != nullptr; - action_node = action_node->NextSiblingElement("Action")) - { - std::vector server_names; - for (auto bt_node = bt_root; - bt_node != nullptr; - bt_node = bt_node->NextSiblingElement()) { - collect_node_attribute(bt_node, action_node, "server_name", &server_names); - } - - if (server_names.empty()) { - callback_definition.push_back(format_callback(action_node, "")); - instance_creation.push_back(format_instance(action_node, "", - action_node->Attribute("ID"))); - } - else if (server_names.size() == 1) { - callback_definition.push_back(format_callback(action_node, "")); - instance_creation.push_back(format_instance(action_node, "", - server_names.at(0))); - } - else { - int suffix_num = 1; - for (std::vector::const_iterator it = server_names.begin(); - it != server_names.end(); ++it, suffix_num++) { - std::string suffix = fmt::format("-{}", suffix_num); - callback_definition.push_back(format_callback(action_node, suffix)); - instance_creation.push_back(format_instance(action_node, suffix, *it)); - } - } - } + collect_eus_actions(package_name, &callback_definition, &instance_creation); if (callback_definition.empty()) return ""; return gen_template.eus_server_template("action", package_name, - callback_definition, instance_creation); + callback_definition, instance_creation, + load_files); } std::string XMLParser::generate_eus_condition_server(const std::string package_name) { - auto format_callback = [this](const XMLElement* node, std::string suffix) { - std::string fmt_string = 1 + R"( -(roseus_bt:define-condition-callback {0}-cb{1} ({2}) - ;; do something - ) -)"; - std::vector param_list; - 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") { - param_list.push_back(port_node->Attribute("name")); - } - } - - return fmt::format(fmt_string, - format_eus_name(node->Attribute("ID")), - suffix, - boost::algorithm::join(param_list, " ")); - }; - - auto format_instance = [this, package_name](const XMLElement* node, std::string suffix, - std::string service_name) { - std::string fmt_string = 1 + R"( -(instance roseus_bt:condition-node :init - "{4}" {0}::{1} - :execute-cb #'{2}-cb{3}) -)"; - return fmt::format(fmt_string, - package_name, - node->Attribute("ID"), - format_eus_name(node->Attribute("ID")), - suffix, - service_name); - }; - - const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); - const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); std::vector callback_definition; std::vector instance_creation; + std::vector load_files; - for (auto condition_node = root->FirstChildElement("Condition"); - condition_node != nullptr; - condition_node = condition_node->NextSiblingElement("Condition")) - { - std::vector server_names; - for (auto bt_node = bt_root; - bt_node != nullptr; - bt_node = bt_node->NextSiblingElement()) { - collect_node_attribute(bt_node, condition_node, "service_name", &server_names); - } - - if (server_names.empty()) { - callback_definition.push_back(format_callback(condition_node, "")); - instance_creation.push_back(format_instance(condition_node, "", - condition_node->Attribute("ID"))); - } - else if (server_names.size() == 1) { - callback_definition.push_back(format_callback(condition_node, "")); - instance_creation.push_back(format_instance(condition_node, "", - server_names.at(0))); - } - else { - int suffix_num = 1; - for (std::vector::const_iterator it = server_names.begin(); - it != server_names.end(); ++it, suffix_num++) { - std::string suffix = fmt::format("-{}", suffix_num); - callback_definition.push_back(format_callback(condition_node, suffix)); - instance_creation.push_back(format_instance(condition_node, suffix, *it)); - } - } - } + collect_eus_conditions(package_name, &callback_definition, &instance_creation); if (callback_definition.empty()) return ""; return gen_template.eus_server_template("condition", package_name, - callback_definition, instance_creation); + callback_definition, instance_creation, + load_files); } std::map XMLParser::generate_all_action_files() { diff --git a/roseus_bt/src/create_bt_package.cpp b/roseus_bt/src/create_bt_package.cpp index 8f3346ef3..9e91d10ef 100644 --- a/roseus_bt/src/create_bt_package.cpp +++ b/roseus_bt/src/create_bt_package.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -56,8 +57,9 @@ int main(int argc, char** argv) *exec_it = boost::filesystem::path(model_file).stem().string(); } - RoseusBT::PackageGenerator pg(package_name, model_files, executable_names, author, - args.count("overwrite")); + 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..30fd2fa40 --- /dev/null +++ b/roseus_bt/src/create_bt_tutorials.cpp @@ -0,0 +1,58 @@ +#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"); + + po::variables_map args; + po::store(po::parse_command_line(argc, argv, desc), args); + po::notify(args); + + // Help + if (args.count("help")) { + std::cout << "\n" << "Create behavior tree package." << "\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"); + + // 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; +} From 1bf6da108bbe59304658b643b8ab6fb22e3fabb7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 14:03:36 +0900 Subject: [PATCH 069/176] (roseus_bt) Rename function port_node_to_message_description -> format_message_description --- roseus_bt/include/roseus_bt/xml_parser.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 7f89c9a81..d576a023a 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -47,7 +47,7 @@ class XMLParser void maybe_push_message_package(const XMLElement* node, std::vector* message_packages); std::string format_eus_name(const std::string input); - std::string port_node_to_message_description(const XMLElement* port_node); + std::string format_message_description(const XMLElement* port_node); 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); @@ -302,7 +302,7 @@ std::string XMLParser::format_node_body(const XMLElement* node) { return boost::algorithm::join(output_list, "\n"); } -std::string XMLParser::port_node_to_message_description(const XMLElement* port_node) { +std::string XMLParser::format_message_description(const XMLElement* port_node) { if (!port_node->Attribute("type") || !port_node->Attribute("name")) { std::string error_str = "Illformed port in "; @@ -324,7 +324,7 @@ std::string XMLParser::generate_action_file_contents(const XMLElement* node) { port_node = port_node->NextSiblingElement()) { std::string name = port_node->Name(); - std::string text = port_node_to_message_description(port_node); + std::string text = format_message_description(port_node); if (name == "input_port" || name == "inout_port") { goal.push_back(text); @@ -345,7 +345,7 @@ std::string XMLParser::generate_service_file_contents(const XMLElement* node) { port_node = port_node->NextSiblingElement()) { std::string name = port_node->Name(); - std::string text = port_node_to_message_description(port_node); + std::string text = format_message_description(port_node); if (name == "input_port") { request.push_back(text); From 4c9b66b03ebd20b26a8fd7b5a4d3f6076668d7f6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 14:04:00 +0900 Subject: [PATCH 070/176] (roseus_bt) Add load guard in package_generator.h --- roseus_bt/include/roseus_bt/package_generator.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index f91592baf..2be02f833 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -1,3 +1,6 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_PACKAGE_GENERATOR_ +#define BEHAVIOR_TREE_ROSEUS_BT_PACKAGE_GENERATOR_ + #include #include #include @@ -266,4 +269,7 @@ void PackageGenerator::write_all_files() { write_package_xml(message_packages); } + } // namespaceRoseusBT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_PACKAGE_GENERATOR_ From 36ac45956973c97138ff9c56994c2b40cbd794ec Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 19:17:22 +0900 Subject: [PATCH 071/176] (roseus_bt) Only split euslisp server files for reactive conditions --- roseus_bt/include/roseus_bt/gen_template.h | 9 +- roseus_bt/include/roseus_bt/tutorial_parser.h | 5 +- roseus_bt/include/roseus_bt/xml_parser.h | 148 ++++++++++++------ 3 files changed, 105 insertions(+), 57 deletions(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 7e7f7a472..aee80c105 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -302,23 +302,22 @@ std::string GenTemplate::eus_server_template(std::string server_type, std::transform(load_files.begin(), load_files.end(), load_files.begin(), format_load_file); std::string fmt_string = 1 + R"( +%1% %2% %3% %4% -%5% -;; define %1% callbacks -%6% +;; define callbacks +%5% ;; create server instances -%7% +%6% ;; spin (roseus_bt:spin) )"; boost::format bfmt = boost::format(fmt_string) % - server_type % format_ros_roseus() % format_load_ros_package() % "(load \"package://roseus_bt/euslisp/nodes.l\")" % diff --git a/roseus_bt/include/roseus_bt/tutorial_parser.h b/roseus_bt/include/roseus_bt/tutorial_parser.h index 48cf485a9..ec4f28dce 100644 --- a/roseus_bt/include/roseus_bt/tutorial_parser.h +++ b/roseus_bt/include/roseus_bt/tutorial_parser.h @@ -69,6 +69,8 @@ std::string TutorialParser::generate_eus_action_server(const std::string package 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 ""; @@ -85,7 +87,8 @@ std::string TutorialParser::generate_eus_condition_server(const std::string pack // Add load files load_files.push_back("package://roseus_bt/sample/sample-task.l"); - collect_eus_conditions(package_name, &callback_definition, &instance_creation); + collect_eus_conditions(package_name, NULL, NULL, + &callback_definition, &instance_creation); if (callback_definition.empty()) return ""; diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index d576a023a..9f3753901 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -32,7 +32,10 @@ class XMLParser XMLDocument doc; GenTemplate gen_template; void collect_node_attribute(const XMLElement* node, const XMLElement* ref_node, - const char* attribute, std::vector* node_attributes); + const char* attribute, + std::vector* node_attributes, + std::vector* parallel_node_attributes, + bool is_parallel); void collect_param_list(const XMLElement* node, std::vector* param_list, std::vector* output_list, std::function fn); @@ -43,7 +46,15 @@ class XMLParser std::vector* instance_creation); void collect_eus_conditions(const std::string package_name, std::vector* callback_definition, - std::vector* instance_creation); + std::vector* instance_creation, + std::vector* parallel_callback_definition, + std::vector* parallel_instance_creation); + void push_eus_server(const XMLElement* node, + std::vector server_names, + std::vector* callback_definition, + std::vector* instance_creation, + std::function format_callback, + std::function format_instance); void maybe_push_message_package(const XMLElement* node, std::vector* message_packages); std::string format_eus_name(const std::string input); @@ -78,14 +89,36 @@ class XMLParser void XMLParser::collect_node_attribute(const XMLElement* node, const XMLElement* ref_node, const char* attribute, - std::vector* node_attributes) { + std::vector* node_attributes, + std::vector* parallel_node_attributes, + bool is_parallel) { + std::string name = node->Name(); + + // is possibly reactive control node + if (name.find("Fallback") != std::string::npos || + name.find("Sequence") != std::string::npos) { + is_parallel = (name.find("Reactive") != std::string::npos); + } + // node name and ID matches if (!std::strcmp(node->Name(), ref_node->Name()) && !std::strcmp(node->Attribute("ID"), ref_node->Attribute("ID"))) { - if (node->Attribute(attribute) && - std::find(node_attributes->begin(), node_attributes->end(), + + // check if the node has the given attribute + if (!node->Attribute(attribute)) { + XMLPrinter printer; + node->Accept(&printer); + throw std::logic_error(fmt::format("{} is required in {} nodes! At {}", + attribute, name, printer.CStr())); + } + + // push if the attribute is unique to the list + if (std::find(node_attributes->begin(), node_attributes->end(), node->Attribute(attribute)) == node_attributes->end()) { - node_attributes->push_back(node->Attribute(attribute)); + if (is_parallel) + parallel_node_attributes->push_back(node->Attribute(attribute)); + else + node_attributes->push_back(node->Attribute(attribute)); } return; } @@ -94,7 +127,9 @@ void XMLParser::collect_node_attribute(const XMLElement* node, child_node != nullptr; child_node = child_node->NextSiblingElement()) { - collect_node_attribute(child_node, ref_node, attribute, node_attributes); + collect_node_attribute(child_node, ref_node, attribute, + node_attributes, parallel_node_attributes, + is_parallel); } } @@ -173,35 +208,22 @@ void XMLParser::collect_eus_actions(const std::string package_name, std::vector server_names; for (auto bt_node = bt_root; bt_node != nullptr; - bt_node = bt_node->NextSiblingElement()) { - collect_node_attribute(bt_node, action_node, "server_name", &server_names); + bt_node = bt_node->NextSiblingElement("BehaviorTree")) { + collect_node_attribute(bt_node, action_node, "server_name", &server_names, &server_names, + false); } - if (server_names.empty()) { - callback_definition->push_back(format_callback(action_node, "")); - instance_creation->push_back(format_instance(action_node, "", - action_node->Attribute("ID"))); - } - else if (server_names.size() == 1) { - callback_definition->push_back(format_callback(action_node, "")); - instance_creation->push_back(format_instance(action_node, "", - server_names.at(0))); - } - else { - int suffix_num = 1; - for (std::vector::const_iterator it = server_names.begin(); - it != server_names.end(); ++it, suffix_num++) { - std::string suffix = fmt::format("-{}", suffix_num); - callback_definition->push_back(format_callback(action_node, suffix)); - instance_creation->push_back(format_instance(action_node, suffix, *it)); - } - } + push_eus_server(action_node, server_names, + callback_definition, instance_creation, + format_callback, format_instance); } } void XMLParser::collect_eus_conditions(const std::string package_name, std::vector* callback_definition, - std::vector* instance_creation) { + std::vector* instance_creation, + std::vector* parallel_callback_definition, + std::vector* parallel_instance_creation) { auto format_callback = [this](const XMLElement* node, std::string suffix) { std::string fmt_string = 1 + R"( @@ -242,34 +264,55 @@ void XMLParser::collect_eus_conditions(const std::string package_name, condition_node = condition_node->NextSiblingElement("Condition")) { std::vector server_names; + std::vector parallel_server_names; for (auto bt_node = bt_root; bt_node != nullptr; - bt_node = bt_node->NextSiblingElement()) { - collect_node_attribute(bt_node, condition_node, "service_name", &server_names); + bt_node = bt_node->NextSiblingElement("BehaviorTree")) { + collect_node_attribute(bt_node, condition_node, "service_name", + &server_names, ¶llel_server_names, + false); } + push_eus_server(condition_node, server_names, + callback_definition, instance_creation, + format_callback, format_instance); + push_eus_server(condition_node, parallel_server_names, + parallel_callback_definition, parallel_instance_creation, + format_callback, format_instance); + } +} - if (server_names.empty()) { - callback_definition->push_back(format_callback(condition_node, "")); - instance_creation->push_back(format_instance(condition_node, "", - condition_node->Attribute("ID"))); - } - else if (server_names.size() == 1) { - callback_definition->push_back(format_callback(condition_node, "")); - instance_creation->push_back(format_instance(condition_node, "", - server_names.at(0))); - } - else { - int suffix_num = 1; - for (std::vector::const_iterator it = server_names.begin(); - it != server_names.end(); ++it, suffix_num++) { - std::string suffix = fmt::format("-{}", suffix_num); - callback_definition->push_back(format_callback(condition_node, suffix)); - instance_creation->push_back(format_instance(condition_node, suffix, *it)); - } - } +void XMLParser::push_eus_server( + const XMLElement* node, + std::vector server_names, + std::vector* callback_definition, + std::vector* instance_creation, + std::function format_callback, + std::function format_instance) +{ + if (callback_definition == NULL || instance_creation == NULL) { + return; + } + + if (server_names.empty()) { + return; + } + + if (server_names.size() == 1) { + callback_definition->push_back(format_callback(node, "")); + instance_creation->push_back(format_instance(node, "", server_names.at(0))); + } + else { + int suffix_num = 1; + for (std::vector::const_iterator it = server_names.begin(); + it != server_names.end(); ++it, suffix_num++) { + std::string suffix = fmt::format("-{}", suffix_num); + callback_definition->push_back(format_callback(node, suffix)); + instance_creation->push_back(format_instance(node, suffix, *it)); } + } } + void XMLParser::maybe_push_message_package(const XMLElement* node, std::vector* message_packages) { std::string msg_type = node->Attribute("type"); @@ -553,6 +596,8 @@ std::string XMLParser::generate_eus_action_server(const std::string package_name 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 ""; @@ -566,7 +611,8 @@ std::string XMLParser::generate_eus_condition_server(const std::string package_n std::vector instance_creation; std::vector load_files; - collect_eus_conditions(package_name, &callback_definition, &instance_creation); + collect_eus_conditions(package_name, NULL, NULL, + &callback_definition, &instance_creation); if (callback_definition.empty()) return ""; From 516eb3d90c5e83b2a1c934ff8edcaacac48d7639 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 19:20:57 +0900 Subject: [PATCH 072/176] (roseus_bt) Ensure newline after load_files in eus_server_template --- roseus_bt/include/roseus_bt/gen_template.h | 1 + 1 file changed, 1 insertion(+) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index aee80c105..f8dcfb51e 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -300,6 +300,7 @@ std::string GenTemplate::eus_server_template(std::string server_type, }; 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% From c05bafbe68481dea8869a058ef930e540d5bcd54 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 20:41:08 +0900 Subject: [PATCH 073/176] (roseus_bt) Wait for topic message on t04_subscriber --- roseus_bt/include/roseus_bt/tutorial_parser.h | 2 ++ roseus_bt/sample/models/t04_subscriber.xml | 13 +++++++++++++ 2 files changed, 15 insertions(+) diff --git a/roseus_bt/include/roseus_bt/tutorial_parser.h b/roseus_bt/include/roseus_bt/tutorial_parser.h index ec4f28dce..5d0527e60 100644 --- a/roseus_bt/include/roseus_bt/tutorial_parser.h +++ b/roseus_bt/include/roseus_bt/tutorial_parser.h @@ -35,6 +35,8 @@ std::string TutorialParser::format_node_body(const XMLElement* node) { if (id == "CheckTrue") return " (send value :data)"; if (id == "atTable") return " (at-spot \"table-front\")"; if (id == "atSpot") return fmt::format(" (at-spot {})", param_list.at(0)); + if (id == "CheckCoords") return fmt::format( + " (not (equal (instance geometry_msgs::Pose :init) {}))", param_list.at(0)); // Actions if (id == "Init") return " (init nil t)"; diff --git a/roseus_bt/sample/models/t04_subscriber.xml b/roseus_bt/sample/models/t04_subscriber.xml index a51ae1449..908d9b283 100644 --- a/roseus_bt/sample/models/t04_subscriber.xml +++ b/roseus_bt/sample/models/t04_subscriber.xml @@ -11,6 +11,16 @@ + + + + + + + + @@ -24,6 +34,9 @@ + + + From 2937e12bfbcac0241bd679e75441c5a60b1b146f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 22:01:38 +0900 Subject: [PATCH 074/176] (roseus_bt) Add roseus_bt.rosinstall --- roseus_bt/roseus_bt.rosinstall | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 roseus_bt/roseus_bt.rosinstall diff --git a/roseus_bt/roseus_bt.rosinstall b/roseus_bt/roseus_bt.rosinstall new file mode 100644 index 000000000..ee71a2687 --- /dev/null +++ b/roseus_bt/roseus_bt.rosinstall @@ -0,0 +1,14 @@ +- git: + local-name: jsk_roseus + uri: https://github.com/Affonso-Gui/jsk_roseus.git + version: roseus_bt + +- 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 From 9f3d1caecf020d8a1c8115a7dfff296a97465da0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 9 Jul 2021 16:10:04 +0900 Subject: [PATCH 075/176] (roseus_bt) Add README files --- roseus_bt/README.md | 50 +++++++++ roseus_bt/sample/README.md | 212 +++++++++++++++++++++++++++++++++++++ 2 files changed, 262 insertions(+) create mode 100644 roseus_bt/README.md create mode 100644 roseus_bt/sample/README.md diff --git a/roseus_bt/README.md b/roseus_bt/README.md new file mode 100644 index 000000000..dbf62f486 --- /dev/null +++ b/roseus_bt/README.md @@ -0,0 +1,50 @@ +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/sample/README.md b/roseus_bt/sample/README.md new file mode 100644 index 000000000..b8ddb3bbc --- /dev/null +++ b/roseus_bt/sample/README.md @@ -0,0 +1,212 @@ +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 pallete + +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 behavior tree composition at the xml model file and the euslisp server file are loaded at runtime, but changes in the node pallete (``) 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 an action with the `topic_name` and `to` fields in the `` section and declare it as a `` and specify its type in the `` section. +Only proper ROS message types are supported by subscriber nodes (e.g. `std_msgs/Int64` instead of `int64`). + +Note how we also add a step to verify and wait for messages. + + +#### 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 final 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 reativity. + +Because in such scenario the condition nodes must be evaluated alongside the running action we prepare two distinct roseus servers -- one for actions and one for conditions. +On the action side it is also necessary to check for the presence of interruption requests with the `(roseus_bt:ok)` function. + +#### 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 +``` From 1f30481185e031d82eee7bf3ae3d16224312cb9f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 9 Jul 2021 18:10:26 +0900 Subject: [PATCH 076/176] (roseus_bt) Move server_name/service_name definition to node pallete --- roseus_bt/include/roseus_bt/xml_parser.h | 189 +++++++------------- roseus_bt/sample/README.md | 6 +- roseus_bt/sample/models/t01_simple_tree.xml | 20 +-- roseus_bt/sample/models/t02_conditions.xml | 24 +-- roseus_bt/sample/models/t03_ports.xml | 36 ++-- roseus_bt/sample/models/t04_subscriber.xml | 37 ++-- roseus_bt/sample/models/t05_subtrees.xml | 49 +++-- roseus_bt/sample/models/t06_reactive.xml | 56 +++--- 8 files changed, 180 insertions(+), 237 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 9f3753901..9c087cbb4 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -31,11 +31,8 @@ class XMLParser XMLDocument doc; GenTemplate gen_template; - void collect_node_attribute(const XMLElement* node, const XMLElement* ref_node, - const char* attribute, - std::vector* node_attributes, - std::vector* parallel_node_attributes, - bool is_parallel); + 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 fn); @@ -49,12 +46,6 @@ class XMLParser std::vector* instance_creation, std::vector* parallel_callback_definition, std::vector* parallel_instance_creation); - void push_eus_server(const XMLElement* node, - std::vector server_names, - std::vector* callback_definition, - std::vector* instance_creation, - std::function format_callback, - std::function format_instance); void maybe_push_message_package(const XMLElement* node, std::vector* message_packages); std::string format_eus_name(const std::string input); @@ -85,52 +76,46 @@ class XMLParser }; +bool XMLParser::is_reactive(const XMLElement* node) { + const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); + + bt_root->NextSiblingElement("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) { -void XMLParser::collect_node_attribute(const XMLElement* node, - const XMLElement* ref_node, - const char* attribute, - std::vector* node_attributes, - std::vector* parallel_node_attributes, - bool is_parallel) { std::string name = node->Name(); // is possibly reactive control node if (name.find("Fallback") != std::string::npos || name.find("Sequence") != std::string::npos) { - is_parallel = (name.find("Reactive") != std::string::npos); + reactive_parent = (name.find("Reactive") != std::string::npos); } - // node name and ID matches - if (!std::strcmp(node->Name(), ref_node->Name()) && + // 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"))) { - - // check if the node has the given attribute - if (!node->Attribute(attribute)) { - XMLPrinter printer; - node->Accept(&printer); - throw std::logic_error(fmt::format("{} is required in {} nodes! At {}", - attribute, name, printer.CStr())); - } - - // push if the attribute is unique to the list - if (std::find(node_attributes->begin(), node_attributes->end(), - node->Attribute(attribute)) == node_attributes->end()) { - if (is_parallel) - parallel_node_attributes->push_back(node->Attribute(attribute)); - else - node_attributes->push_back(node->Attribute(attribute)); - } - return; + return true; } for (auto child_node = node->FirstChildElement(); child_node != nullptr; child_node = child_node->NextSiblingElement()) { - collect_node_attribute(child_node, ref_node, attribute, - node_attributes, parallel_node_attributes, - is_parallel); + if (is_reactive_base(child_node, ref_node, reactive_parent)) + return true; } + return false; } void XMLParser::collect_param_list(const XMLElement* node, @@ -167,10 +152,10 @@ void XMLParser::collect_eus_actions(const std::string package_name, std::vector* callback_definition, std::vector* instance_creation) { - auto format_callback = [this](const XMLElement* node, std::string suffix) { + auto format_callback = [this](const XMLElement* node) { std::string fmt_string = 1 + R"( -(roseus_bt:define-action-callback {0}-execute-cb{1} ({2}) -{3} +(roseus_bt:define-action-callback {0}-execute-cb ({1}) +{2} t) )"; std::vector param_list; @@ -178,44 +163,32 @@ void XMLParser::collect_eus_actions(const std::string package_name, return fmt::format(fmt_string, format_eus_name(node->Attribute("ID")), - suffix, boost::algorithm::join(param_list, " "), format_node_body(node)); }; - auto format_instance = [this, package_name](const XMLElement* node, std::string suffix, - std::string server_name) { + 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 - "{4}" {0}::{1}Action - :execute-cb '{2}-execute-cb{3}) + "{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")), - suffix, server_name); }; const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); - const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); for (auto action_node = root->FirstChildElement("Action"); action_node != nullptr; action_node = action_node->NextSiblingElement("Action")) { - std::vector server_names; - for (auto bt_node = bt_root; - bt_node != nullptr; - bt_node = bt_node->NextSiblingElement("BehaviorTree")) { - collect_node_attribute(bt_node, action_node, "server_name", &server_names, &server_names, - false); - } - - push_eus_server(action_node, server_names, - callback_definition, instance_creation, - format_callback, format_instance); + std::string server_name = action_node->Attribute("server_name"); + callback_definition->push_back(format_callback(action_node)); + instance_creation->push_back(format_instance(action_node, server_name)); } } @@ -225,10 +198,10 @@ void XMLParser::collect_eus_conditions(const std::string package_name, std::vector* parallel_callback_definition, std::vector* parallel_instance_creation) { - auto format_callback = [this](const XMLElement* node, std::string suffix) { + auto format_callback = [this](const XMLElement* node) { std::string fmt_string = 1 + R"( -(roseus_bt:define-condition-callback {0}-cb{1} ({2}) -{3} +(roseus_bt:define-condition-callback {0}-cb ({1}) +{2} ) )"; std::vector param_list; @@ -236,83 +209,47 @@ void XMLParser::collect_eus_conditions(const std::string package_name, return fmt::format(fmt_string, format_eus_name(node->Attribute("ID")), - suffix, boost::algorithm::join(param_list, " "), format_node_body(node)); }; - auto format_instance = [this, package_name](const XMLElement* node, std::string suffix, - std::string service_name) { + 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 - "{4}" {0}::{1} - :execute-cb #'{2}-cb{3}) + "{3}" {0}::{1} + :execute-cb #'{2}-cb) )"; return fmt::format(fmt_string, package_name, node->Attribute("ID"), format_eus_name(node->Attribute("ID")), - suffix, service_name); }; const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); - const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); for (auto condition_node = root->FirstChildElement("Condition"); condition_node != nullptr; condition_node = condition_node->NextSiblingElement("Condition")) { - std::vector server_names; - std::vector parallel_server_names; - for (auto bt_node = bt_root; - bt_node != nullptr; - bt_node = bt_node->NextSiblingElement("BehaviorTree")) { - collect_node_attribute(bt_node, condition_node, "service_name", - &server_names, ¶llel_server_names, - false); - } - push_eus_server(condition_node, server_names, - callback_definition, instance_creation, - format_callback, format_instance); - push_eus_server(condition_node, parallel_server_names, - parallel_callback_definition, parallel_instance_creation, - format_callback, format_instance); - } -} - -void XMLParser::push_eus_server( - const XMLElement* node, - std::vector server_names, - std::vector* callback_definition, - std::vector* instance_creation, - std::function format_callback, - std::function format_instance) -{ - if (callback_definition == NULL || instance_creation == NULL) { - return; - } - - if (server_names.empty()) { - return; - } + std::string service_name = condition_node->Attribute("service_name"); - if (server_names.size() == 1) { - callback_definition->push_back(format_callback(node, "")); - instance_creation->push_back(format_instance(node, "", server_names.at(0))); - } - else { - int suffix_num = 1; - for (std::vector::const_iterator it = server_names.begin(); - it != server_names.end(); ++it, suffix_num++) { - std::string suffix = fmt::format("-{}", suffix_num); - callback_definition->push_back(format_callback(node, suffix)); - instance_creation->push_back(format_instance(node, suffix, *it)); + if (is_reactive(condition_node)) { + if (parallel_callback_definition != NULL && + parallel_instance_creation != NULL) { + parallel_callback_definition->push_back(format_callback(condition_node)); + parallel_instance_creation->push_back(format_instance(condition_node, service_name)); + } + } + else { + if (callback_definition != NULL && instance_creation != NULL) { + callback_definition->push_back(format_callback(condition_node)); + instance_creation->push_back(format_instance(condition_node, service_name)); + } + } } - } } - void XMLParser::maybe_push_message_package(const XMLElement* node, std::vector* message_packages) { std::string msg_type = node->Attribute("type"); @@ -321,7 +258,7 @@ void XMLParser::maybe_push_message_package(const XMLElement* node, std::string pkg = msg_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]+)"); @@ -329,7 +266,7 @@ std::string XMLParser::format_eus_name(const std::string input) { 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) { auto format_setoutput = [](const XMLElement* port_node) { @@ -446,6 +383,10 @@ std::string XMLParser::generate_headers(const std::string package_name) { } std::string XMLParser::generate_action_class(const XMLElement* node, const std::string package_name) { + auto format_server_name = [](const XMLElement* node) { + return fmt::format(" InputPort(\"server_name\", \"{0}\", \"name of the Action Server\")", + node->Attribute("server_name")); + }; auto format_input_port = [](const XMLElement* node) { return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); @@ -469,6 +410,8 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const std:: 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()) @@ -498,6 +441,10 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const std:: } std::string XMLParser::generate_condition_class(const XMLElement* node, const std::string package_name) { + auto format_service_name = [](const XMLElement* node) { + return fmt::format(" InputPort(\"service_name\", \"{0}\", \"name of the ROS service\")", + node->Attribute("service_name")); + }; auto format_input_port = [](const XMLElement* node) { return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); @@ -510,6 +457,8 @@ std::string XMLParser::generate_condition_class(const XMLElement* node, const st 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")) diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index b8ddb3bbc..c2957cf38 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -17,11 +17,11 @@ The model file https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_b - `` specifies the tree structure - `` specifies the custom node pallete -Every `` tag in the `` must be provided with an arbitrary `server_name` field. +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 behavior tree composition at the xml model file and the euslisp server file are loaded at runtime, but changes in the node pallete (``) must be re-generated and re-compiled. +Both the `` tag in the xml model file and the euslisp server are loaded at runtime, but changes in the node pallete (``) must be re-generated and re-compiled. #### Run the code @@ -46,7 +46,7 @@ rosrun groot Groot 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. +Every `` tag in the `` must be provided with an arbitrary `service_name` field. #### Run the code diff --git a/roseus_bt/sample/models/t01_simple_tree.xml b/roseus_bt/sample/models/t01_simple_tree.xml index c82d734f0..fa93d60b0 100644 --- a/roseus_bt/sample/models/t01_simple_tree.xml +++ b/roseus_bt/sample/models/t01_simple_tree.xml @@ -3,20 +3,20 @@ - - - - - + + + + + - - - - - + + + + + diff --git a/roseus_bt/sample/models/t02_conditions.xml b/roseus_bt/sample/models/t02_conditions.xml index fb583ddcb..27264e9c0 100644 --- a/roseus_bt/sample/models/t02_conditions.xml +++ b/roseus_bt/sample/models/t02_conditions.xml @@ -3,24 +3,24 @@ - + - - + + - - - + + + - - - - - - + + + + + + diff --git a/roseus_bt/sample/models/t03_ports.xml b/roseus_bt/sample/models/t03_ports.xml index ab1fd8447..ae769d1f6 100644 --- a/roseus_bt/sample/models/t03_ports.xml +++ b/roseus_bt/sample/models/t03_ports.xml @@ -3,39 +3,37 @@ - - + + - - + + - - - + + + - - + + - + - - - - - - + + + + + + + diff --git a/roseus_bt/sample/models/t04_subscriber.xml b/roseus_bt/sample/models/t04_subscriber.xml index 908d9b283..c288f06b5 100644 --- a/roseus_bt/sample/models/t04_subscriber.xml +++ b/roseus_bt/sample/models/t04_subscriber.xml @@ -3,48 +3,47 @@ - + - - + + + coords="${bottle_coords}"/> - - - + + + - - + + - - - - + - - - + + + + + + + + diff --git a/roseus_bt/sample/models/t05_subtrees.xml b/roseus_bt/sample/models/t05_subtrees.xml index f77eb73a5..3b6787b1b 100644 --- a/roseus_bt/sample/models/t05_subtrees.xml +++ b/roseus_bt/sample/models/t05_subtrees.xml @@ -3,9 +3,8 @@ - - + + @@ -16,48 +15,48 @@ - - + + - - - + + + - - + + - + - - + + + + + - + - - - - - - - - - + + + + + + + + diff --git a/roseus_bt/sample/models/t06_reactive.xml b/roseus_bt/sample/models/t06_reactive.xml index a848a7fa0..f39b886dc 100644 --- a/roseus_bt/sample/models/t06_reactive.xml +++ b/roseus_bt/sample/models/t06_reactive.xml @@ -3,14 +3,12 @@ - - + + - + @@ -21,50 +19,50 @@ - - + + - - - + + + - - + + - + - - + + + + + - + - - - - - - + + + + + + + + - + - - - + From b20ddb4bb2fd5f66a78a5eb4094ce5ae958aaa75 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 9 Jul 2021 23:40:15 +0900 Subject: [PATCH 077/176] (roseus_bt) Avoid pushing duplicates on collect_eus_actions/conditions --- roseus_bt/include/roseus_bt/xml_parser.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 9c087cbb4..0e82514cd 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -187,8 +187,8 @@ void XMLParser::collect_eus_actions(const std::string package_name, action_node = action_node->NextSiblingElement("Action")) { std::string server_name = action_node->Attribute("server_name"); - callback_definition->push_back(format_callback(action_node)); - instance_creation->push_back(format_instance(action_node, server_name)); + push_new(format_callback(action_node), callback_definition); + push_new(format_instance(action_node, server_name), instance_creation); } } @@ -237,14 +237,14 @@ void XMLParser::collect_eus_conditions(const std::string package_name, if (is_reactive(condition_node)) { if (parallel_callback_definition != NULL && parallel_instance_creation != NULL) { - parallel_callback_definition->push_back(format_callback(condition_node)); - parallel_instance_creation->push_back(format_instance(condition_node, service_name)); + push_new(format_callback(condition_node), parallel_callback_definition); + push_new(format_instance(condition_node, service_name), parallel_instance_creation); } } else { if (callback_definition != NULL && instance_creation != NULL) { - callback_definition->push_back(format_callback(condition_node)); - instance_creation->push_back(format_instance(condition_node, service_name)); + push_new(format_callback(condition_node), callback_definition); + push_new(format_instance(condition_node, service_name), instance_creation); } } } From 5c3bc7bae8e4f6fa27d201a0c65f1387eef978ad Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 01:51:46 +0900 Subject: [PATCH 078/176] (roseus_bt) Verify the xml file syntax --- roseus_bt/include/roseus_bt/exceptions.h | 33 +++++++++ roseus_bt/include/roseus_bt/xml_parser.h | 93 ++++++++++++++++++++++++ 2 files changed, 126 insertions(+) create mode 100644 roseus_bt/include/roseus_bt/exceptions.h diff --git a/roseus_bt/include/roseus_bt/exceptions.h b/roseus_bt/include/roseus_bt/exceptions.h new file mode 100644 index 000000000..8848a8185 --- /dev/null +++ b/roseus_bt/include/roseus_bt/exceptions.h @@ -0,0 +1,33 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EXCEPTIONS_ +#define BEHAVIOR_TREE_ROSEUS_BT_EXCEPTIONS_ + +#include +#include + +namespace XMLError +{ + +class XMLError: public std::exception { +public: + XMLError(std::string message): message(message) {} + + const char* what() const noexcept { + return message.c_str(); + } + + std::string message; +}; + +class NoAttribute: public XMLError { +public: + NoAttribute(std::string message) : XMLError(message) {}; +}; + +class UnknownNode: public XMLError { +public: + UnknownNode(std::string message) : XMLError(message) {}; +}; + +} // namespace RoseusBT + +#endif // BEHAVIOR_TREE_ROSEUS_BT_EXCEPTIONS_ diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 0e82514cd..7a238e3dd 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -4,6 +4,7 @@ #include #include #include +#include #include void push_new(std::string elem, std::vector* vec) { @@ -23,6 +24,7 @@ class XMLParser XMLParser(std::string filename) : gen_template() { doc.LoadFile(filename.c_str()); + check_xml_file(); } ~XMLParser() {}; @@ -31,6 +33,7 @@ class XMLParser XMLDocument doc; GenTemplate gen_template; + void check_xml_file(); 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, @@ -76,6 +79,96 @@ class XMLParser }; +void XMLParser::check_xml_file() { + auto format_error = [](const XMLElement* node, std::string message) { + XMLPrinter printer; + node->Accept(&printer); + return fmt::format("{} at line {}: {}", + message, + node->GetLineNum(), + printer.CStr()); + }; + auto check_push = [this](XMLElement* node, std::vector* vec) { + if (std::find(vec->begin(), vec->end(), node->Attribute("ID")) == vec->end()) { + vec->push_back(node->Attribute("ID")); + } + else { + XMLPrinter printer; + node->Accept(&printer); + std::cerr << "Ignoring duplicated node at " << + node->GetLineNum() << ": " << printer.CStr() << std::endl; + doc.DeleteNode(node); + } + }; + + XMLElement* bt_root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + std::vector actions, conditions, subscribers; + + // check tree model + for (auto node = bt_root->FirstChildElement(); + node != nullptr; + node = node->NextSiblingElement()) { + + std::string name = node->Name(); + + if (name != "Action" && + name != "Condition" && + name != "Subscriber" && + name != "SubTree") { + std::string message = fmt::format("Unknown node type {}", name); + throw XMLError::UnknownNode(format_error(node, message)); + } + + if (!node->Attribute("ID")) { + throw XMLError::NoAttribute(format_error(node, "Missing \"ID\" attribute")); + } + + if (name == "Action") { + if (!node->Attribute("server_name")) { + throw XMLError::NoAttribute(format_error(node, "Missing \"server_name\" attribute")); + } + check_push(node, &actions); + } + + if (name == "Condition") { + if (!node->Attribute("service_name")) { + throw XMLError::NoAttribute(format_error(node, "Missing \"service_name\" attribute")); + } + check_push(node, &conditions); + } + + if (name == "Subscriber") { + if (!node->Attribute("type")) { + throw XMLError::NoAttribute(format_error(node, "Missing \"type\" attribute")); + } + check_push(node, &subscribers); + } + + // 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") { + std::string message = fmt::format("Unknown port type {}", port_node->Name()); + throw XMLError::UnknownNode(format_error(port_node, message)); + } + + if (!port_node->Attribute("name")) { + throw XMLError::NoAttribute(format_error(port_node, "Missing \"name\" attribute")); + } + + if (!port_node->Attribute("type")) { + throw XMLError::NoAttribute(format_error(port_node, "Missing \"type\" attribute")); + } + } + } +} + bool XMLParser::is_reactive(const XMLElement* node) { const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); From 577dc5ad840d8a34d99d64528eff7527fa422581 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 10:42:13 +0900 Subject: [PATCH 079/176] (roseus_bt) Fix bug when dealing with multiple duplicated nodes --- roseus_bt/include/roseus_bt/xml_parser.h | 25 +++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 7a238e3dd..b753f7e75 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -88,21 +88,19 @@ void XMLParser::check_xml_file() { node->GetLineNum(), printer.CStr()); }; - auto check_push = [this](XMLElement* node, std::vector* vec) { + 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 { - XMLPrinter printer; - node->Accept(&printer); - std::cerr << "Ignoring duplicated node at " << - node->GetLineNum() << ": " << printer.CStr() << std::endl; - doc.DeleteNode(node); + duplicated_nodes->push_back(node); } }; XMLElement* bt_root = doc.RootElement()->FirstChildElement("TreeNodesModel"); std::vector actions, conditions, subscribers; + std::vector duplicated_nodes; // check tree model for (auto node = bt_root->FirstChildElement(); @@ -127,21 +125,21 @@ void XMLParser::check_xml_file() { if (!node->Attribute("server_name")) { throw XMLError::NoAttribute(format_error(node, "Missing \"server_name\" attribute")); } - check_push(node, &actions); + check_push(node, &actions, &duplicated_nodes); } if (name == "Condition") { if (!node->Attribute("service_name")) { throw XMLError::NoAttribute(format_error(node, "Missing \"service_name\" attribute")); } - check_push(node, &conditions); + check_push(node, &conditions, &duplicated_nodes); } if (name == "Subscriber") { if (!node->Attribute("type")) { throw XMLError::NoAttribute(format_error(node, "Missing \"type\" attribute")); } - check_push(node, &subscribers); + check_push(node, &subscribers, &duplicated_nodes); } // check ports @@ -167,6 +165,15 @@ void XMLParser::check_xml_file() { } } } + + // delete duplicated nodes + for (int i = 0; i < duplicated_nodes.size(); i++) { + XMLElement* node = duplicated_nodes.at(i); + std::cerr << fmt::format("Ignoring duplicated {} node {} at line {}", + node->Name(), node->Attribute("ID"), node->GetLineNum()) + << std::endl; + doc.DeleteNode(node); + } } bool XMLParser::is_reactive(const XMLElement* node) { From 92595ef27b0a0cf9b076e9168aed127dfa9c195f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 11:22:08 +0900 Subject: [PATCH 080/176] (roseus_bt) Add t07_xacro tutorial --- roseus_bt/sample/README.md | 21 +++++++++- roseus_bt/sample/models/t07_xacro.xml.xacro | 40 ++++++++++++++++++ .../models/t07_xacro_pour_task.xml.xacro | 42 +++++++++++++++++++ .../models/t07_xacro_sweep_task.xml.xacro | 37 ++++++++++++++++ roseus_bt/src/create_bt_tutorials.cpp | 1 + 5 files changed, 140 insertions(+), 1 deletion(-) create mode 100644 roseus_bt/sample/models/t07_xacro.xml.xacro create mode 100644 roseus_bt/sample/models/t07_xacro_pour_task.xml.xacro create mode 100644 roseus_bt/sample/models/t07_xacro_sweep_task.xml.xacro diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index c2957cf38..e5596cf1e 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -175,7 +175,7 @@ rosrun groot Groot ## t06_reactive ![t06](https://user-images.githubusercontent.com/20625381/125036676-390d7480-e0ce-11eb-813e-69784c2053a9.gif) -The final 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 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 reativity. @@ -210,3 +210,22 @@ Optionally run Groot for visualization ```bash rosrun groot Groot ``` + +## t07_xacro + +The final example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_bt/sample/models/t07_xacro.xml.xacro illustrates how we can 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. \ No newline at end of file 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/src/create_bt_tutorials.cpp b/roseus_bt/src/create_bt_tutorials.cpp index 30fd2fa40..4260b875f 100644 --- a/roseus_bt/src/create_bt_tutorials.cpp +++ b/roseus_bt/src/create_bt_tutorials.cpp @@ -37,6 +37,7 @@ int main(int argc, char** argv) 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"); // Set executable names executable_names.resize(model_files.size()); From b64163a3d8b5661f8ee6267405d8c1f2ed355838 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 11:26:34 +0900 Subject: [PATCH 081/176] (roseus_bt) Spell check README.md --- roseus_bt/sample/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index e5596cf1e..55ea78a9d 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -15,13 +15,13 @@ catkin build roseus_bt_tutorials 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 pallete +- `` 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 pallete (``) must be re-generated and re-compiled. +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 @@ -177,7 +177,7 @@ rosrun groot Groot 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 reativity. +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. Because in such scenario the condition nodes must be evaluated alongside the running action we prepare two distinct roseus servers -- one for actions and one for conditions. On the action side it is also necessary to check for the presence of interruption requests with the `(roseus_bt:ok)` function. From cd59f35e46004870430bdb59866d3b4731c8716c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 11:56:54 +0900 Subject: [PATCH 082/176] (roseus_bt) Verify root and TreeNodesModel in check_xml_file --- roseus_bt/include/roseus_bt/exceptions.h | 5 +++++ roseus_bt/include/roseus_bt/xml_parser.h | 16 +++++++++++++--- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/roseus_bt/include/roseus_bt/exceptions.h b/roseus_bt/include/roseus_bt/exceptions.h index 8848a8185..a70193712 100644 --- a/roseus_bt/include/roseus_bt/exceptions.h +++ b/roseus_bt/include/roseus_bt/exceptions.h @@ -23,6 +23,11 @@ class NoAttribute: public XMLError { NoAttribute(std::string message) : XMLError(message) {}; }; +class NoNode: public XMLError { +public: + NoNode(std::string message) : XMLError(message) {}; +}; + class UnknownNode: public XMLError { public: UnknownNode(std::string message) : XMLError(message) {}; diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index b753f7e75..43f978523 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -98,10 +98,22 @@ void XMLParser::check_xml_file() { } }; - XMLElement* bt_root = doc.RootElement()->FirstChildElement("TreeNodesModel"); + XMLElement* root = doc.RootElement(); + if (root == nullptr) { + throw XMLError::NoNode("The XML must have a root node called "); + } + if (std::string(root->Name()) != "root") { + throw XMLError::NoNode("The XML must have a root node called "); + } + + XMLElement* bt_root = root->FirstChildElement("TreeNodesModel"); std::vector actions, conditions, subscribers; std::vector duplicated_nodes; + if (bt_root == nullptr) { + throw XMLError::NoNode("The XML must have a node"); + } + // check tree model for (auto node = bt_root->FirstChildElement(); node != nullptr; @@ -179,8 +191,6 @@ void XMLParser::check_xml_file() { bool XMLParser::is_reactive(const XMLElement* node) { const XMLElement* bt_root = doc.RootElement()->FirstChildElement("BehaviorTree"); - bt_root->NextSiblingElement("BehaviorTree"); - for (auto bt_node = bt_root; bt_node != nullptr; bt_node = bt_node->NextSiblingElement("BehaviorTree")) { From 3000d3f1ffcb90eef3ec201c9ebffa956d43eaff Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 12 Jul 2021 11:52:12 +0900 Subject: [PATCH 083/176] (roseus_bt) Add custom exceptions --- roseus_bt/include/roseus_bt/bt_exceptions.h | 41 ++++++++++ roseus_bt/include/roseus_bt/exceptions.h | 38 --------- .../include/roseus_bt/package_generator.h | 6 +- roseus_bt/include/roseus_bt/tutorial_parser.h | 2 +- roseus_bt/include/roseus_bt/xml_exceptions.h | 81 +++++++++++++++++++ roseus_bt/include/roseus_bt/xml_parser.h | 50 +++++------- 6 files changed, 145 insertions(+), 73 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/bt_exceptions.h delete mode 100644 roseus_bt/include/roseus_bt/exceptions.h create mode 100644 roseus_bt/include/roseus_bt/xml_exceptions.h 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/exceptions.h b/roseus_bt/include/roseus_bt/exceptions.h deleted file mode 100644 index a70193712..000000000 --- a/roseus_bt/include/roseus_bt/exceptions.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef BEHAVIOR_TREE_ROSEUS_BT_EXCEPTIONS_ -#define BEHAVIOR_TREE_ROSEUS_BT_EXCEPTIONS_ - -#include -#include - -namespace XMLError -{ - -class XMLError: public std::exception { -public: - XMLError(std::string message): message(message) {} - - const char* what() const noexcept { - return message.c_str(); - } - - std::string message; -}; - -class NoAttribute: public XMLError { -public: - NoAttribute(std::string message) : XMLError(message) {}; -}; - -class NoNode: public XMLError { -public: - NoNode(std::string message) : XMLError(message) {}; -}; - -class UnknownNode: public XMLError { -public: - UnknownNode(std::string message) : XMLError(message) {}; -}; - -} // namespace RoseusBT - -#endif // BEHAVIOR_TREE_ROSEUS_BT_EXCEPTIONS_ diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 2be02f833..1a8b41f33 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -10,6 +10,7 @@ #include #include #include +#include namespace RoseusBT @@ -50,8 +51,7 @@ class PackageGenerator // Check package name std::regex valid_naming ("^[a-zA-Z0-9][a-zA-Z0-9_-]*$"); if (!std::regex_match(package_name, valid_naming)) { - throw std::logic_error( - fmt::format("Package name {} does not follow naming conventions", package_name)); + throw InvalidPackageName(package_name); } }; @@ -98,7 +98,7 @@ bool Query::yn(const std::string message) { if (std::regex_match(answer, no)) return false; } - throw std::logic_error("Invalid input"); + throw InvalidInput(); } template diff --git a/roseus_bt/include/roseus_bt/tutorial_parser.h b/roseus_bt/include/roseus_bt/tutorial_parser.h index 5d0527e60..987aa1e46 100644 --- a/roseus_bt/include/roseus_bt/tutorial_parser.h +++ b/roseus_bt/include/roseus_bt/tutorial_parser.h @@ -58,7 +58,7 @@ std::string TutorialParser::format_node_body(const XMLElement* node) { return fmt::format(fmt_string, output_list.at(0)); } - throw std::logic_error(fmt::format("Unrecognized {} node with ID {}", node->Name(), id)); + throw XMLError::UnknownNode(node); } std::string TutorialParser::generate_eus_action_server(const std::string package_name) { 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..07d366c08 --- /dev/null +++ b/roseus_bt/include/roseus_bt/xml_exceptions.h @@ -0,0 +1,81 @@ +#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 type {}{}", 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))) {}; +}; + + +} // namespace RoseusBT + +#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 index 43f978523..9c502b046 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -4,7 +4,9 @@ #include #include #include -#include +#include +#include +#include #include void push_new(std::string elem, std::vector* vec) { @@ -23,6 +25,9 @@ class XMLParser public: XMLParser(std::string filename) : gen_template() { + if (!boost::filesystem::exists(filename)) { + throw XMLError::FileNotFound(filename); + } doc.LoadFile(filename.c_str()); check_xml_file(); } @@ -80,14 +85,6 @@ class XMLParser }; void XMLParser::check_xml_file() { - auto format_error = [](const XMLElement* node, std::string message) { - XMLPrinter printer; - node->Accept(&printer); - return fmt::format("{} at line {}: {}", - message, - node->GetLineNum(), - printer.CStr()); - }; 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()) { @@ -100,10 +97,10 @@ void XMLParser::check_xml_file() { XMLElement* root = doc.RootElement(); if (root == nullptr) { - throw XMLError::NoNode("The XML must have a root node called "); + throw XMLError::ParsingError(); } if (std::string(root->Name()) != "root") { - throw XMLError::NoNode("The XML must have a root node called "); + throw XMLError::WrongRoot(); } XMLElement* bt_root = root->FirstChildElement("TreeNodesModel"); @@ -111,7 +108,7 @@ void XMLParser::check_xml_file() { std::vector duplicated_nodes; if (bt_root == nullptr) { - throw XMLError::NoNode("The XML must have a node"); + throw XMLError::MissingRequiredNode("TreeNodesModel"); } // check tree model @@ -125,31 +122,30 @@ void XMLParser::check_xml_file() { name != "Condition" && name != "Subscriber" && name != "SubTree") { - std::string message = fmt::format("Unknown node type {}", name); - throw XMLError::UnknownNode(format_error(node, message)); + throw XMLError::UnknownNode(node); } if (!node->Attribute("ID")) { - throw XMLError::NoAttribute(format_error(node, "Missing \"ID\" attribute")); + throw XMLError::MissingRequiredAttribute("ID", node); } if (name == "Action") { if (!node->Attribute("server_name")) { - throw XMLError::NoAttribute(format_error(node, "Missing \"server_name\" attribute")); + throw XMLError::MissingRequiredAttribute("server_name", node); } check_push(node, &actions, &duplicated_nodes); } if (name == "Condition") { if (!node->Attribute("service_name")) { - throw XMLError::NoAttribute(format_error(node, "Missing \"service_name\" attribute")); + throw XMLError::MissingRequiredAttribute("service_name", node); } check_push(node, &conditions, &duplicated_nodes); } if (name == "Subscriber") { if (!node->Attribute("type")) { - throw XMLError::NoAttribute(format_error(node, "Missing \"type\" attribute")); + throw XMLError::MissingRequiredAttribute("type", node); } check_push(node, &subscribers, &duplicated_nodes); } @@ -164,16 +160,15 @@ void XMLParser::check_xml_file() { if (port_name != "input_port" && port_name != "output_port" && port_name != "inout_port") { - std::string message = fmt::format("Unknown port type {}", port_node->Name()); - throw XMLError::UnknownNode(format_error(port_node, message)); + throw XMLError::UnknownPortNode(port_node); } if (!port_node->Attribute("name")) { - throw XMLError::NoAttribute(format_error(port_node, "Missing \"name\" attribute")); + throw XMLError::MissingRequiredAttribute("name", port_node); } if (!port_node->Attribute("type")) { - throw XMLError::NoAttribute(format_error(port_node, "Missing \"type\" attribute")); + throw XMLError::MissingRequiredAttribute("type", port_node); } } } @@ -393,13 +388,6 @@ std::string XMLParser::format_node_body(const XMLElement* node) { } std::string XMLParser::format_message_description(const XMLElement* port_node) { - if (!port_node->Attribute("type") || - !port_node->Attribute("name")) { - std::string error_str = "Illformed port in "; - error_str.append(port_node->Name()); - throw std::logic_error(error_str); - } - std::string output = fmt::format("{} {}", port_node->Attribute("type"), port_node->Attribute("name")); @@ -441,7 +429,7 @@ std::string XMLParser::generate_service_file_contents(const XMLElement* node) { request.push_back(text); } else { - throw std::logic_error("Condition Node only accepts input ports!"); + throw InvalidOutputPort(); } } @@ -586,7 +574,7 @@ std::string XMLParser::generate_subscriber_class(const XMLElement* node) { std::string type = node->Attribute("type"); std::size_t pos = type.find('/'); if (pos == std::string::npos) { - throw std::logic_error(fmt::format("Invalid topic type in: {}", type)); + throw XMLError::InvalidTopicType(type, node); } return fmt::format("{}::{}", type.substr(0, pos), type.substr(1+pos)); }; From c29a6b67dc42b24871c03963fbc32a31aba174fe Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 12 Jul 2021 13:40:21 +0900 Subject: [PATCH 084/176] (roseus_bt) Add logging --- roseus_bt/CMakeLists.txt | 7 +++-- .../include/roseus_bt/package_generator.h | 17 ++++++++++-- roseus_bt/include/roseus_bt/xml_parser.h | 26 +++++++++++++------ roseus_bt/src/create_bt_package.cpp | 14 +++++++++- roseus_bt/src/create_bt_tutorials.cpp | 15 ++++++++++- 5 files changed, 65 insertions(+), 14 deletions(-) diff --git a/roseus_bt/CMakeLists.txt b/roseus_bt/CMakeLists.txt index 88b3f445b..42fb75d2b 100644 --- a/roseus_bt/CMakeLists.txt +++ b/roseus_bt/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.0.2) project(roseus_bt) +SET(Boost_USE_STATIC_LIBS ON) + find_package(catkin REQUIRED COMPONENTS cmake_modules roscpp @@ -8,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(TinyXML2 REQUIRED) +find_package(Boost REQUIRED COMPONENTS log) find_package(fmt) catkin_package( @@ -23,8 +26,8 @@ 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} fmt::fmt) +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} fmt::fmt) +target_link_libraries(create_bt_tutorials ${catkin_LIBRARIES} ${TinyXML2_LIBRARIES} ${Boost_LIBRARIES} fmt::fmt) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 1a8b41f33..9efa6567f 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -119,6 +120,7 @@ void PackageGenerator::copy_xml_file(std::string* xml_filename) { 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); @@ -133,7 +135,9 @@ void PackageGenerator::write_action_files(Parser* parser) { std::map action_files = parser->generate_all_action_files(); for (auto it=action_files.begin(); it!=action_files.end(); ++it) { - std::ofstream output_file(fmt::format("{}/{}", base_dir, it->first)); + 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(); } @@ -147,7 +151,9 @@ void PackageGenerator::write_service_files(Parser* parser) { std::map service_files = parser->generate_all_service_files(); for (auto it=service_files.begin(); it!=service_files.end(); ++it) { - std::ofstream output_file(fmt::format("{}/{}", base_dir, it->first)); + 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(); } @@ -166,6 +172,7 @@ void PackageGenerator::write_cpp_file(Parser* parser, 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, boost::filesystem::absolute(xml_filename).c_str()); @@ -182,6 +189,7 @@ void PackageGenerator::write_eus_action_server(Parser* parser, if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) return; + BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; std::string body = parser->generate_eus_action_server(package_name); if (body.empty()) return; @@ -203,6 +211,7 @@ void PackageGenerator::write_eus_condition_server(Parser* parser, std::string body = parser->generate_eus_condition_server(package_name); if (body.empty()) return; + BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; std::ofstream output_file(dest_file); output_file << body; output_file.close(); @@ -219,6 +228,7 @@ void PackageGenerator::write_cmake_lists(const std::vector 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, @@ -236,6 +246,7 @@ void PackageGenerator::write_package_xml(const std::vector 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); @@ -256,6 +267,8 @@ void PackageGenerator::write_all_files() { std::string xml_filename = xml_filenames.at(i); std::string target_filename = target_filenames.at(i); + BOOST_LOG_TRIVIAL(info) << "Generating " << xml_filename << " files..."; + parser->push_dependencies(&message_packages, &service_files, &action_files); copy_xml_file(&xml_filename); write_action_files(parser); diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 9c502b046..bc331c7f8 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -28,8 +29,9 @@ class XMLParser 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(); + check_xml_file(filename); } ~XMLParser() {}; @@ -38,7 +40,7 @@ class XMLParser XMLDocument doc; GenTemplate gen_template; - void check_xml_file(); + 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, @@ -84,7 +86,7 @@ class XMLParser }; -void XMLParser::check_xml_file() { +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()) { @@ -176,9 +178,8 @@ void XMLParser::check_xml_file() { // delete duplicated nodes for (int i = 0; i < duplicated_nodes.size(); i++) { XMLElement* node = duplicated_nodes.at(i); - std::cerr << fmt::format("Ignoring duplicated {} node {} at line {}", - node->Name(), node->Attribute("ID"), node->GetLineNum()) - << std::endl; + BOOST_LOG_TRIVIAL(warning) << fmt::format("Ignoring duplicated {} node {} at {} line {}", + node->Name(), node->Attribute("ID"), filename, node->GetLineNum()); doc.DeleteNode(node); } } @@ -199,6 +200,7 @@ bool XMLParser::is_reactive_base(const XMLElement* node, const XMLElement* ref_n 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 || @@ -233,13 +235,19 @@ void XMLParser::collect_param_list(const XMLElement* node, port_node = port_node->NextSiblingElement()) { std::string name = port_node->Name(); + BOOST_LOG_TRIVIAL(trace) << "collect_param_list: transversing" << name << "..."; + if (name == "input_port" || name == "inout_port") { - if (param_list != NULL) + if (param_list != NULL) { + BOOST_LOG_TRIVIAL(trace) << "collect_param_list: collecting input:" << name << "..."; param_list->push_back(port_node->Attribute("name")); + } } if (name == "output_port" || name == "inout_port") { - if (output_list != NULL) + if (output_list != NULL) { + BOOST_LOG_TRIVIAL(trace) << "collect_param_list: collecting output:" << name << "..."; output_list->push_back(fn(port_node)); + } } } } @@ -292,6 +300,7 @@ void XMLParser::collect_eus_actions(const std::string package_name, action_node = action_node->NextSiblingElement("Action")) { std::string server_name = action_node->Attribute("server_name"); + BOOST_LOG_TRIVIAL(trace) << "collect_eus_actions: transversing " << server_name << "..."; push_new(format_callback(action_node), callback_definition); push_new(format_instance(action_node, server_name), instance_creation); } @@ -338,6 +347,7 @@ void XMLParser::collect_eus_conditions(const std::string package_name, condition_node = condition_node->NextSiblingElement("Condition")) { std::string service_name = condition_node->Attribute("service_name"); + BOOST_LOG_TRIVIAL(trace) << "collect_eus_conditions: transversing " << service_name << "..."; if (is_reactive(condition_node)) { if (parallel_callback_definition != NULL && diff --git a/roseus_bt/src/create_bt_package.cpp b/roseus_bt/src/create_bt_package.cpp index 9e91d10ef..c42180080 100644 --- a/roseus_bt/src/create_bt_package.cpp +++ b/roseus_bt/src/create_bt_package.cpp @@ -3,6 +3,9 @@ #include #include #include +#include +#include +#include namespace po = boost::program_options; @@ -20,7 +23,8 @@ int main(int argc, char** argv) "executable name (defaults to model filename)") ("author,a", po::value(&author)->default_value("The Author"), "author name") - ("overwrite,y", "overwrite all existing files"); + ("overwrite,y", "overwrite all existing files") + ("verbose,v", "print all logging messages"); po::positional_options_description positional_arguments; positional_arguments.add("package_name", 1); @@ -30,6 +34,14 @@ int main(int argc, char** argv) 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::debug; + } + 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"; diff --git a/roseus_bt/src/create_bt_tutorials.cpp b/roseus_bt/src/create_bt_tutorials.cpp index 4260b875f..5c7b0b045 100644 --- a/roseus_bt/src/create_bt_tutorials.cpp +++ b/roseus_bt/src/create_bt_tutorials.cpp @@ -4,6 +4,9 @@ #include #include #include +#include +#include +#include namespace po = boost::program_options; @@ -17,12 +20,22 @@ int main(int argc, char** argv) po::options_description desc("usage"); desc.add_options() ("help,h", "show this help message and exit") - ("overwrite,y", "overwrite all existing files"); + ("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::debug; + } + + 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"; From a6931ffe8fd388e9a3b9d6169d6da8e17d8aaa56 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 12 Jul 2021 13:50:03 +0900 Subject: [PATCH 085/176] (roseus_bt) Declare template as static member variable --- roseus_bt/include/roseus_bt/xml_parser.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index bc331c7f8..0f92995a9 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -25,7 +25,7 @@ class XMLParser public: - XMLParser(std::string filename) : gen_template() { + XMLParser(std::string filename) { if (!boost::filesystem::exists(filename)) { throw XMLError::FileNotFound(filename); } @@ -39,7 +39,7 @@ class XMLParser protected: XMLDocument doc; - GenTemplate gen_template; + 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); @@ -86,6 +86,8 @@ class XMLParser }; +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) { From d0ebddbdf830c345408c02902bba9e38de5aa8da Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 13 Aug 2021 15:51:00 +0900 Subject: [PATCH 086/176] (roseus_bt) Downgrade cmake minimum required version --- roseus_bt/CMakeLists.txt | 2 +- roseus_bt/include/roseus_bt/pkg_template.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/roseus_bt/CMakeLists.txt b/roseus_bt/CMakeLists.txt index 42fb75d2b..19dd506fa 100644 --- a/roseus_bt/CMakeLists.txt +++ b/roseus_bt/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 2.8.3) project(roseus_bt) SET(Boost_USE_STATIC_LIBS ON) diff --git a/roseus_bt/include/roseus_bt/pkg_template.h b/roseus_bt/include/roseus_bt/pkg_template.h index 82479c42f..c5f1da13a 100644 --- a/roseus_bt/include/roseus_bt/pkg_template.h +++ b/roseus_bt/include/roseus_bt/pkg_template.h @@ -45,7 +45,7 @@ std::string PkgTemplate::cmake_lists_template(std::string package_name, std::vector action_files, std::vector add_executables) { std::string fmt_string = 1+ R"( -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 2.8.3) project(%1%) find_package(catkin REQUIRED COMPONENTS From 5e2799b00f9f2d464672421ef1cf5e60d305e0c9 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 13 Aug 2021 17:03:10 +0900 Subject: [PATCH 087/176] (roseus_bt) Update README title case --- roseus_bt/README.md | 3 +-- roseus_bt/sample/README.md | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/roseus_bt/README.md b/roseus_bt/README.md index dbf62f486..237bd7d5e 100644 --- a/roseus_bt/README.md +++ b/roseus_bt/README.md @@ -1,5 +1,4 @@ -roseus_bt -============ +# 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). diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index 55ea78a9d..cfa72db7f 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -1,5 +1,4 @@ -roseus_bt_tutorials -============ +# roseus_bt_tutorials Create and build the tutorial package From 06b96bd28b5c52f63b47d013aaf8abba8ffa0e66 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 16 Aug 2021 13:48:49 +0900 Subject: [PATCH 088/176] (roseus_bt) Update .rosinstall --- roseus_bt/roseus_bt.rosinstall | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/roseus_bt/roseus_bt.rosinstall b/roseus_bt/roseus_bt.rosinstall index ee71a2687..188a54ba1 100644 --- a/roseus_bt/roseus_bt.rosinstall +++ b/roseus_bt/roseus_bt.rosinstall @@ -1,7 +1,7 @@ - git: local-name: jsk_roseus - uri: https://github.com/Affonso-Gui/jsk_roseus.git - version: roseus_bt + uri: https://github.com/jsk-ros-pkg/jsk_roseus.git + version: master - git: local-name: BehaviorTree.ROS From ff4a3c862ea0c99be9ff828b283d67a839ae4299 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 16 Aug 2021 15:13:34 +0900 Subject: [PATCH 089/176] (roseus_bt) Fix trailing whitespace in nodes.l --- roseus_bt/euslisp/nodes.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index a78f9a3c4..d45098ffc 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -57,7 +57,7 @@ (,get-result ,@arg-list)))))) (setf (get ',name :function-type) :action-node-callback) (setf (get ',name :function-documentation) ,(format nil "~S" arg-list))))) - + (defmacro define-condition-callback (name arg-list &rest body) (user::with-gensyms (request get-result) From 67ad63e2e9cf0638ff551c7696bb1a69667ee5a7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 16 Aug 2021 16:20:51 +0900 Subject: [PATCH 090/176] (roseus_bt) Add checks for accept-cb --- roseus_bt/euslisp/nodes.l | 1 + 1 file changed, 1 insertion(+) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index d45098ffc..cfe932c81 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -88,6 +88,7 @@ (error "action-node-callback function expected for execute-cb")) (check-fn-closure execute-cb) (check-fn-closure preempt-cb) + (check-fn-closure accept-cb) (send-super :init ns spec :execute-cb execute-cb From 57e2bc61d3b2171e82d58e6f8b0fd166602cc14e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 24 Aug 2021 14:08:12 +0900 Subject: [PATCH 091/176] (roseus_bt) Add additional checks for local context on global callbacks --- roseus_bt/euslisp/nodes.l | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index cfe932c81..63bfa4aa6 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -25,12 +25,19 @@ (defun check-fn-closure (fn) (when (and (consp fn) (eql (car fn) 'lambda-closure) - (not (zerop (third fn)))) + (or (not (zerop (third fn))) + (not (zerop (fourth fn))))) (let ((name (get-fn-sym fn))) - (ros::ros-warn - (format nil "Possibly harmful lambda-closure in #'~A! Try using '~A instead." - name - name))))) + (if name + (ros::ros-warn + (format nil "Possibly harmful lambda-closure in #'~A! Try using '~A instead." + name + name)) + (ros::ros-warn + (format nil "Possibly harmful lambda context in ~A! Reseting to 0..." + (append (subseq fn 0 5) '(...))) + (setf (third fn) 0) + (setf (fourth fn) 0)))))) ;; macros From 6de184b69add3531ba519fb5d0a9e5d16f58eba1 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 21 Sep 2021 18:56:08 +0900 Subject: [PATCH 092/176] (roseus_bt) Add groupname option to condition servers --- roseus_bt/euslisp/nodes.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 63bfa4aa6..01fcbcef9 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -109,7 +109,7 @@ (defclass condition-node :super propertied-object) (defmethod condition-node - (:init (ns spec &key execute-cb) + (:init (ns spec &key execute-cb groupname) (unless (eq (get (get-fn-sym execute-cb) :function-type) :condition-node-callback) (ros::ros-error (concatenate-string @@ -117,7 +117,7 @@ "Make sure to define execution callbacks using the define-condition-callback macro")) (error "condition-node-callback function expected for execute-cb")) - (ros::advertise-service ns spec execute-cb) + (ros::advertise-service ns spec execute-cb :groupname groupname) (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.")) From 67365f1228e30d513a1995bcbd4e6e8fe1fffd3b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 21 Sep 2021 21:59:42 +0900 Subject: [PATCH 093/176] (roseus_bt) Use classes instead of macros --- roseus_bt/euslisp/nodes.l | 91 ++++----------- roseus_bt/include/roseus_bt/tutorial_parser.h | 41 ++++--- roseus_bt/include/roseus_bt/xml_parser.h | 108 ++++++++++++------ roseus_bt/sample/sample-task.l | 4 +- 4 files changed, 119 insertions(+), 125 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 01fcbcef9..cb54ae7e5 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -3,9 +3,7 @@ (in-package "ROSEUS_BT") -(export '(define-action-callback define-condition-callback - action-node condition-node spin-once spin - set-output ok)) +(export '(action-node condition-node spin-once spin)) (defvar *action-list*) (defvar *condition-list*) @@ -40,59 +38,10 @@ (setf (fourth fn) 0)))))) -;; macros -(defmacro define-action-callback (name arg-list &rest body) - (user::with-gensyms (server goal get-result) - `(prog1 - (defun ,name (,server ,goal) - (labels ((set-output (name val) - (let ((msg (send ,server :feedback - (intern (string-upcase name) *keyword-package*) val))) - (send ,server :publish-feedback msg))) - (ok () - (send ,server :spin-once) - (not (send ,server :is-preempt-requested))) - (,get-result ,arg-list - (block ,name - ,@body))) - (let (,@(mapcar - #'(lambda (k) `(,k (send ,goal :goal - ,(intern (symbol-pname k) *keyword-package*)))) - arg-list)) - (send ,server :set-succeeded - (send ,server :result :success - (,get-result ,@arg-list)))))) - (setf (get ',name :function-type) :action-node-callback) - (setf (get ',name :function-documentation) ,(format nil "~S" arg-list))))) - - -(defmacro define-condition-callback (name arg-list &rest body) - (user::with-gensyms (request get-result) - `(prog1 - (defun ,name (,request) - (labels ((,get-result ,arg-list - (block ,name - ,@body))) - (let ((response (send ,request :response)) - ,@(mapcar - #'(lambda (k) `(,k (send ,request ,(intern (symbol-pname k) *keyword-package*)))) - arg-list)) - (send response :success (,get-result ,@arg-list)) - response))) - (setf (get ',name :function-type) :condition-node-callback) - (setf (get ',name :function-documentation) ,(format nil "~S" arg-list))))) - - ;; classes (defclass action-node :super ros::simple-action-server) (defmethod action-node (:init (ns spec &key execute-cb preempt-cb accept-cb groupname) - (unless (eq (get (get-fn-sym execute-cb) :function-type) :action-node-callback) - (ros::ros-error - (concatenate-string - "Wrong function type detected! " - "Make sure to define execution callbacks using the define-action-callback macro")) - (error "action-node-callback function expected for execute-cb")) (check-fn-closure execute-cb) (check-fn-closure preempt-cb) (check-fn-closure accept-cb) @@ -105,23 +54,30 @@ (push self *action-list*) (if *condition-list* (ros::ros-warn "Actions and Conditions detected in the same node! Start two separate nodes when reacitivity is required.")) - self)) - -(defclass condition-node :super propertied-object) + self) + (:set-output (name val) + (let ((msg (send self :feedback (intern (string-upcase name) *keyword-package*) val))) + (send self :publish-feedback msg))) + (:ok () + (send self :spin-once) + (not (send self :is-preempt-requested)))) + +(defclass condition-node :super propertied-object :slots (execute-cb)) (defmethod condition-node - (:init (ns spec &key execute-cb groupname) - (unless (eq (get (get-fn-sym execute-cb) :function-type) :condition-node-callback) - (ros::ros-error - (concatenate-string - "Wrong function type detected! " - "Make sure to define execution callbacks using the define-condition-callback macro")) - (error "condition-node-callback function expected for execute-cb")) - - (ros::advertise-service ns spec execute-cb :groupname groupname) + (:init (ns spec &key ((:execute-cb exec-cb)) groupname) + (check-fn-closure exec-cb) + (setq execute-cb exec-cb) + (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)) + self) + (:request-cb (msg) + (let ((response (send msg :response))) + (send response :success (funcall execute-cb self msg)) + response))) ;; functions @@ -134,8 +90,3 @@ (while t (spin-once) (ros::sleep))) - -(defun set-output (name val) - (error "not defined in global scope")) - -(defun ok () t) diff --git a/roseus_bt/include/roseus_bt/tutorial_parser.h b/roseus_bt/include/roseus_bt/tutorial_parser.h index 987aa1e46..8037bf398 100644 --- a/roseus_bt/include/roseus_bt/tutorial_parser.h +++ b/roseus_bt/include/roseus_bt/tutorial_parser.h @@ -18,7 +18,7 @@ class TutorialParser : public XMLParser ~TutorialParser() {}; protected: - virtual std::string format_node_body(const XMLElement* node) override; + 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; @@ -26,36 +26,39 @@ class TutorialParser : public XMLParser }; -std::string TutorialParser::format_node_body(const XMLElement* node) { +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 " (send value :data)"; - if (id == "atTable") return " (at-spot \"table-front\")"; - if (id == "atSpot") return fmt::format(" (at-spot {})", param_list.at(0)); + 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") return fmt::format("{}(at-spot {})", pad, param_list.at(0)); if (id == "CheckCoords") return fmt::format( - " (not (equal (instance geometry_msgs::Pose :init) {}))", param_list.at(0)); + "{}(not (equal (instance geometry_msgs::Pose :init) {}))", pad, param_list.at(0)); // Actions - if (id == "Init") return " (init nil t)"; - if (id == "InitWithBroom") return " (init t t)"; - if (id == "MoveToTable") return " (go-to-spot \"table-front\")"; - if (id == "PickBottle") return " (pick-sushi-bottle)"; - if (id == "PourBottle") return " (pour-sushi-bottle)"; - if (id == "PlaceBottle") return " (place-sushi-bottle)\n (reset-pose)"; - if (id == "SweepFloor") return " (sweep-floor #'roseus_bt:ok)\n (reset-pose)"; - + 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 == "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 == "SweepFloor") + return fmt::format("{0}(sweep-floor #'send server :ok)\n{0}(reset-pose)", pad); if (id == "MoveTo") - return fmt::format(" (go-to-spot {})", param_list.at(0)); + return fmt::format("{}(go-to-spot {})", pad, param_list.at(0)); if (id == "PickBottleAt") - return fmt::format(" (pick-sushi-bottle (ros::tf-pose->coords {}))", param_list.at(0)); + return fmt::format("{}(pick-sushi-bottle (ros::tf-pose->coords {}))", pad, param_list.at(0)); if (id == "setCoords") { std::string fmt_string = 1 + R"( - (roseus_bt:set-output - "{}" (ros::coords->tf-pose (make-coords :pos #f(1850 400 700)))))"; - return fmt::format(fmt_string, output_list.at(0)); +{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)); } throw XMLError::UnknownNode(node); diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 0f92995a9..04602028b 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -45,7 +45,8 @@ class XMLParser 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 fn); + 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_eus_actions(const std::string package_name, @@ -68,7 +69,7 @@ class XMLParser std::string generate_subscriber_class(const XMLElement* node); std::string generate_main_function(const std::string roscpp_node_name, const std::string xml_filename); - virtual std::string format_node_body(const XMLElement* node); + virtual std::string format_node_body(const XMLElement* node, int padding); public: @@ -230,7 +231,8 @@ bool XMLParser::is_reactive_base(const XMLElement* node, const XMLElement* ref_n void XMLParser::collect_param_list(const XMLElement* node, std::vector* param_list, std::vector* output_list, - std::function fn) { + std::function param_fn, + std::function output_fn) { for (auto port_node = node->FirstChildElement(); port_node != nullptr; @@ -242,13 +244,13 @@ void XMLParser::collect_param_list(const XMLElement* node, if (name == "input_port" || name == "inout_port") { if (param_list != NULL) { BOOST_LOG_TRIVIAL(trace) << "collect_param_list: collecting input:" << name << "..."; - param_list->push_back(port_node->Attribute("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:" << name << "..."; - output_list->push_back(fn(port_node)); + output_list->push_back(output_fn(port_node)); } } } @@ -257,29 +259,48 @@ void XMLParser::collect_param_list(const XMLElement* node, void XMLParser::collect_param_list(const XMLElement* node, std::vector* param_list, std::vector* output_list) { - auto format_output_port = [](const XMLElement* port_node) { + auto format_port = [](const XMLElement* port_node) { return port_node->Attribute("name"); }; - collect_param_list(node, param_list, output_list, format_output_port); + collect_param_list(node, param_list, output_list, format_port, format_port); } void XMLParser::collect_eus_actions(const std::string package_name, std::vector* callback_definition, std::vector* instance_creation) { - auto format_callback = [this](const XMLElement* node) { - std::string fmt_string = 1 + R"( -(roseus_bt:define-action-callback {0}-execute-cb ({1}) + 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) + (send server :set-succeeded + (send server :result :success t)))) )"; - std::vector param_list; - collect_param_list(node, ¶m_list, NULL); + return fmt::format(fmt_string, + format_eus_name(node->Attribute("ID")), + boost::algorithm::join(param_list, "\n "), + format_node_body(node, 4)); + } - return fmt::format(fmt_string, - format_eus_name(node->Attribute("ID")), - boost::algorithm::join(param_list, " "), - format_node_body(node)); + // no parameters + std::string fmt_string = 1 + R"( +(defun {0}-execute-cb (server goal) +{1} + (send server :set-succeeded + (send server :result :success 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) { @@ -314,26 +335,44 @@ void XMLParser::collect_eus_conditions(const std::string package_name, std::vector* parallel_callback_definition, std::vector* parallel_instance_creation) { - auto format_callback = [this](const XMLElement* node) { - std::string fmt_string = 1 + R"( -(roseus_bt:define-condition-callback {0}-cb ({1}) + 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} - ) + )) )"; - std::vector param_list; - collect_param_list(node, ¶m_list, NULL); - return fmt::format(fmt_string, - format_eus_name(node->Attribute("ID")), - boost::algorithm::join(param_list, " "), - format_node_body(node)); + 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) + :execute-cb '{2}-cb) )"; return fmt::format(fmt_string, package_name, @@ -385,16 +424,17 @@ std::string XMLParser::format_eus_name(const std::string input) { return out; } -std::string XMLParser::format_node_body(const XMLElement* node) { - auto format_setoutput = [](const XMLElement* port_node) { - return fmt::format(" ;; (roseus_bt:set-output \"{0}\" <{1}>)", +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 \"{1}\" <{2}>)", port_node->Attribute("name"), - port_node->Attribute("type")); + port_node->Attribute("type")). + insert(0, padding, ' '); }; std::vector output_list; - output_list.push_back(" ;; do something"); - collect_param_list(node, NULL, &output_list, format_setoutput); + 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"); } diff --git a/roseus_bt/sample/sample-task.l b/roseus_bt/sample/sample-task.l index 39d2ad013..adef136b9 100644 --- a/roseus_bt/sample/sample-task.l +++ b/roseus_bt/sample/sample-task.l @@ -82,12 +82,12 @@ ;; Sweep task -(defun sweep-floor (&optional (check-fn #'(lambda () t))) +(defun sweep-floor (&optional (check-fn #'(lambda () t)) &rest args) (let ((broom-obj (send *room* :object "broom")) (i 0)) (send *robot* :dual-arm-ik (send broom-obj :handle) :debug-view t) - (while (funcall check-fn) + (while (apply check-fn args) (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)) From 26a76794d4f3e990bbd48ef3d7ce2bd48c2480b7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 22 Sep 2021 16:43:39 +0900 Subject: [PATCH 094/176] (roseus_bt) Call set-succeeded from the :execute-cb method --- roseus_bt/euslisp/nodes.l | 26 ++++++++++++++++++++++-- roseus_bt/include/roseus_bt/xml_parser.h | 6 ++---- 2 files changed, 26 insertions(+), 6 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index cb54ae7e5..700f0e6f9 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -4,6 +4,9 @@ (in-package "ROSEUS_BT") (export '(action-node condition-node 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*) @@ -55,6 +58,25 @@ (if *condition-list* (ros::ros-warn "Actions and Conditions detected in the same node! Start two separate nodes when reacitivity 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 (not (equal status actionlib_msgs::GoalStatus::*pending*)) + ;; avoid double posting to finished requests + (send self :set-succeeded + (send self :result :success (not (not res))))))) + (send self :publish-status)) (:set-output (name val) (let ((msg (send self :feedback (intern (string-upcase name) *keyword-package*) val))) (send self :publish-feedback msg))) @@ -76,7 +98,7 @@ self) (:request-cb (msg) (let ((response (send msg :response))) - (send response :success (funcall execute-cb self msg)) + (send response :success (not (not (funcall execute-cb self msg)))) response))) @@ -84,7 +106,7 @@ (defun spin-once () (ros::spin-once) (dolist (ac *action-list*) - (send ac :worker))) + (send ac :execute-cb))) (defun spin () (while t diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 04602028b..399cf7937 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -282,8 +282,7 @@ void XMLParser::collect_eus_actions(const std::string package_name, (defun {0}-execute-cb (server goal) (let ({1}) {2} - (send server :set-succeeded - (send server :result :success t)))) + t)) )"; return fmt::format(fmt_string, format_eus_name(node->Attribute("ID")), @@ -295,8 +294,7 @@ void XMLParser::collect_eus_actions(const std::string package_name, std::string fmt_string = 1 + R"( (defun {0}-execute-cb (server goal) {1} - (send server :set-succeeded - (send server :result :success t))) + t) )"; return fmt::format(fmt_string, format_eus_name(node->Attribute("ID")), From 58c4d404cb99d3de2351a3590b7e9faef1d1687d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 22 Sep 2021 17:02:37 +0900 Subject: [PATCH 095/176] (roseus_bt) Use :is-active to evaluate ongoing requests --- roseus_bt/euslisp/nodes.l | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 700f0e6f9..5ce16521f 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -72,8 +72,7 @@ (:execute-cb () (when (and goal execute-cb) (let ((res (funcall execute-cb self goal))) - (when (not (equal status actionlib_msgs::GoalStatus::*pending*)) - ;; avoid double posting to finished requests + (when (send self :is-active) (send self :set-succeeded (send self :result :success (not (not res))))))) (send self :publish-status)) From cacc955e73e7f7ff2962d4404d6e8dd7c9a57edb Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 8 Oct 2021 16:32:34 +0900 Subject: [PATCH 096/176] (roseus_bt) Use trace level on verbose option --- roseus_bt/src/create_bt_package.cpp | 2 +- roseus_bt/src/create_bt_tutorials.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/roseus_bt/src/create_bt_package.cpp b/roseus_bt/src/create_bt_package.cpp index c42180080..d174b5377 100644 --- a/roseus_bt/src/create_bt_package.cpp +++ b/roseus_bt/src/create_bt_package.cpp @@ -37,7 +37,7 @@ int main(int argc, char** argv) // Initialize Logger auto logger_level = boost::log::trivial::warning; if (args.count("verbose")) { - logger_level = boost::log::trivial::debug; + logger_level = boost::log::trivial::trace; } boost::log::core::get()->set_filter( boost::log::trivial::severity >= logger_level); diff --git a/roseus_bt/src/create_bt_tutorials.cpp b/roseus_bt/src/create_bt_tutorials.cpp index 5c7b0b045..b0bb17b17 100644 --- a/roseus_bt/src/create_bt_tutorials.cpp +++ b/roseus_bt/src/create_bt_tutorials.cpp @@ -30,7 +30,7 @@ int main(int argc, char** argv) // Initialize Logger auto logger_level = boost::log::trivial::warning; if (args.count("verbose")) { - logger_level = boost::log::trivial::debug; + logger_level = boost::log::trivial::trace; } boost::log::core::get()->set_filter( From 89f08cdb8891922060494ca868813f54bd0dd59d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 8 Oct 2021 16:40:28 +0900 Subject: [PATCH 097/176] (roseus_bt) Format tracing messages --- roseus_bt/include/roseus_bt/xml_parser.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 399cf7937..6e928a8e2 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -203,7 +203,7 @@ bool XMLParser::is_reactive_base(const XMLElement* node, const XMLElement* ref_n bool reactive_parent) { std::string name = node->Name(); - BOOST_LOG_TRIVIAL(trace) << "is_reactive_base: transversing" << name << "..."; + BOOST_LOG_TRIVIAL(trace) << "is_reactive_base: transversing " << name << "..."; // is possibly reactive control node if (name.find("Fallback") != std::string::npos || @@ -239,17 +239,17 @@ void XMLParser::collect_param_list(const XMLElement* node, port_node = port_node->NextSiblingElement()) { std::string name = port_node->Name(); - BOOST_LOG_TRIVIAL(trace) << "collect_param_list: transversing" << name << "..."; + BOOST_LOG_TRIVIAL(trace) << "collect_param_list: transversing " << name << "..."; if (name == "input_port" || name == "inout_port") { if (param_list != NULL) { - BOOST_LOG_TRIVIAL(trace) << "collect_param_list: collecting input:" << name << "..."; + BOOST_LOG_TRIVIAL(trace) << "collect_param_list: collecting input: " << 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:" << name << "..."; + BOOST_LOG_TRIVIAL(trace) << "collect_param_list: collecting output: " << name << "..."; output_list->push_back(output_fn(port_node)); } } From 2dcfc4b2bd25c1b9549065a3f133cce6d8ebf5be Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 8 Oct 2021 16:59:34 +0900 Subject: [PATCH 098/176] (roseus_bt) Add port name to trace log --- roseus_bt/include/roseus_bt/xml_parser.h | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 6e928a8e2..54d3c64c5 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -239,17 +239,21 @@ void XMLParser::collect_param_list(const XMLElement* node, port_node = port_node->NextSiblingElement()) { std::string name = port_node->Name(); - BOOST_LOG_TRIVIAL(trace) << "collect_param_list: transversing " << 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: " << name << "..."; + 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: " << name << "..."; + BOOST_LOG_TRIVIAL(trace) << "collect_param_list: collecting output: " << + port_name << "..."; output_list->push_back(output_fn(port_node)); } } From abbf3f20406a0e2cd630d55e87b35bb0c3014364 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 8 Oct 2021 17:04:38 +0900 Subject: [PATCH 099/176] (roseus_bt) Fix fmt::format indexing in format_node_body --- roseus_bt/include/roseus_bt/xml_parser.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 54d3c64c5..13f88593b 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -428,7 +428,7 @@ std::string XMLParser::format_eus_name(const std::string input) { 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 \"{1}\" <{2}>)", + return fmt::format(";; (send server :set-output \"{0}\" <{1}>)", port_node->Attribute("name"), port_node->Attribute("type")). insert(0, padding, ' '); From 48923257cd78a0c11b5fcf5edecc9b2902ae41c6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 22 Sep 2021 15:30:09 +0900 Subject: [PATCH 100/176] (roseus_bt) Add monitor-groupname for separate goal and cancel handlers --- roseus_bt/euslisp/nodes.l | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 5ce16521f..dc50d832c 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -42,9 +42,10 @@ ;; classes -(defclass action-node :super ros::simple-action-server) +(defclass action-node :super ros::simple-action-server :slots (monitor-groupname)) (defmethod action-node - (:init (ns spec &key execute-cb preempt-cb accept-cb groupname) + (:init (ns spec &key execute-cb preempt-cb accept-cb groupname + ((:monitor-groupname monitor-gn))) (check-fn-closure execute-cb) (check-fn-closure preempt-cb) (check-fn-closure accept-cb) @@ -54,6 +55,13 @@ :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 reacitivity is required.")) @@ -80,8 +88,13 @@ (let ((msg (send self :feedback (intern (string-upcase name) *keyword-package*) val))) (send self :publish-feedback msg))) (:ok () - (send self :spin-once) - (not (send self :is-preempt-requested)))) + (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 condition-node :super propertied-object :slots (execute-cb)) (defmethod condition-node From aa770971c5fa9b188a49c77a01150f12dd393e1e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 24 Feb 2022 14:20:46 +0900 Subject: [PATCH 101/176] (roseus_bt) Add groupname option to condition nodes --- roseus_bt/euslisp/nodes.l | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index dc50d832c..1d1f774ec 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -96,11 +96,12 @@ (send self :spin-once)))) -(defclass condition-node :super propertied-object :slots (execute-cb)) +(defclass condition-node :super propertied-object :slots (execute-cb groupname)) (defmethod condition-node - (:init (ns spec &key ((:execute-cb exec-cb)) groupname) + (:init (ns spec &key ((:execute-cb exec-cb)) ((:groupname gn))) (check-fn-closure exec-cb) (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)) @@ -111,13 +112,19 @@ (:request-cb (msg) (let ((response (send msg :response))) (send response :success (not (not (funcall execute-cb self msg)))) - response))) + response)) + (:spin-once () + (if groupname + (ros::spin-once groupname) + (ros::spin-once)))) ;; functions (defun spin-once () - (ros::spin-once) + (dolist (cc *condition-list*) + (send cc :spin-once)) (dolist (ac *action-list*) + (send ac :spin-once) (send ac :execute-cb))) (defun spin () From 980a526e9039c98d04d945b32943518b4071a477 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 13:29:09 +0900 Subject: [PATCH 102/176] (roseus_bt) Check server connections at init time --- roseus_bt/include/roseus_bt/eus_action_node.h | 13 ++++++++++++- roseus_bt/include/roseus_bt/eus_condition_node.h | 12 +++++++++++- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_action_node.h b/roseus_bt/include/roseus_bt/eus_action_node.h index 0ec620eeb..5b6ca5687 100644 --- a/roseus_bt/include/roseus_bt/eus_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_action_node.h @@ -16,7 +16,18 @@ class EusActionNode: public BT::RosActionNode { protected: EusActionNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration& conf): - BT::RosActionNode(nh, name, 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; diff --git a/roseus_bt/include/roseus_bt/eus_condition_node.h b/roseus_bt/include/roseus_bt/eus_condition_node.h index 52ed5aab1..ed9d2fee6 100644 --- a/roseus_bt/include/roseus_bt/eus_condition_node.h +++ b/roseus_bt/include/roseus_bt/eus_condition_node.h @@ -10,7 +10,17 @@ template class EusConditionNode : public BT::RosServiceNode { protected: - EusConditionNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration& conf): BT::RosServiceNode(nh, name, conf) {}; + 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); + + 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; From b004e63ff93c60bdf98cf77d1307518afa5b40b1 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 17:35:41 +0900 Subject: [PATCH 103/176] (roseus_bt) Stablish persistent connection with service client at init time --- roseus_bt/include/roseus_bt/eus_condition_node.h | 1 + 1 file changed, 1 insertion(+) diff --git a/roseus_bt/include/roseus_bt/eus_condition_node.h b/roseus_bt/include/roseus_bt/eus_condition_node.h index ed9d2fee6..f4c1d2a9d 100644 --- a/roseus_bt/include/roseus_bt/eus_condition_node.h +++ b/roseus_bt/include/roseus_bt/eus_condition_node.h @@ -14,6 +14,7 @@ class EusConditionNode : public BT::RosServiceNode 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); From f27e4ab9b864a4531c1ba126953705795aee9a79 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 17:39:40 +0900 Subject: [PATCH 104/176] (roseus_bt) Add sleep in sample task loop --- roseus_bt/sample/sample-task.l | 1 + 1 file changed, 1 insertion(+) diff --git a/roseus_bt/sample/sample-task.l b/roseus_bt/sample/sample-task.l index adef136b9..5cf7669a6 100644 --- a/roseus_bt/sample/sample-task.l +++ b/roseus_bt/sample/sample-task.l @@ -92,4 +92,5 @@ (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)))) From 8af94a835732e07066c65e5fb80f8ebae7f44bf5 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 17:41:36 +0900 Subject: [PATCH 105/176] (roseus_bt) Increase default spin rate --- roseus_bt/euslisp/nodes.l | 2 ++ roseus_bt/include/roseus_bt/gen_template.h | 5 ++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 1d1f774ec..04e8ae91f 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -83,6 +83,8 @@ (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))) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index f8dcfb51e..78ba2f35a 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -261,7 +261,7 @@ int main(int argc, char **argv) { ros::spinOnce(); status = tree.tickRoot(); - ros::Duration sleep_time(0.05); + ros::Duration sleep_time(0.005); sleep_time.sleep(); } @@ -314,6 +314,9 @@ std::string GenTemplate::eus_server_template(std::string server_type, ;; create server instances %6% +;; set rate +(ros::rate 100) + ;; spin (roseus_bt:spin) )"; From c08a19d7b24c65491511c343b8e1498e9a1e12e2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 20:24:57 +0900 Subject: [PATCH 106/176] (roseus_bt) Update RetryUntilSuccessful name --- roseus_bt/sample/models/t04_subscriber.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/roseus_bt/sample/models/t04_subscriber.xml b/roseus_bt/sample/models/t04_subscriber.xml index c288f06b5..74f6bc344 100644 --- a/roseus_bt/sample/models/t04_subscriber.xml +++ b/roseus_bt/sample/models/t04_subscriber.xml @@ -10,7 +10,7 @@ - + @@ -18,7 +18,7 @@ - + From 49c39db257f93e015c7c61c6a99400fb31bb9c3a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 23:36:21 +0900 Subject: [PATCH 107/176] (roseus_bt) Add rosbridgecpp submodule --- .gitmodules | 3 +++ 1 file changed, 3 insertions(+) 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 From b2bb0caf13a64bd1db0d5f100446a06c0446337f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 23:38:56 +0900 Subject: [PATCH 108/176] (roseus_bt) Add eus_remote_action_node.h --- roseus_bt/include/roseus_bt/eus_nodes.h | 1 + .../roseus_bt/eus_remote_action_node.h | 123 ++++++++++++++++++ 2 files changed, 124 insertions(+) create mode 100644 roseus_bt/include/roseus_bt/eus_remote_action_node.h diff --git a/roseus_bt/include/roseus_bt/eus_nodes.h b/roseus_bt/include/roseus_bt/eus_nodes.h index a331478ba..f4b43362a 100644 --- a/roseus_bt/include/roseus_bt/eus_nodes.h +++ b/roseus_bt/include/roseus_bt/eus_nodes.h @@ -4,6 +4,7 @@ #include "eus_action_node.h" #include "eus_condition_node.h" #include "eus_subscriber_node.h" +#include "eus_remote_action_node.h" #endif // BEHAVIOR_TREE_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..d15626481 --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -0,0 +1,123 @@ +#ifndef BEHAVIOR_TREE_EUS_REMOTE_ACTION_NODE_HPP_ +#define BEHAVIOR_TREE_EUS_REMOTE_ACTION_NODE_HPP_ +#define DEBUG + +#include +#include +#include +#include + + +namespace BT +{ + +// Helper Node to call a rosbridge websocket inside a BT::ActionNode +class EusRemoteActionNode : public BT::ActionNodeBase +{ +protected: + + EusRemoteActionNode(const std::string& master, int port, const std::string message_type, const std::string& name, const BT::NodeConfiguration & conf): + BT::ActionNodeBase(name, conf), + rbc_(fmt::format("{}:{}", master, std::to_string(port))) + { + server_name_ = getInput("server_name").value(); + goal_topic_ = fmt::format("{}/goal", server_name_); + result_topic_ = fmt::format("{}/result", server_name_); + feedback_topic_ = fmt::format("{}/feedback", server_name_); + + rbc_.addClient("goal_publisher"); + rbc_.addClient("goal_advertiser"); + rbc_.advertise("goal_advertiser", goal_topic_, message_type); + } + +public: + + EusRemoteActionNode() = delete; + + virtual ~EusRemoteActionNode() = default; + + static PortsList providedPorts() + { + return { + InputPort("server_name", "name of the Action Server") + }; + } + + virtual bool sendGoal(rapidjson::Document& goal) = 0; + + // virtual NodeStatus onResult( const ResultType& res) = 0; + // virtual NodeStatus onFailedRequest(FailureCause failure) + // { + // return NodeStatus::FAILURE; + // } + + virtual void halt() override + { + // if( status() == NodeStatus::RUNNING ) + // { + // action_client_->cancelGoal(); + // } + setStatus(NodeStatus::IDLE); + // action_client_->waitForResult(); + } + +protected: + RosbridgeWsClient rbc_; + + 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; + } + + rapidjson::Document action_goal; + action_goal.SetObject(); + action_goal.AddMember("goal", goal, action_goal.GetAllocator()); + // TODO: add header and goal_id + + rbc_.publish(goal_topic_, action_goal); + } + + // TODO: wait for result + // TODO: track feedback + return NodeStatus::SUCCESS; + } +}; + + +/// Method to register the service into a factory. +/// It gives you the opportunity to set the ros::NodeHandle. + 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_BT_REMOTE_ACTION_NODE_HPP_ From 432f893cc563ef845c3dde3c07d94b730655da85 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 19 Apr 2022 07:37:18 +0900 Subject: [PATCH 109/176] (roseus_bt) Add EusRemoteAction generation code --- roseus_bt/include/roseus_bt/gen_template.h | 46 ++++++++ roseus_bt/include/roseus_bt/pkg_template.h | 5 +- roseus_bt/include/roseus_bt/xml_parser.h | 121 ++++++++++++++------- 3 files changed, 130 insertions(+), 42 deletions(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 78ba2f35a..eed5b879a 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -25,6 +25,11 @@ class GenTemplate 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::string host_name, int host_port, + 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); @@ -149,6 +154,47 @@ EusActionNode<%1%::%2%Action>(handle, name, conf) {} } +std::string GenTemplate::remote_action_class_template( + std::string package_name, std::string nodeID, + std::string host_name, int host_port, + std::vector provided_ports, + std::vector get_inputs, + std::vector set_outputs) +{ + std::string fmt_string = 1 + R"( +class %2%: public EusRemoteActionNode +{ + +public: + %2%(const std::string& name, const NodeConfiguration& conf): +EusRemoteActionNode("%3%", %4%, "%1%/%2%ActionGoal", name, conf) {} + + static PortsList providedPorts() + { + return { +%5% + }; + } + + bool sendGoal(rapidjson::Document& goal) override + { +%6% + return true; + } +}; +)"; + boost::format bfmt = boost::format(fmt_string) % + package_name % + nodeID % + host_name % + host_port % + boost::algorithm::join(provided_ports, ",\n") % + boost::algorithm::join(get_inputs, "\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) { diff --git a/roseus_bt/include/roseus_bt/pkg_template.h b/roseus_bt/include/roseus_bt/pkg_template.h index c5f1da13a..909f9a620 100644 --- a/roseus_bt/include/roseus_bt/pkg_template.h +++ b/roseus_bt/include/roseus_bt/pkg_template.h @@ -55,6 +55,7 @@ find_package(catkin REQUIRED COMPONENTS roseus_bt %2% ) +find_package(fmt) add_service_files( FILES @@ -77,10 +78,12 @@ catkin_package( CATKIN_DEPENDS message_runtime %2% + DEPENDS fmt ) include_directories(${catkin_INCLUDE_DIRS}) +add_subdirectory(${roseus_bt_SOURCE_PREFIX}/include/rosbridgecpp rosbridgecpp) %5% )"; @@ -177,7 +180,7 @@ std::string PkgTemplate::generate_cmake_lists(std::string package_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}) +target_link_libraries(%1% ${catkin_LIBRARIES} rosbridgecpp fmt::fmt) )"; diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 13f88593b..ad7859a8c 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -64,7 +64,8 @@ class XMLParser 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_action_class(const XMLElement* node, const std::string package_name, + bool remote); std::string generate_condition_class(const XMLElement* node, const std::string package_name); std::string generate_subscriber_class(const XMLElement* node); std::string generate_main_function(const std::string roscpp_node_name, const std::string xml_filename); @@ -124,6 +125,7 @@ void XMLParser::check_xml_file(std::string filename) { std::string name = node->Name(); if (name != "Action" && + name != "RemoteAction" && name != "Condition" && name != "Subscriber" && name != "SubTree") { @@ -134,7 +136,7 @@ void XMLParser::check_xml_file(std::string filename) { throw XMLError::MissingRequiredAttribute("ID", node); } - if (name == "Action") { + if (name == "Action" || name == "RemoteAction") { if (!node->Attribute("server_name")) { throw XMLError::MissingRequiredAttribute("server_name", node); } @@ -320,14 +322,17 @@ void XMLParser::collect_eus_actions(const std::string package_name, const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); - for (auto action_node = root->FirstChildElement("Action"); - action_node != nullptr; - action_node = action_node->NextSiblingElement("Action")) + for (auto node = root->FirstChildElement(); + node != nullptr; + node = node->NextSiblingElement()) { - std::string server_name = action_node->Attribute("server_name"); - BOOST_LOG_TRIVIAL(trace) << "collect_eus_actions: transversing " << server_name << "..."; - push_new(format_callback(action_node), callback_definition); - push_new(format_instance(action_node, server_name), instance_creation); + std::string name = node->Name(); + if (name == "Action" || name == "RemoteAction") { + 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); + } } } @@ -517,6 +522,13 @@ std::string XMLParser::generate_headers(const std::string package_name) { headers.push_back(format_action_node(action_node, package_name)); } + for (auto action_node = root->FirstChildElement("RemoteAction"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("RemoteAction")) + { + headers.push_back(format_action_node(action_node, package_name)); + } + for (auto condition_node = root->FirstChildElement("Condition"); condition_node != nullptr; condition_node = condition_node->NextSiblingElement("Condition")) @@ -534,7 +546,7 @@ std::string XMLParser::generate_headers(const std::string package_name) { return gen_template.headers_template(headers); } -std::string XMLParser::generate_action_class(const XMLElement* node, const std::string package_name) { +std::string XMLParser::generate_action_class(const XMLElement* node, const std::string package_name, bool remote=false) { auto format_server_name = [](const XMLElement* node) { return fmt::format(" InputPort(\"server_name\", \"{0}\", \"name of the Action Server\")", node->Attribute("server_name")); @@ -588,6 +600,13 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const std:: provided_output_ports.end()); + if (remote) { + return gen_template.remote_action_class_template( + package_name, node->Attribute("ID"), + node->Attribute("host_name"), + std::atoi(node->Attribute("host_port")), + provided_ports, get_inputs, set_outputs); + } return gen_template.action_class_template(package_name, node->Attribute("ID"), provided_ports, get_inputs, set_outputs); } @@ -649,6 +668,10 @@ std::string XMLParser::generate_main_function(const std::string roscpp_node_name 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")); @@ -670,6 +693,13 @@ std::string XMLParser::generate_main_function(const std::string roscpp_node_name 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")) @@ -729,12 +759,21 @@ std::map XMLParser::generate_all_action_files() { 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; } @@ -766,7 +805,15 @@ std::string XMLParser::generate_cpp_file(const std::string package_name, action_node != nullptr; action_node = action_node->NextSiblingElement("Action")) { - output.append(generate_action_class(action_node, package_name)); + output.append(generate_action_class(action_node, package_name, false)); + output.append("\n\n"); + } + + for (auto action_node = root->FirstChildElement("RemoteAction"); + action_node != nullptr; + action_node = action_node->NextSiblingElement("RemoteAction")) + { + output.append(generate_action_class(action_node, package_name, true)); output.append("\n\n"); } @@ -803,38 +850,30 @@ void XMLParser::push_dependencies(std::vector* message_packages, const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); - for (auto action_node = root->FirstChildElement("Action"); - action_node != nullptr; - action_node = action_node->NextSiblingElement("Action")) - { - push_new(format_action_file(action_node), action_files); - for (auto port_node = action_node->FirstChildElement(); - port_node != nullptr; - port_node = port_node->NextSiblingElement()) - { - maybe_push_message_package(port_node, message_packages); - } - } + for (auto node = root->FirstChildElement(); + node != nullptr; + node = node->NextSiblingElement()) { - for (auto condition_node = root->FirstChildElement("Condition"); - condition_node != nullptr; - condition_node = condition_node->NextSiblingElement("Condition")) - { - push_new(format_service_file(condition_node), service_files); - for (auto port_node = condition_node->FirstChildElement(); - port_node != nullptr; - port_node = port_node->NextSiblingElement()) - { - maybe_push_message_package(port_node, message_packages); - } - } + std::string name = node->Name(); - for (auto subscriber_node = root->FirstChildElement("Subscriber"); - subscriber_node != nullptr; - subscriber_node = subscriber_node->NextSiblingElement("Subscriber")) - { - maybe_push_message_package(subscriber_node, message_packages); - } + if (name == "Action" || name == "RemoteAction") { + push_new(format_action_file(node), action_files); + } + if (name == "Condition") { + push_new(format_service_file(node), service_files); + } + if (name == "Action" || name == "RemoteAction" || name == "Condition") { + for (auto port_node = node->FirstChildElement(); + port_node != nullptr; + port_node = port_node->NextSiblingElement()) + { + maybe_push_message_package(port_node, message_packages); + } + } + if (name == "Subscriber") { + maybe_push_message_package(node, message_packages); + } + } } From 48abde887960826c4afc50631177ba0f18dd50e8 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 19 Apr 2022 08:28:22 +0900 Subject: [PATCH 110/176] (roseus_bt) Add t08_multimaster --- roseus_bt/include/roseus_bt/tutorial_parser.h | 1 + roseus_bt/sample/README.md | 48 ++++++++++++++++++- roseus_bt/sample/models/t08_multimaster.xml | 34 +++++++++++++ roseus_bt/src/create_bt_tutorials.cpp | 1 + 4 files changed, 82 insertions(+), 2 deletions(-) create mode 100644 roseus_bt/sample/models/t08_multimaster.xml diff --git a/roseus_bt/include/roseus_bt/tutorial_parser.h b/roseus_bt/include/roseus_bt/tutorial_parser.h index 8037bf398..e4387bd94 100644 --- a/roseus_bt/include/roseus_bt/tutorial_parser.h +++ b/roseus_bt/include/roseus_bt/tutorial_parser.h @@ -43,6 +43,7 @@ std::string TutorialParser::format_node_body(const XMLElement* node, int padding 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, ' '); diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index cfa72db7f..646c1474e 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -212,7 +212,7 @@ rosrun groot Groot ## t07_xacro -The final example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_bt/sample/models/t07_xacro.xml.xacro illustrates how we can use the xacro package to improve readability and modularity of the model file descriptions. +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 @@ -227,4 +227,48 @@ rosrun xacro xacro t07_xacro_sweep_task.xml.xacro -o t07_xacro_sweep_task.xml ma ``` Note how port variables need to be quoted (e.g.`$${var}`) to use the xacro syntax. -The `{var}` notation also works. \ No newline at end of file +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 `` tag in the ``, and add a `host_name` and `host_port` field to it. As a normal action, it also requires to have the `server_name` field set. + +#### Run the code + +Run the first rosbridge_server: +```bash +roslaunch rosbridge_server rosbridge_websocket.launch +``` + +Run the first roseus server: +```bash +roscd roseus_bt_tutorials/euslisp +roseus t08_multimaster-action-server.l +``` + +Run the second rosbridge_server: +```bash +export ROS_MASTER_URI=http://localhost:11312 +roslaunch rosbridge_server rosbridge_websocket.launch port:=9091 +``` + +Run the second roseus server: +```bash +export ROS_MASTER_URI=http://localhost:11312 +roscd roseus_bt_tutorials/euslisp +roseus t08_multimaster-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/t08_multimaster.xml b/roseus_bt/sample/models/t08_multimaster.xml new file mode 100644 index 000000000..0a8102e06 --- /dev/null +++ b/roseus_bt/sample/models/t08_multimaster.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/roseus_bt/src/create_bt_tutorials.cpp b/roseus_bt/src/create_bt_tutorials.cpp index b0bb17b17..ec9ab824e 100644 --- a/roseus_bt/src/create_bt_tutorials.cpp +++ b/roseus_bt/src/create_bt_tutorials.cpp @@ -51,6 +51,7 @@ int main(int argc, char** argv) 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()); From fe7de02effae669007bcd661d176420d37704f9a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 21 Apr 2022 17:39:43 +0900 Subject: [PATCH 111/176] (roseus_bt) Add ws_action_client base --- .../roseus_bt/eus_remote_action_node.h | 21 ++--- .../include/roseus_bt/ws_action_client.h | 83 +++++++++++++++++++ 2 files changed, 88 insertions(+), 16 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/ws_action_client.h diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h index d15626481..2266ec390 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -4,8 +4,7 @@ #include #include -#include -#include +#include namespace BT @@ -18,17 +17,8 @@ class EusRemoteActionNode : public BT::ActionNodeBase EusRemoteActionNode(const std::string& master, int port, const std::string message_type, const std::string& name, const BT::NodeConfiguration & conf): BT::ActionNodeBase(name, conf), - rbc_(fmt::format("{}:{}", master, std::to_string(port))) - { - server_name_ = getInput("server_name").value(); - goal_topic_ = fmt::format("{}/goal", server_name_); - result_topic_ = fmt::format("{}/result", server_name_); - feedback_topic_ = fmt::format("{}/feedback", server_name_); - - rbc_.addClient("goal_publisher"); - rbc_.addClient("goal_advertiser"); - rbc_.advertise("goal_advertiser", goal_topic_, message_type); - } + action_client_(master, port, getInput("server_name").value(), message_type) + {} public: @@ -62,7 +52,7 @@ class EusRemoteActionNode : public BT::ActionNodeBase } protected: - RosbridgeWsClient rbc_; + RosbridgeActionClient action_client_; std::string server_name_; std::string goal_topic_; @@ -85,9 +75,8 @@ class EusRemoteActionNode : public BT::ActionNodeBase rapidjson::Document action_goal; action_goal.SetObject(); action_goal.AddMember("goal", goal, action_goal.GetAllocator()); - // TODO: add header and goal_id - rbc_.publish(goal_topic_, action_goal); + action_client_.sendGoal(action_goal); } // TODO: wait for result 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..c0f074948 --- /dev/null +++ b/roseus_bt/include/roseus_bt/ws_action_client.h @@ -0,0 +1,83 @@ +#ifndef WS_ACTION_CLIENT_ +#define WS_ACTION_CLIENT_ + +#include +#include + + +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) + { + goal_topic_ = fmt::format("{}/goal", server_name_); + result_topic_ = fmt::format("{}/result", server_name_); + cancel_topic_ = fmt::format("{}/cancel", server_name_); + + action_goal_type_ = fmt::format("{}Goal", action_type); + + rbc_.addClient("goal_publisher"); + rbc_.addClient("goal_advertiser"); + rbc_.addClient("cancel_advertiser"); + rbc_.addClient("result_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_publisher"); + rbc_.removeClient("goal_advertiser"); + rbc_.removeClient("cancel_advertiser"); + rbc_.removeClient("result_subscriber"); + } + + void sendGoal(const rapidjson::Document& goal) { + // TODO: add header and goal_id + is_active_ = true; + rbc_.publish(goal_topic_, goal); + } + + void cancelGoal() { + rapidjson::Document msg; + msg.SetObject(); + is_active_ = false; + rbc_.publish(cancel_topic_, msg); + } + + bool isActive() { + return is_active_; + } + +// getResult +// waitForServer + +protected: + RosbridgeWsClient rbc_; + + bool is_active_; + + std::string server_name_; + std::string goal_topic_; + std::string result_topic_; + std::string cancel_topic_; + + std::string action_goal_type_; + +protected: + + void resultCallback(std::shared_ptr connection, std::shared_ptr in_message) + { + std::string message = in_message->string(); + std::cout << "resultCallback(): Message Received: " << message << std::endl; + is_active_ = false; + } + +}; + +#endif // WS_ACTION_CLIENT_ From 4107e61821c4d04a1bfde3480555be43494c989c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 21 Apr 2022 21:02:39 +0900 Subject: [PATCH 112/176] (roseus_bt) Subscribe remote action results --- .../roseus_bt/eus_remote_action_node.h | 18 ++++++++------ roseus_bt/include/roseus_bt/gen_template.h | 15 ++++++++++-- .../include/roseus_bt/ws_action_client.h | 24 +++++++++++++++---- 3 files changed, 44 insertions(+), 13 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h index 2266ec390..f1fa9919b 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -35,7 +35,8 @@ class EusRemoteActionNode : public BT::ActionNodeBase virtual bool sendGoal(rapidjson::Document& goal) = 0; - // virtual NodeStatus onResult( const ResultType& res) = 0; + virtual NodeStatus onResult(const rapidjson::Value& res) = 0; + // virtual NodeStatus onFailedRequest(FailureCause failure) // { // return NodeStatus::FAILURE; @@ -72,16 +73,19 @@ class EusRemoteActionNode : public BT::ActionNodeBase return NodeStatus::FAILURE; } - rapidjson::Document action_goal; - action_goal.SetObject(); - action_goal.AddMember("goal", goal, action_goal.GetAllocator()); - - action_client_.sendGoal(action_goal); + action_client_.sendGoal(goal); } // TODO: wait for result // TODO: track feedback - return NodeStatus::SUCCESS; + 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() ); } }; diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index eed5b879a..e0728e9a1 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -124,13 +124,13 @@ EusActionNode<%1%::%2%Action>(handle, name, conf) {} return true; } - void onFeedback( const typename FeedbackType::ConstPtr& feedback) override + void onFeedback(const typename FeedbackType::ConstPtr& feedback) override { %5% return; } - NodeStatus onResult( const ResultType& result) override + NodeStatus onResult(const ResultType& result) override { if (result.success) return NodeStatus::SUCCESS; return NodeStatus::FAILURE; @@ -181,6 +181,17 @@ EusRemoteActionNode("%3%", %4%, "%1%/%2%ActionGoal", name, conf) {} %6% return true; } + + 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) % diff --git a/roseus_bt/include/roseus_bt/ws_action_client.h b/roseus_bt/include/roseus_bt/ws_action_client.h index c0f074948..7bfd6a32b 100644 --- a/roseus_bt/include/roseus_bt/ws_action_client.h +++ b/roseus_bt/include/roseus_bt/ws_action_client.h @@ -4,7 +4,6 @@ #include #include - class RosbridgeActionClient { public: @@ -37,10 +36,16 @@ class RosbridgeActionClient rbc_.removeClient("result_subscriber"); } - void sendGoal(const rapidjson::Document& goal) { + 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_, goal); + rbc_.publish(goal_topic_, action_goal); } void cancelGoal() { @@ -54,13 +59,18 @@ class RosbridgeActionClient return is_active_; } -// getResult + rapidjson::Value getResult() { + // TODO: reset result after getting + return result_["msg"].GetObject()["result"].GetObject(); + } + // waitForServer protected: RosbridgeWsClient rbc_; bool is_active_; + rapidjson::Value result_; std::string server_name_; std::string goal_topic_; @@ -75,6 +85,12 @@ class RosbridgeActionClient { std::string message = in_message->string(); std::cout << "resultCallback(): Message Received: " << message << std::endl; + + rapidjson::Document document(rapidjson::kObjectType); + document.Parse(message.c_str()); + rapidjson::Value res(document, document.GetAllocator()); + result_ = res; + is_active_ = false; } From 0be68a401fb89fab1daef702d47370fb4ecac70b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 21 Apr 2022 21:35:14 +0900 Subject: [PATCH 113/176] (roseus_bt) Enable halting remote actions --- roseus_bt/include/roseus_bt/eus_remote_action_node.h | 9 ++++----- roseus_bt/include/roseus_bt/ws_action_client.h | 11 +++++++++-- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h index f1fa9919b..b8346e257 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -44,12 +44,11 @@ class EusRemoteActionNode : public BT::ActionNodeBase virtual void halt() override { - // if( status() == NodeStatus::RUNNING ) - // { - // action_client_->cancelGoal(); - // } + if (action_client_.isActive()) { + action_client_.cancelGoal(); + } setStatus(NodeStatus::IDLE); - // action_client_->waitForResult(); + action_client_.waitForResult(); } protected: diff --git a/roseus_bt/include/roseus_bt/ws_action_client.h b/roseus_bt/include/roseus_bt/ws_action_client.h index 7bfd6a32b..70a1d2ce4 100644 --- a/roseus_bt/include/roseus_bt/ws_action_client.h +++ b/roseus_bt/include/roseus_bt/ws_action_client.h @@ -9,7 +9,8 @@ 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) + server_name_(server_name), + is_active_(false) { goal_topic_ = fmt::format("{}/goal", server_name_); result_topic_ = fmt::format("{}/result", server_name_); @@ -51,7 +52,6 @@ class RosbridgeActionClient void cancelGoal() { rapidjson::Document msg; msg.SetObject(); - is_active_ = false; rbc_.publish(cancel_topic_, msg); } @@ -64,6 +64,13 @@ class RosbridgeActionClient 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(100)); + } + } + // waitForServer protected: From 76ee5e5690989aa2372b1f2f150a9f57b060f4c3 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 13:50:24 +0900 Subject: [PATCH 114/176] (roseus_bt) Fix message type parameter in EusRemoteActionNode --- roseus_bt/include/roseus_bt/gen_template.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index e0728e9a1..753cfd6f2 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -167,7 +167,7 @@ class %2%: public EusRemoteActionNode public: %2%(const std::string& name, const NodeConfiguration& conf): -EusRemoteActionNode("%3%", %4%, "%1%/%2%ActionGoal", name, conf) {} +EusRemoteActionNode("%3%", %4%, "%1%/%2%Action", name, conf) {} static PortsList providedPorts() { From be590541547816f78fa8679c051b166b1f94ba9f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 13:53:23 +0900 Subject: [PATCH 115/176] (roseus_bt) Enable input and output ports in remote actions --- .../roseus_bt/eus_remote_action_node.h | 40 ++++++++- roseus_bt/include/roseus_bt/gen_template.h | 13 ++- .../include/roseus_bt/ws_action_client.h | 10 ++- roseus_bt/include/roseus_bt/xml_parser.h | 82 ++++++++++++++++--- 4 files changed, 125 insertions(+), 20 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h index b8346e257..5a97894a8 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -18,7 +18,12 @@ class EusRemoteActionNode : public BT::ActionNodeBase EusRemoteActionNode(const std::string& master, int port, const std::string message_type, const std::string& name, const BT::NodeConfiguration & conf): BT::ActionNodeBase(name, conf), action_client_(master, port, getInput("server_name").value(), message_type) - {} + { + auto cb = std::bind(&EusRemoteActionNode::feedbackCallback, this, + std::placeholders::_1, + std::placeholders::_2); + action_client_.registerFeedbackCallback(cb); + } public: @@ -33,9 +38,10 @@ class EusRemoteActionNode : public BT::ActionNodeBase }; } - virtual bool sendGoal(rapidjson::Document& goal) = 0; + 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) // { @@ -66,7 +72,7 @@ class EusRemoteActionNode : public BT::ActionNodeBase rapidjson::Document goal; goal.SetObject(); - bool valid_goal = sendGoal(goal); + bool valid_goal = sendGoal(&goal); if( !valid_goal ) { return NodeStatus::FAILURE; @@ -86,6 +92,34 @@ class EusRemoteActionNode : public BT::ActionNodeBase return onResult( action_client_.getResult() ); } + + // port related + // TODO: avoid unnecessary dumping & parsing + // cannot use rapidjson::Value directly because it is not `CopyConstructible' + // 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(); + std::cout << "feedbackCallback(): Message Received: " << message << std::endl; + + 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) + { + rapidjson::StringBuffer strbuf; + rapidjson::Writer writer(strbuf); + rapidjson::Document document; + document.CopyFrom(message[name.c_str()], document.GetAllocator()); + document.Accept(writer); + setOutput(name, strbuf.GetString()); + } }; diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 753cfd6f2..862b84ee4 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -176,12 +176,20 @@ EusRemoteActionNode("%3%", %4%, "%1%/%2%Action", name, conf) {} }; } - bool sendGoal(rapidjson::Document& goal) override + bool sendGoal(rapidjson::Document* goal) override { + std::string json; + rapidjson::Document document; %6% return true; } + void onFeedback(const rapidjson::Value& feedback) override + { +%7% + return; + } + NodeStatus onResult(const rapidjson::Value& result) override { if (result.HasMember("success") && @@ -200,7 +208,8 @@ EusRemoteActionNode("%3%", %4%, "%1%/%2%Action", name, conf) {} host_name % host_port % boost::algorithm::join(provided_ports, ",\n") % - boost::algorithm::join(get_inputs, "\n"); + boost::algorithm::join(get_inputs, "\n") % + boost::algorithm::join(set_outputs, "\n"); return bfmt.str(); } diff --git a/roseus_bt/include/roseus_bt/ws_action_client.h b/roseus_bt/include/roseus_bt/ws_action_client.h index 70a1d2ce4..575ddb184 100644 --- a/roseus_bt/include/roseus_bt/ws_action_client.h +++ b/roseus_bt/include/roseus_bt/ws_action_client.h @@ -15,13 +15,14 @@ class RosbridgeActionClient 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_publisher"); 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, @@ -31,10 +32,14 @@ class RosbridgeActionClient } ~RosbridgeActionClient() { - rbc_.removeClient("goal_publisher"); 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) { @@ -83,6 +88,7 @@ class RosbridgeActionClient std::string goal_topic_; std::string result_topic_; std::string cancel_topic_; + std::string feedback_topic_; std::string action_goal_type_; diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index ad7859a8c..9e487521a 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -64,8 +64,8 @@ class XMLParser 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, - bool remote); + 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_subscriber_class(const XMLElement* node); std::string generate_main_function(const std::string roscpp_node_name, const std::string xml_filename); @@ -546,7 +546,7 @@ std::string XMLParser::generate_headers(const std::string package_name) { return gen_template.headers_template(headers); } -std::string XMLParser::generate_action_class(const XMLElement* node, const std::string package_name, bool remote=false) { +std::string XMLParser::generate_action_class(const XMLElement* node, const std::string package_name) { auto format_server_name = [](const XMLElement* node) { return fmt::format(" InputPort(\"server_name\", \"{0}\", \"name of the Action Server\")", node->Attribute("server_name")); @@ -568,7 +568,6 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const std:: node->Attribute("name")); }; - std::vector provided_input_ports; std::vector provided_output_ports; std::vector get_inputs; @@ -600,17 +599,74 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const std:: provided_output_ports.end()); - if (remote) { - return gen_template.remote_action_class_template( - package_name, node->Attribute("ID"), - node->Attribute("host_name"), - std::atoi(node->Attribute("host_port")), - provided_ports, get_inputs, set_outputs); - } 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_server_name = [](const XMLElement* node) { + return fmt::format(" InputPort(\"server_name\", \"{0}\", \"name of the Action Server\")", + node->Attribute("server_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(R"( + getInput("{0}", json); + document.Parse(json.c_str()); + rapidjson::Value {0}(document, document.GetAllocator()); + goal->AddMember("{0}", {0}, goal->GetAllocator());)", + node->Attribute("name")); + }; + auto format_set_output = [](const XMLElement* node) { + return fmt::format(" setOutputFromMessage(\"{0}\", feedback);", + 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.remote_action_class_template( + package_name, node->Attribute("ID"), + node->Attribute("host_name"), + std::atoi(node->Attribute("host_port")), + provided_ports, get_inputs, set_outputs); +} + std::string XMLParser::generate_condition_class(const XMLElement* node, const std::string package_name) { auto format_service_name = [](const XMLElement* node) { return fmt::format(" InputPort(\"service_name\", \"{0}\", \"name of the ROS service\")", @@ -805,7 +861,7 @@ std::string XMLParser::generate_cpp_file(const std::string package_name, action_node != nullptr; action_node = action_node->NextSiblingElement("Action")) { - output.append(generate_action_class(action_node, package_name, false)); + output.append(generate_action_class(action_node, package_name)); output.append("\n\n"); } @@ -813,7 +869,7 @@ std::string XMLParser::generate_cpp_file(const std::string package_name, action_node != nullptr; action_node = action_node->NextSiblingElement("RemoteAction")) { - output.append(generate_action_class(action_node, package_name, true)); + output.append(generate_remote_action_class(action_node, package_name)); output.append("\n\n"); } From 70ba8d0387a6ba517d7e42b1f33dd028c83359b9 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 14:59:14 +0900 Subject: [PATCH 116/176] (roseus_bt) Allow to set host_name and host_port dynamically --- .../roseus_bt/eus_remote_action_node.h | 11 ++++++++--- roseus_bt/include/roseus_bt/gen_template.h | 12 ++++-------- roseus_bt/include/roseus_bt/xml_parser.h | 19 ++++++++++++++----- roseus_bt/sample/README.md | 1 + roseus_bt/sample/models/t08_multimaster.xml | 18 +++++++----------- 5 files changed, 34 insertions(+), 27 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h index 5a97894a8..d87c75d86 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -15,9 +15,12 @@ class EusRemoteActionNode : public BT::ActionNodeBase { protected: - EusRemoteActionNode(const std::string& master, int port, const std::string message_type, const std::string& name, const BT::NodeConfiguration & conf): + EusRemoteActionNode(const std::string message_type, const std::string& name, const BT::NodeConfiguration & conf): BT::ActionNodeBase(name, conf), - action_client_(master, port, getInput("server_name").value(), message_type) + 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, @@ -34,7 +37,9 @@ class EusRemoteActionNode : public BT::ActionNodeBase static PortsList providedPorts() { return { - InputPort("server_name", "name of the Action Server") + 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") }; } diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 862b84ee4..47c1727f1 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -26,7 +26,6 @@ class GenTemplate std::vector get_inputs, std::vector set_outputs); std::string remote_action_class_template(std::string package_name, std::string nodeID, - std::string host_name, int host_port, std::vector provided_ports, std::vector get_inputs, std::vector set_outputs); @@ -156,7 +155,6 @@ EusActionNode<%1%::%2%Action>(handle, name, conf) {} std::string GenTemplate::remote_action_class_template( std::string package_name, std::string nodeID, - std::string host_name, int host_port, std::vector provided_ports, std::vector get_inputs, std::vector set_outputs) @@ -167,12 +165,12 @@ class %2%: public EusRemoteActionNode public: %2%(const std::string& name, const NodeConfiguration& conf): -EusRemoteActionNode("%3%", %4%, "%1%/%2%Action", name, conf) {} +EusRemoteActionNode("%1%/%2%Action", name, conf) {} static PortsList providedPorts() { return { -%5% +%3% }; } @@ -180,13 +178,13 @@ EusRemoteActionNode("%3%", %4%, "%1%/%2%Action", name, conf) {} { std::string json; rapidjson::Document document; -%6% +%4% return true; } void onFeedback(const rapidjson::Value& feedback) override { -%7% +%5% return; } @@ -205,8 +203,6 @@ EusRemoteActionNode("%3%", %4%, "%1%/%2%Action", name, conf) {} boost::format bfmt = boost::format(fmt_string) % package_name % nodeID % - host_name % - host_port % boost::algorithm::join(provided_ports, ",\n") % boost::algorithm::join(get_inputs, "\n") % boost::algorithm::join(set_outputs, "\n"); diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 9e487521a..5016ca052 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -608,6 +608,14 @@ std::string XMLParser::generate_remote_action_class(const XMLElement* node, cons return fmt::format(" InputPort(\"server_name\", \"{0}\", \"name of the Action Server\")", node->Attribute("server_name")); }; + auto format_host_name = [](const XMLElement* node) { + return fmt::format(" InputPort(\"host_name\", \"{0}\", \"name of the rosbridge_server host\")", + node->Attribute("host_name")); + }; + auto format_host_port = [](const XMLElement* node) { + return fmt::format(" InputPort(\"host_port\", {0}, \"port of the rosbridge_server host\")", + node->Attribute("host_port")); + }; auto format_input_port = [](const XMLElement* node) { return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); @@ -635,6 +643,10 @@ std::string XMLParser::generate_remote_action_class(const XMLElement* node, cons 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; @@ -660,11 +672,8 @@ std::string XMLParser::generate_remote_action_class(const XMLElement* node, cons provided_output_ports.end()); - return gen_template.remote_action_class_template( - package_name, node->Attribute("ID"), - node->Attribute("host_name"), - std::atoi(node->Attribute("host_port")), - provided_ports, get_inputs, set_outputs); + 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) { diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index 646c1474e..75ece9fd6 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -235,6 +235,7 @@ This example shows how to use the rosbridge interface to assign different hosts 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 `` tag in the ``, and add a `host_name` and `host_port` field to it. As a normal action, it also requires to have the `server_name` field set. +The `host_name` and `host_port` can be set either statically (in the `TreeNodesModel` definition) or dynamically (within the `BehaviorTree` node instance), although we recommend to set it statically to avoid possible runtime errors. #### Run the code diff --git a/roseus_bt/sample/models/t08_multimaster.xml b/roseus_bt/sample/models/t08_multimaster.xml index 0a8102e06..601406ad4 100644 --- a/roseus_bt/sample/models/t08_multimaster.xml +++ b/roseus_bt/sample/models/t08_multimaster.xml @@ -5,15 +5,13 @@ - - - + - - - - + + @@ -24,10 +22,8 @@ - - + + From 483ecabc5dc6da32438eb12a6228700e0e5362dc Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 17:24:49 +0900 Subject: [PATCH 117/176] (roseus_bt) Add EusRemoteCondition node --- roseus_bt/include/roseus_bt/eus_nodes.h | 1 + .../roseus_bt/eus_remote_condition_node.h | 105 +++++++++++++++ roseus_bt/include/roseus_bt/gen_template.h | 50 +++++++ .../include/roseus_bt/ws_service_client.h | 75 +++++++++++ roseus_bt/include/roseus_bt/xml_parser.h | 125 +++++++++++++++--- 5 files changed, 336 insertions(+), 20 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/eus_remote_condition_node.h create mode 100644 roseus_bt/include/roseus_bt/ws_service_client.h diff --git a/roseus_bt/include/roseus_bt/eus_nodes.h b/roseus_bt/include/roseus_bt/eus_nodes.h index f4b43362a..16e9a3f7b 100644 --- a/roseus_bt/include/roseus_bt/eus_nodes.h +++ b/roseus_bt/include/roseus_bt/eus_nodes.h @@ -5,6 +5,7 @@ #include "eus_condition_node.h" #include "eus_subscriber_node.h" #include "eus_remote_action_node.h" +#include "eus_remote_condition_node.h" #endif // BEHAVIOR_TREE_EUS_NODES_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..64120e924 --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_remote_condition_node.h @@ -0,0 +1,105 @@ +#ifndef BEHAVIOR_TREE_EUS_REMOTE_CONDITION_NODE_HPP_ +#define BEHAVIOR_TREE_EUS_REMOTE_CONDITION_NODE_HPP_ + +#include +#include +#include + + +namespace BT +{ + +// Helper Node to call a rosbridge websocket inside a BT::ActionNode +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: + + 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: + 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); + } + + if (service_client_.isActive()) { + return NodeStatus::RUNNING; + } + + 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_EUS_REMOTE_CONDITION_NODE_HPP_ diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 47c1727f1..cac6258c3 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -32,6 +32,9 @@ class GenTemplate 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::string message_field); std::string main_function_template(std::string roscpp_node_name, @@ -258,6 +261,53 @@ class %2%: public EusConditionNode<%1%::%2%> } +std::string GenTemplate::remote_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 %1%: public EusRemoteConditionNode +{ + +public: + %1%(const std::string& name, const NodeConfiguration& conf): + EusRemoteConditionNode(name, conf) {} + + static PortsList providedPorts() + { + return { +%2% + }; + } + + void sendRequest(rapidjson::Document *request) override + { + std::string json; + rapidjson::Document document; +%3% + } + + 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) % + nodeID % + boost::algorithm::join(provided_ports, ",\n") % + boost::algorithm::join(get_inputs, "\n"); + + return bfmt.str(); +} + + std::string GenTemplate::subscriber_class_template(std::string nodeID, std::string message_type, std::string message_field) { if (!message_field.empty()) { 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..69b54db95 --- /dev/null +++ b/roseus_bt/include/roseus_bt/ws_service_client.h @@ -0,0 +1,75 @@ +#ifndef WS_SERVICE_CLIENT_ +#define WS_SERVICE_CLIENT_ + +#include +#include + +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(100)); + } + } + +// 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(); + std::cout << "serviceResponseCallback(): Message Received: " << message << std::endl; + + 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); + } + +}; + +#endif // WS_SERVICE_CLIENT_ diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 5016ca052..7984041ac 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -67,6 +67,7 @@ class XMLParser 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_main_function(const std::string roscpp_node_name, const std::string xml_filename); @@ -127,6 +128,7 @@ void XMLParser::check_xml_file(std::string filename) { if (name != "Action" && name != "RemoteAction" && name != "Condition" && + name != "RemoteCondition" && name != "Subscriber" && name != "SubTree") { throw XMLError::UnknownNode(node); @@ -143,7 +145,7 @@ void XMLParser::check_xml_file(std::string filename) { check_push(node, &actions, &duplicated_nodes); } - if (name == "Condition") { + if (name == "Condition" || name == "RemoteCondition") { if (!node->Attribute("service_name")) { throw XMLError::MissingRequiredAttribute("service_name", node); } @@ -390,24 +392,27 @@ void XMLParser::collect_eus_conditions(const std::string package_name, const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); - for (auto condition_node = root->FirstChildElement("Condition"); - condition_node != nullptr; - condition_node = condition_node->NextSiblingElement("Condition")) + for (auto node = root->FirstChildElement(); + node != nullptr; + node = node->NextSiblingElement()) { - std::string service_name = condition_node->Attribute("service_name"); - BOOST_LOG_TRIVIAL(trace) << "collect_eus_conditions: transversing " << service_name << "..."; - - if (is_reactive(condition_node)) { - if (parallel_callback_definition != NULL && - parallel_instance_creation != NULL) { - push_new(format_callback(condition_node), parallel_callback_definition); - push_new(format_instance(condition_node, service_name), parallel_instance_creation); + std::string name = node->Name(); + if (name == "Condition" || name == "RemoteCondition") { + std::string service_name = node->Attribute("service_name"); + BOOST_LOG_TRIVIAL(trace) << "collect_eus_conditions: transversing " << 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(condition_node), callback_definition); - push_new(format_instance(condition_node, service_name), 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); + } } } } @@ -536,6 +541,13 @@ std::string XMLParser::generate_headers(const std::string package_name) { headers.push_back(format_condition_node(condition_node, package_name)); } + for (auto condition_node = root->FirstChildElement("RemoteCondition"); + condition_node != nullptr; + condition_node = condition_node->NextSiblingElement("RemoteCondition")) + { + headers.push_back(format_condition_node(condition_node, package_name)); + } + for (auto subscriber_node = root->FirstChildElement("Subscriber"); subscriber_node != nullptr; subscriber_node = subscriber_node->NextSiblingElement("Subscriber")) @@ -707,6 +719,53 @@ std::string XMLParser::generate_condition_class(const XMLElement* node, const st provided_ports, get_inputs); } +std::string XMLParser::generate_remote_condition_class(const XMLElement* node, const std::string package_name) { + auto format_service_name = [](const XMLElement* node) { + return fmt::format(" InputPort(\"service_name\", \"{0}\", \"name of the ROS service\")", + node->Attribute("service_name")); + }; + auto format_host_name = [](const XMLElement* node) { + return fmt::format(" InputPort(\"host_name\", \"{0}\", \"name of the rosbridge_server host\")", + node->Attribute("host_name")); + }; + auto format_host_port = [](const XMLElement* node) { + return fmt::format(" InputPort(\"host_port\", {0}, \"port of the rosbridge_server host\")", + node->Attribute("host_port")); + }; + 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(R"( + getInput("{0}", json); + document.Parse(json.c_str()); + rapidjson::Value {0}(document, document.GetAllocator()); + request->AddMember("{0}", {0}, request->GetAllocator());)", + 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_input(port_node)); + } + + 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("type"); @@ -726,7 +785,6 @@ std::string XMLParser::generate_subscriber_class(const XMLElement* node) { format_field(node)); } - std::string XMLParser::generate_main_function(const std::string roscpp_node_name, const std::string xml_filename) { auto format_action_node = [](const XMLElement* node) { @@ -741,6 +799,10 @@ std::string XMLParser::generate_main_function(const std::string roscpp_node_name 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")); @@ -772,6 +834,13 @@ std::string XMLParser::generate_main_function(const std::string roscpp_node_name 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")) @@ -855,6 +924,13 @@ std::map XMLParser::generate_all_service_files() { { 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; } @@ -890,6 +966,14 @@ std::string XMLParser::generate_cpp_file(const std::string 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")) @@ -924,10 +1008,11 @@ void XMLParser::push_dependencies(std::vector* message_packages, if (name == "Action" || name == "RemoteAction") { push_new(format_action_file(node), action_files); } - if (name == "Condition") { + if (name == "Condition" || name == "RemoteCondition") { push_new(format_service_file(node), service_files); } - if (name == "Action" || name == "RemoteAction" || name == "Condition") { + if (name == "Action" || name == "RemoteAction" || + name == "Condition" || name == "RemoteCondition") { for (auto port_node = node->FirstChildElement(); port_node != nullptr; port_node = port_node->NextSiblingElement()) From e53cd40174a98a7183a208359f1db7da372a655e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 17:25:18 +0900 Subject: [PATCH 118/176] (roseus_bt) Indent --- roseus_bt/include/roseus_bt/eus_remote_action_node.h | 3 --- roseus_bt/include/roseus_bt/xml_parser.h | 6 +++--- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h index d87c75d86..e51180b23 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -86,8 +86,6 @@ class EusRemoteActionNode : public BT::ActionNodeBase action_client_.sendGoal(goal); } - // TODO: wait for result - // TODO: track feedback if (action_client_.isActive()) { return NodeStatus::RUNNING; } @@ -129,7 +127,6 @@ class EusRemoteActionNode : public BT::ActionNodeBase /// Method to register the service into a factory. -/// It gives you the opportunity to set the ros::NodeHandle. template static void RegisterRemoteAction(BT::BehaviorTreeFactory& factory, const std::string& registration_ID) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 7984041ac..c18db5535 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -620,7 +620,7 @@ std::string XMLParser::generate_remote_action_class(const XMLElement* node, cons return fmt::format(" InputPort(\"server_name\", \"{0}\", \"name of the Action Server\")", node->Attribute("server_name")); }; - auto format_host_name = [](const XMLElement* node) { + auto format_host_name = [](const XMLElement* node) { return fmt::format(" InputPort(\"host_name\", \"{0}\", \"name of the rosbridge_server host\")", node->Attribute("host_name")); }; @@ -655,9 +655,9 @@ std::string XMLParser::generate_remote_action_class(const XMLElement* node, cons std::vector set_outputs; provided_input_ports.push_back(format_server_name(node)); - if (node->Attribute("host_name")) { + if (node->Attribute("host_name")) { provided_input_ports.push_back(format_host_name(node));} - if (node->Attribute("host_port")) { + if (node->Attribute("host_port")) { provided_input_ports.push_back(format_host_port(node));} for (auto port_node = node->FirstChildElement(); From 916a6a0d1446891edae652ab798966171beed8cb Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 17:39:46 +0900 Subject: [PATCH 119/176] (roseus_bt) Reduce duplicate formatting functions --- roseus_bt/include/roseus_bt/xml_parser.h | 64 ++++++++++++------------ 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index c18db5535..55fab9586 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -61,6 +61,10 @@ class XMLParser 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 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); @@ -458,6 +462,34 @@ std::string XMLParser::format_message_description(const XMLElement* port_node) { 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::generate_action_file_contents(const XMLElement* node) { std::vector goal, feedback; @@ -559,10 +591,6 @@ std::string XMLParser::generate_headers(const std::string package_name) { } std::string XMLParser::generate_action_class(const XMLElement* node, const std::string package_name) { - auto format_server_name = [](const XMLElement* node) { - return fmt::format(" InputPort(\"server_name\", \"{0}\", \"name of the Action Server\")", - node->Attribute("server_name")); - }; auto format_input_port = [](const XMLElement* node) { return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); @@ -616,18 +644,6 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const std:: } std::string XMLParser::generate_remote_action_class(const XMLElement* node, const std::string package_name) { - auto format_server_name = [](const XMLElement* node) { - return fmt::format(" InputPort(\"server_name\", \"{0}\", \"name of the Action Server\")", - node->Attribute("server_name")); - }; - auto format_host_name = [](const XMLElement* node) { - return fmt::format(" InputPort(\"host_name\", \"{0}\", \"name of the rosbridge_server host\")", - node->Attribute("host_name")); - }; - auto format_host_port = [](const XMLElement* node) { - return fmt::format(" InputPort(\"host_port\", {0}, \"port of the rosbridge_server host\")", - node->Attribute("host_port")); - }; auto format_input_port = [](const XMLElement* node) { return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); @@ -689,10 +705,6 @@ std::string XMLParser::generate_remote_action_class(const XMLElement* node, cons } std::string XMLParser::generate_condition_class(const XMLElement* node, const std::string package_name) { - auto format_service_name = [](const XMLElement* node) { - return fmt::format(" InputPort(\"service_name\", \"{0}\", \"name of the ROS service\")", - node->Attribute("service_name")); - }; auto format_input_port = [](const XMLElement* node) { return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); @@ -720,18 +732,6 @@ std::string XMLParser::generate_condition_class(const XMLElement* node, const st } std::string XMLParser::generate_remote_condition_class(const XMLElement* node, const std::string package_name) { - auto format_service_name = [](const XMLElement* node) { - return fmt::format(" InputPort(\"service_name\", \"{0}\", \"name of the ROS service\")", - node->Attribute("service_name")); - }; - auto format_host_name = [](const XMLElement* node) { - return fmt::format(" InputPort(\"host_name\", \"{0}\", \"name of the rosbridge_server host\")", - node->Attribute("host_name")); - }; - auto format_host_port = [](const XMLElement* node) { - return fmt::format(" InputPort(\"host_port\", {0}, \"port of the rosbridge_server host\")", - node->Attribute("host_port")); - }; auto format_input_port = [](const XMLElement* node) { return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); From 0d7992d5c55c50ef6728be4efb39dae49e4f4ae0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 18:47:59 +0900 Subject: [PATCH 120/176] (roseus_bt) Add default file logger --- roseus_bt/include/roseus_bt/gen_template.h | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index cac6258c3..1c804df27 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -89,6 +89,7 @@ std::string GenTemplate::headers_template(std::vector headers) { #include #include #include +#include %1% @@ -364,7 +365,11 @@ int main(int argc, char **argv) %3%%4%%5% %2% + std::string timestamp = std::to_string(ros::Time::now().toNSec()); + std::string log_filename(fmt::format("%6%", timestamp)); + StdCoutLogger logger_cout(tree); + FileLogger logger_file(tree, log_filename.c_str()); PublisherZMQ publisher_zmq(tree); NodeStatus status = NodeStatus::IDLE; @@ -377,6 +382,7 @@ int main(int argc, char **argv) sleep_time.sleep(); } + std::cout << "Writed log to file: " << log_filename << std::endl; return 0; } )"; @@ -385,12 +391,16 @@ int main(int argc, char **argv) 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) % format_ros_init() % format_create_tree() % boost::algorithm::join(register_actions, "\n") % boost::algorithm::join(register_conditions, "\n") % - boost::algorithm::join(register_subscribers, "\n"); + boost::algorithm::join(register_subscribers, "\n") % + file_format.str(); return bfmt.str(); } From 635fc22659efbcdc8cd9b83936d270a24e062ca4 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 19:21:57 +0900 Subject: [PATCH 121/176] (roseus_bt) Add 'update_field_name' to set output ports once at a time --- roseus_bt/euslisp/nodes.l | 1 + roseus_bt/include/roseus_bt/eus_remote_action_node.h | 2 ++ roseus_bt/include/roseus_bt/gen_template.h | 1 + roseus_bt/include/roseus_bt/xml_parser.h | 5 +++-- 4 files changed, 7 insertions(+), 2 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 04e8ae91f..4968ad560 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -88,6 +88,7 @@ (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) diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h index e51180b23..fd202f4f1 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -116,6 +116,8 @@ class EusRemoteActionNode : public BT::ActionNodeBase void setOutputFromMessage(const std::string& name, const rapidjson::Value& message) { + if (message["update_field_name"].GetString() != name) return; + rapidjson::StringBuffer strbuf; rapidjson::Writer writer(strbuf); rapidjson::Document document; diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 1c804df27..dff9c4d64 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -57,6 +57,7 @@ std::string GenTemplate::action_file_template(std::vector goal, --- bool success --- +string update_field_name %2% )"; diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 55fab9586..51e65971e 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -604,8 +604,9 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const std:: node->Attribute("name")); }; auto format_set_output = [](const XMLElement* node) { - return fmt::format(" setOutput(\"{0}\", feedback->{0});", - node->Attribute("name")); + return fmt::format( + " if (feedback->update_field_name == \"{0}\") setOutput(\"{0}\", feedback->{0});", + node->Attribute("name")); }; std::vector provided_input_ports; From 1c86e7c266c26e5656ed3ea469b339bb72d76ec1 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 19:30:18 +0900 Subject: [PATCH 122/176] (roseus_bt) Move #DEBUG definition to generated files --- roseus_bt/include/roseus_bt/eus_remote_action_node.h | 3 ++- roseus_bt/include/roseus_bt/gen_template.h | 1 + roseus_bt/include/roseus_bt/ws_action_client.h | 2 ++ roseus_bt/include/roseus_bt/ws_service_client.h | 2 ++ 4 files changed, 7 insertions(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h index fd202f4f1..3d5caa39a 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -1,6 +1,5 @@ #ifndef BEHAVIOR_TREE_EUS_REMOTE_ACTION_NODE_HPP_ #define BEHAVIOR_TREE_EUS_REMOTE_ACTION_NODE_HPP_ -#define DEBUG #include #include @@ -105,7 +104,9 @@ class EusRemoteActionNode : public BT::ActionNodeBase 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()); diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index dff9c4d64..37f156626 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -87,6 +87,7 @@ std::string GenTemplate::headers_template(std::vector headers) { std::string fmt_string = 1 + R"( #include +#define DEBUG // rosbridgecpp logging #include #include #include diff --git a/roseus_bt/include/roseus_bt/ws_action_client.h b/roseus_bt/include/roseus_bt/ws_action_client.h index 575ddb184..2ec0333ee 100644 --- a/roseus_bt/include/roseus_bt/ws_action_client.h +++ b/roseus_bt/include/roseus_bt/ws_action_client.h @@ -97,7 +97,9 @@ class RosbridgeActionClient 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()); diff --git a/roseus_bt/include/roseus_bt/ws_service_client.h b/roseus_bt/include/roseus_bt/ws_service_client.h index 69b54db95..f8ca850f4 100644 --- a/roseus_bt/include/roseus_bt/ws_service_client.h +++ b/roseus_bt/include/roseus_bt/ws_service_client.h @@ -59,7 +59,9 @@ class RosbridgeServiceClient 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()); From 7f1c9341ae8c4fc58fcbb33ae5ec5a5bab5ea24a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 20:04:58 +0900 Subject: [PATCH 123/176] (roseus_bt) Add generate_eus_remote_action_server and generate_eus_remote_condition_server --- roseus_bt/include/roseus_bt/xml_parser.h | 100 +++++++++++++++++++++-- 1 file changed, 95 insertions(+), 5 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 51e65971e..b61cbb85e 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -49,9 +49,21 @@ class XMLParser std::function output_fn); void collect_param_list(const XMLElement* node, std::vector* param_list, std::vector* output_list); + void collect_eus_actions(const std::string package_name, + std::vector* callback_definition, + std::vector* instance_creation, + std::string host_name, + std::string host_port); 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, + std::string host_name, + std::string host_port); void collect_eus_conditions(const std::string package_name, std::vector* callback_definition, std::vector* instance_creation, @@ -89,8 +101,13 @@ class XMLParser std::vector* service_files); 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, + std::string host_name, + std::string host_port); 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, + std::string host_name, + std::string host_port); }; GenTemplate XMLParser::gen_template = GenTemplate(); @@ -279,8 +296,10 @@ void XMLParser::collect_param_list(const XMLElement* node, void XMLParser::collect_eus_actions(const std::string package_name, std::vector* callback_definition, - std::vector* instance_creation) { - + std::vector* instance_creation, + std::string host_name, + std::string host_port) +{ auto format_action_param = [](const XMLElement* node) { return fmt::format("({0} (send goal :goal :{0}))", node->Attribute("name")); }; @@ -335,6 +354,12 @@ void XMLParser::collect_eus_actions(const std::string package_name, std::string name = node->Name(); if (name == "Action" || name == "RemoteAction") { std::string server_name = node->Attribute("server_name"); + if ((name == "RemoteAction" && host_name.size() && host_port.size()) && + (node->Attribute("host_name") != host_name || + node->Attribute("host_port") != host_port)) { + 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); @@ -342,12 +367,21 @@ void XMLParser::collect_eus_actions(const std::string package_name, } } +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) { - + std::vector* parallel_instance_creation, + std::string host_name, + std::string host_port) +{ auto format_condition_param = [](const XMLElement* node) { return fmt::format("({0} (send request :{0}))", node->Attribute("name")); }; @@ -403,6 +437,13 @@ void XMLParser::collect_eus_conditions(const std::string package_name, std::string name = node->Name(); if (name == "Condition" || name == "RemoteCondition") { std::string service_name = node->Attribute("service_name"); + if ((name == "RemoteCondition" && host_name.size() && host_port.size()) && + (node->Attribute("host_name") != host_name || + node->Attribute("host_port") != host_port)) { + BOOST_LOG_TRIVIAL(trace) << "collect_eus_conditions: skipping " << service_name << "..."; + continue; + } + BOOST_LOG_TRIVIAL(trace) << "collect_eus_conditions: transversing " << service_name << "..."; if (is_reactive(node)) { @@ -422,6 +463,17 @@ void XMLParser::collect_eus_conditions(const std::string package_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 XMLElement* node, std::vector* message_packages) { std::string msg_type = node->Attribute("type"); @@ -872,6 +924,25 @@ std::string XMLParser::generate_eus_action_server(const std::string package_name load_files); } +std::string XMLParser::generate_eus_remote_action_server(const std::string package_name, + std::string host_name, + std::string host_port) +{ + std::vector callback_definition; + std::vector instance_creation; + std::vector load_files; + + collect_eus_actions(package_name, &callback_definition, &instance_creation, host_name, host_port); + collect_eus_conditions(package_name, &callback_definition, &instance_creation, + NULL, NULL, host_name, host_port); + + 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; @@ -887,6 +958,25 @@ std::string XMLParser::generate_eus_condition_server(const std::string package_n load_files); } +std::string XMLParser::generate_eus_remote_condition_server(const std::string package_name, + std::string host_name, + std::string host_port) +{ + std::vector callback_definition; + std::vector instance_creation; + std::vector load_files; + + collect_eus_conditions(package_name, NULL, NULL, + &callback_definition, &instance_creation, + host_name, host_port); + + 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")); From 492d25a8ee650c407cc4c81539212d51a5598637 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 21:31:22 +0900 Subject: [PATCH 124/176] (roseus_bt) Split euslisp files for each host --- .../include/roseus_bt/package_generator.h | 44 +++--- roseus_bt/include/roseus_bt/xml_parser.h | 127 +++++++++++++----- 2 files changed, 121 insertions(+), 50 deletions(-) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 9efa6567f..afb4ff0f6 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -185,17 +185,21 @@ void PackageGenerator::write_eus_action_server(Parser* parser, std::string base_dir = fmt::format("{}/euslisp", package_name); boost::filesystem::create_directories(base_dir); - std::string dest_file = fmt::format("{}/{}-action-server.l", base_dir, target_filename); - if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) - return; + std::map server_files = parser->generate_all_eus_action_servers(package_name); - BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; - std::string body = parser->generate_eus_action_server(package_name); - if (body.empty()) return; + for (auto it=server_files.begin(); it!=server_files.end(); ++it) { + std::string remote_host = it->first; + std::string body = it->second; + std::string dest_file = fmt::format("{}/{}{}-action-server.l", + base_dir, target_filename, remote_host); + if (body.empty()) continue; + if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) continue; - std::ofstream output_file(dest_file); - output_file << body; - output_file.close(); + BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; + std::ofstream output_file(dest_file); + output_file << body; + output_file.close(); + } } template @@ -204,17 +208,21 @@ void PackageGenerator::write_eus_condition_server(Parser* parser, std::string base_dir = fmt::format("{}/euslisp", package_name); boost::filesystem::create_directories(base_dir); - std::string dest_file = fmt::format("{}/{}-condition-server.l", base_dir, target_filename); - if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) - return; + std::map server_files = parser->generate_all_eus_condition_servers(package_name); - std::string body = parser->generate_eus_condition_server(package_name); - if (body.empty()) return; + for (auto it=server_files.begin(); it!=server_files.end(); ++it) { + std::string remote_host = it->first; + std::string body = it->second; + std::string dest_file = fmt::format("{}/{}{}-condition-server.l", + base_dir, target_filename, remote_host); + if (body.empty()) continue; + 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(); + BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; + std::ofstream output_file(dest_file); + output_file << body; + output_file.close(); + } } template diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index b61cbb85e..dee5f09e1 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -49,11 +49,11 @@ class XMLParser 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, - std::string host_name, - std::string host_port); + const std::string remote_host); void collect_eus_actions(const std::string package_name, std::vector* callback_definition, std::vector* instance_creation); @@ -62,8 +62,7 @@ class XMLParser std::vector* instance_creation, std::vector* parallel_callback_definition, std::vector* parallel_instance_creation, - std::string host_name, - std::string host_port); + const std::string remote_host); void collect_eus_conditions(const std::string package_name, std::vector* callback_definition, std::vector* instance_creation, @@ -77,6 +76,7 @@ class XMLParser 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 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); @@ -89,25 +89,25 @@ class XMLParser 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 xml_filename); void push_dependencies(std::vector* message_packages, std::vector* action_files, std::vector* service_files); - - 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, - std::string host_name, - std::string host_port); - 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, - std::string host_name, - std::string host_port); }; GenTemplate XMLParser::gen_template = GenTemplate(); @@ -294,11 +294,28 @@ void XMLParser::collect_param_list(const XMLElement* node, 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, - std::string host_name, - std::string host_port) + const std::string remote_host) { auto format_action_param = [](const XMLElement* node) { return fmt::format("({0} (send goal :goal :{0}))", node->Attribute("name")); @@ -354,9 +371,14 @@ void XMLParser::collect_eus_actions(const std::string package_name, std::string name = node->Name(); if (name == "Action" || name == "RemoteAction") { std::string server_name = node->Attribute("server_name"); - if ((name == "RemoteAction" && host_name.size() && host_port.size()) && - (node->Attribute("host_name") != host_name || - node->Attribute("host_port") != host_port)) { + if (name == "RemoteAction" && remote_host.empty() && + node->Attribute("host_name") && node->Attribute("host_port")) { + // host is clear; callback should only be defined when remote_host matches + BOOST_LOG_TRIVIAL(trace) << "collect_eus_actions: skipping " << server_name << "..."; + continue; + } + if (name == "RemoteAction" && !remote_host.empty() && + format_remote_host(node) != remote_host) { BOOST_LOG_TRIVIAL(trace) << "collect_eus_actions: skipping " << server_name << "..."; continue; } @@ -371,7 +393,7 @@ 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, "", ""); + collect_eus_actions(package_name, callback_definition, instance_creation, ""); } void XMLParser::collect_eus_conditions(const std::string package_name, @@ -379,8 +401,7 @@ void XMLParser::collect_eus_conditions(const std::string package_name, std::vector* instance_creation, std::vector* parallel_callback_definition, std::vector* parallel_instance_creation, - std::string host_name, - std::string host_port) + const std::string remote_host) { auto format_condition_param = [](const XMLElement* node) { return fmt::format("({0} (send request :{0}))", node->Attribute("name")); @@ -437,9 +458,14 @@ void XMLParser::collect_eus_conditions(const std::string package_name, std::string name = node->Name(); if (name == "Condition" || name == "RemoteCondition") { std::string service_name = node->Attribute("service_name"); - if ((name == "RemoteCondition" && host_name.size() && host_port.size()) && - (node->Attribute("host_name") != host_name || - node->Attribute("host_port") != host_port)) { + if (name == "RemoteCondition" && remote_host.empty() && + node->Attribute("host_name") && node->Attribute("host_port")) { + // host is clear; callback should only be defined when remote_host matches + BOOST_LOG_TRIVIAL(trace) << "collect_eus_conditions: skipping " << service_name << "..."; + continue; + } + if (name == "RemoteCondition" && !remote_host.empty() && + format_remote_host(node) != remote_host) { BOOST_LOG_TRIVIAL(trace) << "collect_eus_conditions: skipping " << service_name << "..."; continue; } @@ -471,7 +497,7 @@ void XMLParser::collect_eus_conditions(const std::string package_name, { collect_eus_conditions(package_name, callback_definition, instance_creation, parallel_callback_definition, parallel_instance_creation, - "", ""); + ""); } void XMLParser::maybe_push_message_package(const XMLElement* node, @@ -542,6 +568,13 @@ std::string XMLParser::format_host_port(const XMLElement* node) { 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::generate_action_file_contents(const XMLElement* node) { std::vector goal, feedback; @@ -925,16 +958,15 @@ std::string XMLParser::generate_eus_action_server(const std::string package_name } std::string XMLParser::generate_eus_remote_action_server(const std::string package_name, - std::string host_name, - std::string host_port) + 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, host_name, host_port); + collect_eus_actions(package_name, &callback_definition, &instance_creation, remote_host); collect_eus_conditions(package_name, &callback_definition, &instance_creation, - NULL, NULL, host_name, host_port); + NULL, NULL, remote_host); if (callback_definition.empty()) return ""; @@ -959,8 +991,7 @@ std::string XMLParser::generate_eus_condition_server(const std::string package_n } std::string XMLParser::generate_eus_remote_condition_server(const std::string package_name, - std::string host_name, - std::string host_port) + const std::string remote_host) { std::vector callback_definition; std::vector instance_creation; @@ -968,7 +999,7 @@ std::string XMLParser::generate_eus_remote_condition_server(const std::string pa collect_eus_conditions(package_name, NULL, NULL, &callback_definition, &instance_creation, - host_name, host_port); + remote_host); if (callback_definition.empty()) return ""; @@ -1025,6 +1056,38 @@ std::map XMLParser::generate_all_service_files() { 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 xml_filename) { From 140abda75c4ff526d0870dc1136fa227031f377d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 21:48:21 +0900 Subject: [PATCH 125/176] (roseus_bt) Enable splitting eus files in tutorials --- roseus_bt/include/roseus_bt/tutorial_parser.h | 45 +++++++++++++++++++ roseus_bt/sample/models/t08_multimaster.xml | 6 ++- 2 files changed, 49 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/tutorial_parser.h b/roseus_bt/include/roseus_bt/tutorial_parser.h index e4387bd94..0678f3a7a 100644 --- a/roseus_bt/include/roseus_bt/tutorial_parser.h +++ b/roseus_bt/include/roseus_bt/tutorial_parser.h @@ -22,7 +22,11 @@ class TutorialParser : public XMLParser 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; }; @@ -85,6 +89,28 @@ std::string TutorialParser::generate_eus_action_server(const std::string package 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; @@ -103,6 +129,25 @@ std::string TutorialParser::generate_eus_condition_server(const std::string pack 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 diff --git a/roseus_bt/sample/models/t08_multimaster.xml b/roseus_bt/sample/models/t08_multimaster.xml index 601406ad4..9bc4f41a9 100644 --- a/roseus_bt/sample/models/t08_multimaster.xml +++ b/roseus_bt/sample/models/t08_multimaster.xml @@ -22,8 +22,10 @@ - - + + From 6aee87dab78cdfbd3b29a0a8347200aa7c4bce50 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 22:26:05 +0900 Subject: [PATCH 126/176] (roseus_bt) Fix file splitting with mixed remote and local nodes --- roseus_bt/include/roseus_bt/xml_parser.h | 97 ++++++++++++--------- roseus_bt/sample/models/t08_multimaster.xml | 6 +- 2 files changed, 60 insertions(+), 43 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index dee5f09e1..2ae2beca2 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -364,21 +364,30 @@ void XMLParser::collect_eus_actions(const std::string package_name, 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") { + if (remote_host.empty()) { + for (auto node = root->FirstChildElement("Action"); + node != nullptr; + node = node->NextSiblingElement("Action")) + { std::string server_name = node->Attribute("server_name"); - if (name == "RemoteAction" && remote_host.empty() && - node->Attribute("host_name") && node->Attribute("host_port")) { - // host is clear; callback should only be defined when remote_host matches - BOOST_LOG_TRIVIAL(trace) << "collect_eus_actions: skipping " << 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 (name == "RemoteAction" && !remote_host.empty() && - format_remote_host(node) != remote_host) { + if (format_remote_host(node) != remote_host) { BOOST_LOG_TRIVIAL(trace) << "collect_eus_actions: skipping " << server_name << "..."; continue; } @@ -449,42 +458,52 @@ void XMLParser::collect_eus_conditions(const std::string package_name, 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"); - for (auto node = root->FirstChildElement(); - node != nullptr; - node = node->NextSiblingElement()) - { - std::string name = node->Name(); - if (name == "Condition" || name == "RemoteCondition") { + if (remote_host.empty()) { + for (auto node = root->FirstChildElement("Condition"); + node != nullptr; + node = node->NextSiblingElement("Condition")) + { std::string service_name = node->Attribute("service_name"); - if (name == "RemoteCondition" && remote_host.empty() && - node->Attribute("host_name") && node->Attribute("host_port")) { - // host is clear; callback should only be defined when remote_host matches - BOOST_LOG_TRIVIAL(trace) << "collect_eus_conditions: skipping " << 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 (name == "RemoteCondition" && !remote_host.empty() && - format_remote_host(node) != remote_host) { + 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 << "..."; - - 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); - } - } + maybe_push(node, service_name); } } } diff --git a/roseus_bt/sample/models/t08_multimaster.xml b/roseus_bt/sample/models/t08_multimaster.xml index 9bc4f41a9..601406ad4 100644 --- a/roseus_bt/sample/models/t08_multimaster.xml +++ b/roseus_bt/sample/models/t08_multimaster.xml @@ -22,10 +22,8 @@ - - + + From ee35e8cd1cd4d3dd064c6e62c2bd7ebb9a07cc06 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 11:56:21 +0900 Subject: [PATCH 127/176] (roseus_bt) Enable non-ROS input ports in remote nodes --- .../roseus_bt/eus_remote_action_node.h | 10 ++- .../roseus_bt/eus_remote_condition_node.h | 8 ++- roseus_bt/include/roseus_bt/gen_template.h | 13 ++-- roseus_bt/include/roseus_bt/xml_exceptions.h | 8 ++- roseus_bt/include/roseus_bt/xml_parser.h | 72 ++++++++++++++----- 5 files changed, 81 insertions(+), 30 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h index 3d5caa39a..86b354646 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -10,6 +10,7 @@ namespace BT { // Helper Node to call a rosbridge websocket inside a BT::ActionNode +template class EusRemoteActionNode : public BT::ActionNodeBase { protected: @@ -29,8 +30,11 @@ class EusRemoteActionNode : public BT::ActionNodeBase public: - EusRemoteActionNode() = delete; + using BaseClass = EusRemoteActionNode; + using ActionType = ActionT; + using GoalType = typename ActionT::_action_goal_type::_goal_type; + EusRemoteActionNode() = delete; virtual ~EusRemoteActionNode() = default; static PortsList providedPorts() @@ -130,7 +134,7 @@ class EusRemoteActionNode : public BT::ActionNodeBase /// Method to register the service into a factory. - template static +template static void RegisterRemoteAction(BT::BehaviorTreeFactory& factory, const std::string& registration_ID) { @@ -142,7 +146,7 @@ class EusRemoteActionNode : public BT::ActionNodeBase manifest.type = getType(); manifest.ports = DerivedT::providedPorts(); manifest.registration_ID = registration_ID; - const auto& basic_ports = EusRemoteActionNode::providedPorts(); + const auto& basic_ports = EusRemoteActionNode::providedPorts(); manifest.ports.insert( basic_ports.begin(), basic_ports.end() ); factory.registerBuilder( manifest, builder ); } diff --git a/roseus_bt/include/roseus_bt/eus_remote_condition_node.h b/roseus_bt/include/roseus_bt/eus_remote_condition_node.h index 64120e924..b63d2d561 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_condition_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_condition_node.h @@ -10,6 +10,7 @@ namespace BT { // Helper Node to call a rosbridge websocket inside a BT::ActionNode +template class EusRemoteConditionNode : public BT::ActionNodeBase { protected: @@ -23,8 +24,11 @@ class EusRemoteConditionNode : public BT::ActionNodeBase public: - EusRemoteConditionNode() = delete; + using BaseClass = EusRemoteConditionNode; + using ServiceType = ServiceT; + using RequestType = typename ServiceT::Request; + EusRemoteConditionNode() = delete; virtual ~EusRemoteConditionNode() = default; static PortsList providedPorts() @@ -94,7 +98,7 @@ template static manifest.type = getType(); manifest.ports = DerivedT::providedPorts(); manifest.registration_ID = registration_ID; - const auto& basic_ports = EusRemoteConditionNode::providedPorts(); + const auto& basic_ports = EusRemoteConditionNode::providedPorts(); manifest.ports.insert( basic_ports.begin(), basic_ports.end() ); factory.registerBuilder( manifest, builder ); diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 37f156626..032b0ee28 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -166,7 +166,7 @@ std::string GenTemplate::remote_action_class_template( std::vector set_outputs) { std::string fmt_string = 1 + R"( -class %2%: public EusRemoteActionNode +class %2%: public EusRemoteActionNode<%1%::%2%Action> { public: @@ -184,6 +184,7 @@ EusRemoteActionNode("%1%/%2%Action", name, conf) {} { std::string json; rapidjson::Document document; + GoalType ros_msg; %4% return true; } @@ -268,17 +269,17 @@ std::string GenTemplate::remote_condition_class_template(std::string package_nam std::vector provided_ports, std::vector get_inputs) { std::string fmt_string = 1 + R"( -class %1%: public EusRemoteConditionNode +class %2%: public EusRemoteConditionNode<%1%::%2%> { public: - %1%(const std::string& name, const NodeConfiguration& conf): + %2%(const std::string& name, const NodeConfiguration& conf): EusRemoteConditionNode(name, conf) {} static PortsList providedPorts() { return { -%2% +%3% }; } @@ -286,7 +287,8 @@ class %1%: public EusRemoteConditionNode { std::string json; rapidjson::Document document; -%3% + RequestType ros_msg; +%4% } NodeStatus onResponse(const rapidjson::Value& result) override @@ -303,6 +305,7 @@ class %1%: public EusRemoteConditionNode )"; boost::format bfmt = boost::format(fmt_string) % + package_name % nodeID % boost::algorithm::join(provided_ports, ",\n") % boost::algorithm::join(get_inputs, "\n"); diff --git a/roseus_bt/include/roseus_bt/xml_exceptions.h b/roseus_bt/include/roseus_bt/xml_exceptions.h index 07d366c08..b924ace12 100644 --- a/roseus_bt/include/roseus_bt/xml_exceptions.h +++ b/roseus_bt/include/roseus_bt/xml_exceptions.h @@ -66,7 +66,7 @@ class UnknownNode: public XMLError { class UnknownPortNode: public XMLError { public: UnknownPortNode(const XMLElement* node) : - XMLError(fmt::format("Unknown port type {}{}", node->Name(), get_place(node))) {}; + XMLError(fmt::format("Unknown port node {}{}", node->Name(), get_place(node))) {}; }; class InvalidTopicType: public XMLError { @@ -75,6 +75,12 @@ class InvalidTopicType: public XMLError { 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 RoseusBT diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 2ae2beca2..f9296db50 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -77,6 +77,7 @@ class XMLParser 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 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); @@ -594,6 +595,50 @@ std::string XMLParser::format_remote_host(const XMLElement* node) { 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) + // parse ros message from json string + return fmt::format(R"( + getInput("{0}", json); + document.Parse(json.c_str()); + 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::generate_action_file_contents(const XMLElement* node) { std::vector goal, feedback; @@ -750,21 +795,18 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const std:: std::string XMLParser::generate_remote_action_class(const XMLElement* node, const std::string package_name) { auto format_input_port = [](const XMLElement* node) { - return fmt::format(" InputPort(\"{0}\")", + std::string msg_type = node->Attribute("type"); + if (msg_type.find('/') != std::string::npos) + // ros messages are parsed from json + return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); + 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(R"( - getInput("{0}", json); - document.Parse(json.c_str()); - rapidjson::Value {0}(document, document.GetAllocator()); - goal->AddMember("{0}", {0}, goal->GetAllocator());)", - node->Attribute("name")); - }; auto format_set_output = [](const XMLElement* node) { return fmt::format(" setOutputFromMessage(\"{0}\", feedback);", node->Attribute("name")); @@ -788,7 +830,7 @@ std::string XMLParser::generate_remote_action_class(const XMLElement* node, cons 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)); + 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)); @@ -841,14 +883,6 @@ std::string XMLParser::generate_remote_condition_class(const XMLElement* node, c return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); }; - auto format_get_input = [](const XMLElement* node) { - return fmt::format(R"( - getInput("{0}", json); - document.Parse(json.c_str()); - rapidjson::Value {0}(document, document.GetAllocator()); - request->AddMember("{0}", {0}, request->GetAllocator());)", - node->Attribute("name")); - }; std::vector provided_ports; std::vector get_inputs; @@ -864,7 +898,7 @@ std::string XMLParser::generate_remote_condition_class(const XMLElement* node, c 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)); + get_inputs.push_back(format_get_remote_input(port_node, "request")); } return gen_template.remote_condition_class_template(package_name, node->Attribute("ID"), From 8950c7a98b986acfdff919b73f61a5a6161b48ef Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 12:07:03 +0900 Subject: [PATCH 128/176] (roseus_bt) Avoid unnecessary declarations at sendGoal and sendRequest --- roseus_bt/include/roseus_bt/gen_template.h | 30 ++++++++++++++++------ 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 032b0ee28..052039f3c 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -165,6 +165,16 @@ std::string GenTemplate::remote_action_class_template( std::vector get_inputs, std::vector set_outputs) { + auto format_send_goal = [](const std::string body) { + std::string decl = 1 + R"( + std::string json; + rapidjson::Document 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> { @@ -182,9 +192,6 @@ EusRemoteActionNode("%1%/%2%Action", name, conf) {} bool sendGoal(rapidjson::Document* goal) override { - std::string json; - rapidjson::Document document; - GoalType ros_msg; %4% return true; } @@ -211,7 +218,7 @@ EusRemoteActionNode("%1%/%2%Action", name, conf) {} package_name % nodeID % boost::algorithm::join(provided_ports, ",\n") % - boost::algorithm::join(get_inputs, "\n") % + format_send_goal(boost::algorithm::join(get_inputs, "\n")) % boost::algorithm::join(set_outputs, "\n"); return bfmt.str(); @@ -268,6 +275,16 @@ class %2%: public EusConditionNode<%1%::%2%> 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%> { @@ -285,9 +302,6 @@ class %2%: public EusRemoteConditionNode<%1%::%2%> void sendRequest(rapidjson::Document *request) override { - std::string json; - rapidjson::Document document; - RequestType ros_msg; %4% } @@ -308,7 +322,7 @@ class %2%: public EusRemoteConditionNode<%1%::%2%> package_name % nodeID % boost::algorithm::join(provided_ports, ",\n") % - boost::algorithm::join(get_inputs, "\n"); + format_send_request(boost::algorithm::join(get_inputs, "\n")); return bfmt.str(); } From d41fda10f9f42faf8bdf1e341beace58f0574085 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 12:18:02 +0900 Subject: [PATCH 129/176] (roseus_bt) Update t08_multimaster --- roseus_bt/include/roseus_bt/tutorial_parser.h | 3 +- roseus_bt/sample/README.md | 7 ++- roseus_bt/sample/models/t08_multimaster.xml | 49 +++++++++++++++---- 3 files changed, 45 insertions(+), 14 deletions(-) diff --git a/roseus_bt/include/roseus_bt/tutorial_parser.h b/roseus_bt/include/roseus_bt/tutorial_parser.h index 0678f3a7a..337613599 100644 --- a/roseus_bt/include/roseus_bt/tutorial_parser.h +++ b/roseus_bt/include/roseus_bt/tutorial_parser.h @@ -39,7 +39,8 @@ std::string TutorialParser::format_node_body(const XMLElement* node, int padding // 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") return fmt::format("{}(at-spot {})", pad, param_list.at(0)); + if (id == "atSpot" || id == "atTableSpot" || id == "atBroomSpot") + return fmt::format("{}(at-spot {})", pad, param_list.at(0)); if (id == "CheckCoords") return fmt::format( "{}(not (equal (instance geometry_msgs::Pose :init) {}))", pad, param_list.at(0)); diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index 75ece9fd6..f81b53dda 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -234,8 +234,7 @@ The `{var}` notation also works. 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 `` tag in the ``, and add a `host_name` and `host_port` field to it. As a normal action, it also requires to have the `server_name` field set. -The `host_name` and `host_port` can be set either statically (in the `TreeNodesModel` definition) or dynamically (within the `BehaviorTree` node instance), although we recommend to set it statically to avoid possible runtime errors. +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. #### Run the code @@ -247,7 +246,7 @@ roslaunch rosbridge_server rosbridge_websocket.launch Run the first roseus server: ```bash roscd roseus_bt_tutorials/euslisp -roseus t08_multimaster-action-server.l +roseus t08_multimaster_localhost9090-action-server.l ``` Run the second rosbridge_server: @@ -260,7 +259,7 @@ Run the second roseus server: ```bash export ROS_MASTER_URI=http://localhost:11312 roscd roseus_bt_tutorials/euslisp -roseus t08_multimaster-action-server.l +roseus t08_multimaster_localhost9091-action-server.l ``` Run the cpp client diff --git a/roseus_bt/sample/models/t08_multimaster.xml b/roseus_bt/sample/models/t08_multimaster.xml index 601406ad4..040c370f4 100644 --- a/roseus_bt/sample/models/t08_multimaster.xml +++ b/roseus_bt/sample/models/t08_multimaster.xml @@ -5,25 +5,56 @@ - + + + + + + + - - + + + + + + + + - + - + + + + + + + + + + + + + + - - From 1d6da771e86a322a4723d5c768878dd14e546be2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 13:36:36 +0900 Subject: [PATCH 130/176] (roseus_bt) Add submodule directory --- roseus_bt/include/rosbridgecpp | 1 + 1 file changed, 1 insertion(+) create mode 160000 roseus_bt/include/rosbridgecpp 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 From 7a6897ea79775ca902522910cb4f888af1699987 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 14:06:41 +0900 Subject: [PATCH 131/176] (roseus_bt) Update submodules at build time --- roseus_bt/CMakeLists.txt | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/roseus_bt/CMakeLists.txt b/roseus_bt/CMakeLists.txt index 19dd506fa..37e0e7176 100644 --- a/roseus_bt/CMakeLists.txt +++ b/roseus_bt/CMakeLists.txt @@ -20,6 +20,18 @@ catkin_package( 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}) From 5da902b6efc652ac69df00800e7433ffffae2db8 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 27 Apr 2022 20:39:44 +0900 Subject: [PATCH 132/176] (roseus_bt) Add basic_types.h --- roseus_bt/include/roseus_bt/basic_types.cpp | 51 +++++++++++++++++++++ roseus_bt/include/roseus_bt/basic_types.h | 29 ++++++++++++ 2 files changed, 80 insertions(+) create mode 100644 roseus_bt/include/roseus_bt/basic_types.cpp create mode 100644 roseus_bt/include/roseus_bt/basic_types.h 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..ab60fffa5 --- /dev/null +++ b/roseus_bt/include/roseus_bt/basic_types.cpp @@ -0,0 +1,51 @@ +#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_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::ACTION; + if( str == "RemoteCondition" ) return NodeType::CONDITION; + 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..05d9d6fba --- /dev/null +++ b/roseus_bt/include/roseus_bt/basic_types.h @@ -0,0 +1,29 @@ +#ifndef BEHAVIOR_TREE_EUS_NODE_TYPE_HPP_ +#define BEHAVIOR_TREE_EUS_NODE_TYPE_HPP_ + +#include + + +namespace roseus_bt +{ + +enum class NodeType +{ + UNDEFINED = 0, + ACTION, + CONDITION, + CONTROL, + DECORATOR, + SUBTREE, + SUBSCRIBER, + REMOTE_ACTION, + REMOTE_CONDITION, +}; + +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_EUS_NODE_TYPE_HPP_ From d20d4a1a3ec30e01c6cce1428f22078648508c3b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 28 Apr 2022 11:18:17 +0900 Subject: [PATCH 133/176] (roseus_bt) Change subscriber port names --- .../include/roseus_bt/eus_subscriber_node.h | 4 +-- roseus_bt/include/roseus_bt/gen_template.h | 2 +- .../include/roseus_bt/package_generator.h | 2 ++ roseus_bt/include/roseus_bt/xml_parser.h | 25 +++++++++---------- roseus_bt/sample/README.md | 2 +- roseus_bt/sample/models/t04_subscriber.xml | 4 +-- roseus_bt/sample/models/t06_reactive.xml | 4 +-- 7 files changed, 22 insertions(+), 21 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_subscriber_node.h b/roseus_bt/include/roseus_bt/eus_subscriber_node.h index 689e2bef7..f6ebd6464 100644 --- a/roseus_bt/include/roseus_bt/eus_subscriber_node.h +++ b/roseus_bt/include/roseus_bt/eus_subscriber_node.h @@ -28,12 +28,12 @@ class EusSubscriberNode: public BT::ActionNodeBase static PortsList providedPorts() { return { InputPort("topic_name", "name of the subscribed topic"), - OutputPort("to", "port to where messages are redirected") + OutputPort("output_port", "port to where messages are redirected") }; } virtual void callback(MessageT msg) { - setOutput("to", msg); + setOutput("output_port", msg); } virtual BT::NodeStatus tick() override final diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 052039f3c..af85e8855 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -333,7 +333,7 @@ std::string GenTemplate::subscriber_class_template(std::string nodeID, std::stri if (!message_field.empty()) { std::string fmt_string = R"( virtual void callback(%1% msg) { - setOutput("to", msg.%2%); + setOutput("output_port", msg.%2%); } )"; diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index afb4ff0f6..569a8c25c 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -277,7 +277,9 @@ void PackageGenerator::write_all_files() { BOOST_LOG_TRIVIAL(info) << "Generating " << xml_filename << " files..."; + BOOST_LOG_TRIVIAL(info) << "Checking dependencies..."; parser->push_dependencies(&message_packages, &service_files, &action_files); + copy_xml_file(&xml_filename); write_action_files(parser); write_service_files(parser); diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index f9296db50..1e3b319b9 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -68,7 +68,7 @@ class XMLParser std::vector* instance_creation, std::vector* parallel_callback_definition, std::vector* parallel_instance_creation); - void maybe_push_message_package(const XMLElement* node, + 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); @@ -175,8 +175,8 @@ void XMLParser::check_xml_file(std::string filename) { } if (name == "Subscriber") { - if (!node->Attribute("type")) { - throw XMLError::MissingRequiredAttribute("type", node); + if (!node->Attribute("message_type")) { + throw XMLError::MissingRequiredAttribute("message_type", node); } check_push(node, &subscribers, &duplicated_nodes); } @@ -520,12 +520,11 @@ void XMLParser::collect_eus_conditions(const std::string package_name, ""); } -void XMLParser::maybe_push_message_package(const XMLElement* node, +void XMLParser::maybe_push_message_package(const std::string message_type, std::vector* message_packages) { - std::string msg_type = node->Attribute("type"); - std::size_t pos = msg_type.find('/'); + std::size_t pos = message_type.find('/'); if (pos != std::string::npos) { - std::string pkg = msg_type.substr(0, pos); + std::string pkg = message_type.substr(0, pos); push_new(pkg, message_packages); } } @@ -695,7 +694,7 @@ std::string XMLParser::generate_headers(const std::string package_name) { }; auto format_subscriber_node = [](const XMLElement* node) { - return fmt::format("#include <{}.h>", node->Attribute("type")); + return fmt::format("#include <{}.h>", node->Attribute("message_type")); }; const XMLElement* root = doc.RootElement()->FirstChildElement("TreeNodesModel"); @@ -907,7 +906,7 @@ std::string XMLParser::generate_remote_condition_class(const XMLElement* node, c std::string XMLParser::generate_subscriber_class(const XMLElement* node) { auto format_type = [](const XMLElement* node) { - std::string type = node->Attribute("type"); + std::string type = node->Attribute("message_type"); std::size_t pos = type.find('/'); if (pos == std::string::npos) { throw XMLError::InvalidTopicType(type, node); @@ -915,8 +914,8 @@ std::string XMLParser::generate_subscriber_class(const XMLElement* node) { return fmt::format("{}::{}", type.substr(0, pos), type.substr(1+pos)); }; auto format_field = [](const XMLElement* node) { - if (!node->Attribute("field")) return ""; - return node->Attribute("field"); + if (!node->Attribute("message_field")) return ""; + return node->Attribute("message_field"); }; return gen_template.subscriber_class_template(node->Attribute("ID"), @@ -1224,11 +1223,11 @@ void XMLParser::push_dependencies(std::vector* message_packages, port_node != nullptr; port_node = port_node->NextSiblingElement()) { - maybe_push_message_package(port_node, message_packages); + maybe_push_message_package(port_node->Attribute("type"), message_packages); } } if (name == "Subscriber") { - maybe_push_message_package(node, message_packages); + maybe_push_message_package(node->Attribute("message_type"), message_packages); } } } diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index f81b53dda..04a1e5b95 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -107,7 +107,7 @@ The fourth example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/rose Such port variables are initialized with an empty message instance and updated every time a new topic message arrives. -To do this we add an action with the `topic_name` and `to` fields in the `` section and declare it as a `` and specify its type in the `` section. +To do this we add an action with the `topic_name` and `output_port` fields in the `` section, and declare it as a `` and specify `message_type` and optionally `message_field` in the `` section. Only proper ROS message types are supported by subscriber nodes (e.g. `std_msgs/Int64` instead of `int64`). Note how we also add a step to verify and wait for messages. diff --git a/roseus_bt/sample/models/t04_subscriber.xml b/roseus_bt/sample/models/t04_subscriber.xml index 74f6bc344..479a43163 100644 --- a/roseus_bt/sample/models/t04_subscriber.xml +++ b/roseus_bt/sample/models/t04_subscriber.xml @@ -4,7 +4,7 @@ - + @@ -44,7 +44,7 @@ - + diff --git a/roseus_bt/sample/models/t06_reactive.xml b/roseus_bt/sample/models/t06_reactive.xml index f39b886dc..38b3f2c01 100644 --- a/roseus_bt/sample/models/t06_reactive.xml +++ b/roseus_bt/sample/models/t06_reactive.xml @@ -5,7 +5,7 @@ - + @@ -63,7 +63,7 @@ - + From 8742e87877d227c68e7e175dc996ac00c1d2419c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 28 Apr 2022 11:28:32 +0900 Subject: [PATCH 134/176] (roseus_bt) Allow to set default topic_name at Subscriber nodes --- .../include/roseus_bt/eus_subscriber_node.h | 5 +++ roseus_bt/include/roseus_bt/gen_template.h | 34 +++++++++++++++---- roseus_bt/include/roseus_bt/xml_parser.h | 26 +++++++++++++- 3 files changed, 58 insertions(+), 7 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_subscriber_node.h b/roseus_bt/include/roseus_bt/eus_subscriber_node.h index f6ebd6464..564621dd8 100644 --- a/roseus_bt/include/roseus_bt/eus_subscriber_node.h +++ b/roseus_bt/include/roseus_bt/eus_subscriber_node.h @@ -22,6 +22,9 @@ class EusSubscriberNode: public BT::ActionNodeBase ros::Subscriber sub_; public: + + using MessageType = MessageT; + EusSubscriberNode() = delete; virtual ~EusSubscriberNode() = default; @@ -62,6 +65,8 @@ template static 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 ); } diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index af85e8855..32b285f68 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -36,7 +36,8 @@ class GenTemplate std::vector provided_ports, std::vector get_inputs); std::string subscriber_class_template(std::string nodeID, std::string message_type, - std::string message_field); + std::string message_field, + std::vector provided_ports); std::string main_function_template(std::string roscpp_node_name, std::string xml_filename, std::vector register_actions, @@ -328,14 +329,17 @@ class %2%: public EusRemoteConditionNode<%1%::%2%> } -std::string GenTemplate::subscriber_class_template(std::string nodeID, std::string message_type, - std::string message_field) { +std::string GenTemplate::subscriber_class_template(std::string nodeID, + std::string message_type, + std::string message_field, + std::vector provided_ports) { + std::string provided_ports_body; + if (!message_field.empty()) { std::string fmt_string = R"( virtual void callback(%1% msg) { setOutput("output_port", msg.%2%); - } -)"; + })"; boost::format bfmt = boost::format(fmt_string) % message_type % @@ -344,17 +348,35 @@ std::string GenTemplate::subscriber_class_template(std::string nodeID, std::stri message_field = bfmt.str(); } + 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& node_name, const NodeConfiguration& conf) : EusSubscriberNode<%2%>(handle, node_name, conf) {} -%3%}; +%3% +%4% +}; )"; boost::format bfmt = boost::format(fmt_string) % nodeID % message_type % + provided_ports_body % message_field; return bfmt.str(); diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 1e3b319b9..c57518667 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -917,10 +917,34 @@ std::string XMLParser::generate_subscriber_class(const XMLElement* node) { if (!node->Attribute("message_field")) return ""; return node->Attribute("message_field"); }; + auto format_port = [](const XMLElement* port_node) { + std::string name = port_node->Attribute("name"); + if (name == "topic_name" && + port_node->Attribute("default")) + { + return fmt::format( + " InputPort(\"topic_name\", \"{0}\", \"name of the subscribed topic\")", + port_node->Attribute("default")); + } + return std::string(); + }; + + std::vector provided_ports; + + for (auto port_node = node->FirstChildElement("input_port"); + port_node != nullptr; + port_node = port_node->NextSiblingElement("input_port")) + { + std::string port = format_port(port_node); + if (!port.empty()) { + provided_ports.push_back(port); + } + } return gen_template.subscriber_class_template(node->Attribute("ID"), format_type(node), - format_field(node)); + format_field(node), + provided_ports); } std::string XMLParser::generate_main_function(const std::string roscpp_node_name, From ba405cb6ae02fae260e2f780e9d2002c8c654a99 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 28 Apr 2022 15:42:54 +0900 Subject: [PATCH 135/176] (roseus_bt) Add RemoteSubscriber --- roseus_bt/include/roseus_bt/basic_types.cpp | 3 + roseus_bt/include/roseus_bt/basic_types.h | 1 + roseus_bt/include/roseus_bt/eus_nodes.h | 2 +- .../roseus_bt/eus_remote_subscriber_node.h | 108 ++++++++++++++++++ .../include/roseus_bt/eus_subscriber_node.h | 19 +-- roseus_bt/include/roseus_bt/gen_template.h | 48 +++++++- .../include/roseus_bt/ws_subscriber_client.h | 30 +++++ roseus_bt/include/roseus_bt/xml_parser.h | 87 +++++++++++--- 8 files changed, 270 insertions(+), 28 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h create mode 100644 roseus_bt/include/roseus_bt/ws_subscriber_client.h diff --git a/roseus_bt/include/roseus_bt/basic_types.cpp b/roseus_bt/include/roseus_bt/basic_types.cpp index ab60fffa5..ecc74616f 100644 --- a/roseus_bt/include/roseus_bt/basic_types.cpp +++ b/roseus_bt/include/roseus_bt/basic_types.cpp @@ -19,6 +19,8 @@ std::string toStr(NodeType type) return "SubTree"; case NodeType::SUBSCRIBER: return "Subscriber"; + case NodeType::REMOTE_SUBSCRIBER: + return "RemoteSubscriber"; case NodeType::REMOTE_ACTION: return "RemoteAction"; case NodeType::REMOTE_CONDITION: @@ -36,6 +38,7 @@ NodeType convertFromString(BT::StringView str) if( str == "Decorator" ) return NodeType::DECORATOR; if( str == "SubTree" || str == "SubTreePlus" ) return NodeType::SUBTREE; if( str == "Subscriber" ) return NodeType::SUBSCRIBER; + if( str == "RemoteSubscriber" ) return NodeType::REMOTE_SUBSCRIBER; if( str == "RemoteAction" ) return NodeType::ACTION; if( str == "RemoteCondition" ) return NodeType::CONDITION; return NodeType::UNDEFINED; diff --git a/roseus_bt/include/roseus_bt/basic_types.h b/roseus_bt/include/roseus_bt/basic_types.h index 05d9d6fba..513c61a28 100644 --- a/roseus_bt/include/roseus_bt/basic_types.h +++ b/roseus_bt/include/roseus_bt/basic_types.h @@ -18,6 +18,7 @@ enum class NodeType SUBSCRIBER, REMOTE_ACTION, REMOTE_CONDITION, + REMOTE_SUBSCRIBER, }; std::string toStr(NodeType type); diff --git a/roseus_bt/include/roseus_bt/eus_nodes.h b/roseus_bt/include/roseus_bt/eus_nodes.h index 16e9a3f7b..601e6d676 100644 --- a/roseus_bt/include/roseus_bt/eus_nodes.h +++ b/roseus_bt/include/roseus_bt/eus_nodes.h @@ -6,6 +6,6 @@ #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_EUS_NODES_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..08d5ab630 --- /dev/null +++ b/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h @@ -0,0 +1,108 @@ +#ifndef BEHAVIOR_TREE_EUS_REMOTE_SUBSCRIBER_NODE_HPP_ +#define BEHAVIOR_TREE_EUS_REMOTE_SUBSCRIBER_NODE_HPP_ + +#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()) + { + 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"), + 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: + RosbridgeSubscriberClient subscriber_client_; + +protected: + virtual void callback(const rapidjson::Value& msg) { + setOutputFromMessage("output_port", msg); + } + + 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::StringBuffer strbuf; + rapidjson::Writer writer(strbuf); + rapidjson::Document document; + document.CopyFrom(message, document.GetAllocator()); + document.Accept(writer); + setOutput(name, strbuf.GetString()); + } +}; + + +/// 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 index 564621dd8..31e1e0564 100644 --- a/roseus_bt/include/roseus_bt/eus_subscriber_node.h +++ b/roseus_bt/include/roseus_bt/eus_subscriber_node.h @@ -1,7 +1,6 @@ #ifndef BEHAVIOR_TREE_EUS_SUBSCRIBER_NODE_HPP_ #define BEHAVIOR_TREE_EUS_SUBSCRIBER_NODE_HPP_ -#include namespace BT { @@ -17,10 +16,6 @@ class EusSubscriberNode: public BT::ActionNodeBase sub_ = node_.subscribe(topic_name, 1000, &EusSubscriberNode::callback, this); } -private: - ros::NodeHandle& node_; - ros::Subscriber sub_; - public: using MessageType = MessageT; @@ -35,10 +30,6 @@ class EusSubscriberNode: public BT::ActionNodeBase }; } - virtual void callback(MessageT msg) { - setOutput("output_port", msg); - } - virtual BT::NodeStatus tick() override final { return NodeStatus::SUCCESS; @@ -48,6 +39,16 @@ class EusSubscriberNode: public BT::ActionNodeBase { setStatus(NodeStatus::IDLE); } + +protected: + ros::NodeHandle& node_; + ros::Subscriber sub_; + +protected: + virtual void callback(MessageT msg) { + setOutput("output_port", msg); + } + }; diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 32b285f68..c07cf0918 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -38,6 +38,8 @@ class GenTemplate std::string subscriber_class_template(std::string nodeID, std::string message_type, std::string message_field, 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 xml_filename, std::vector register_actions, @@ -234,8 +236,8 @@ class %2%: public EusConditionNode<%1%::%2%> { public: - %2%(ros::NodeHandle& handle, const std::string& node_name, const NodeConfiguration& conf): - EusConditionNode<%1%::%2%>(handle, node_name, conf) {} + %2%(ros::NodeHandle& handle, const std::string& name, const NodeConfiguration& conf): + EusConditionNode<%1%::%2%>(handle, name, conf) {} static PortsList providedPorts() { @@ -367,8 +369,8 @@ std::string GenTemplate::subscriber_class_template(std::string nodeID, class %1%: public EusSubscriberNode<%2%> { public: - %1%(ros::NodeHandle& handle, const std::string& node_name, const NodeConfiguration& conf) : - EusSubscriberNode<%2%>(handle, node_name, conf) {} + %1%(ros::NodeHandle& handle, const std::string& name, const NodeConfiguration& conf) : + EusSubscriberNode<%2%>(handle, name, conf) {} %3% %4% }; @@ -383,6 +385,44 @@ class %1%: public EusSubscriberNode<%2%> } +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 xml_filename, std::vector register_actions, 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..1cd90ee44 --- /dev/null +++ b/roseus_bt/include/roseus_bt/ws_subscriber_client.h @@ -0,0 +1,30 @@ +#ifndef WS_SUBSCRIBER_CLIENT_ +#define WS_SUBSCRIBER_CLIENT_ + +#include +#include + +class RosbridgeSubscriberClient +{ +public: + RosbridgeSubscriberClient(const std::string& master, int port, const std::string& topic_name): + rbc_(fmt::format("{}:{}", master, std::to_string(port))), + topic_name_(topic_name) + { + rbc_.addClient("topic_subscriber"); + } + + ~RosbridgeSubscriberClient() { + rbc_.removeClient("topic_subscriber"); + } + + void registerCallback(auto callback) { + rbc_.subscribe("topic_subscriber", topic_name_, callback); + } + +protected: + RosbridgeWsClient rbc_; + std::string topic_name_; +}; + +#endif // WS_SUBSCRIBER_CLIENT_ diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index c57518667..65b090ecf 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -86,6 +86,7 @@ class XMLParser 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 xml_filename); virtual std::string format_node_body(const XMLElement* node, int padding); @@ -152,6 +153,7 @@ void XMLParser::check_xml_file(std::string filename) { name != "Condition" && name != "RemoteCondition" && name != "Subscriber" && + name != "RemoteSubscriber" && name != "SubTree") { throw XMLError::UnknownNode(node); } @@ -174,7 +176,7 @@ void XMLParser::check_xml_file(std::string filename) { check_push(node, &conditions, &duplicated_nodes); } - if (name == "Subscriber") { + if (name == "Subscriber" || name == "RemoteSubscriber") { if (!node->Attribute("message_type")) { throw XMLError::MissingRequiredAttribute("message_type", node); } @@ -735,6 +737,13 @@ std::string XMLParser::generate_headers(const std::string package_name) { headers.push_back(format_subscriber_node(subscriber_node)); } + for (auto subscriber_node = root->FirstChildElement("RemoteSubscriber"); + subscriber_node != nullptr; + subscriber_node = subscriber_node->NextSiblingElement("RemoteSubscriber")) + { + headers.push_back(format_subscriber_node(subscriber_node)); + } + return gen_template.headers_template(headers); } @@ -918,15 +927,9 @@ std::string XMLParser::generate_subscriber_class(const XMLElement* node) { return node->Attribute("message_field"); }; auto format_port = [](const XMLElement* port_node) { - std::string name = port_node->Attribute("name"); - if (name == "topic_name" && - port_node->Attribute("default")) - { - return fmt::format( - " InputPort(\"topic_name\", \"{0}\", \"name of the subscribed topic\")", - port_node->Attribute("default")); - } - return std::string(); + return fmt::format( + " InputPort(\"topic_name\", \"{0}\", \"name of the subscribed topic\")", + port_node->Attribute("default")); }; std::vector provided_ports; @@ -935,9 +938,9 @@ std::string XMLParser::generate_subscriber_class(const XMLElement* node) { port_node != nullptr; port_node = port_node->NextSiblingElement("input_port")) { - std::string port = format_port(port_node); - if (!port.empty()) { - provided_ports.push_back(port); + std::string name = port_node->Attribute("name"); + if (name == "topic_name" && port_node->Attribute("default")) { + provided_ports.push_back(format_port(port_node)); } } @@ -947,6 +950,43 @@ std::string XMLParser::generate_subscriber_class(const XMLElement* 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 xml_filename) { auto format_action_node = [](const XMLElement* node) { @@ -969,6 +1009,10 @@ std::string XMLParser::generate_main_function(const std::string roscpp_node_name 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; @@ -1010,6 +1054,13 @@ std::string XMLParser::generate_main_function(const std::string roscpp_node_name 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, xml_filename, register_actions, register_conditions, @@ -1212,6 +1263,14 @@ std::string XMLParser::generate_cpp_file(const std::string package_name, 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, xml_filename)); return output; @@ -1250,7 +1309,7 @@ void XMLParser::push_dependencies(std::vector* message_packages, maybe_push_message_package(port_node->Attribute("type"), message_packages); } } - if (name == "Subscriber") { + if (name == "Subscriber" || name == "RemoteSubscriber") { maybe_push_message_package(node->Attribute("message_type"), message_packages); } } From 2267f71a4d77f6c58ba341c819c3ffae1e81453e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 28 Apr 2022 23:08:45 +0900 Subject: [PATCH 136/176] (roseus_bt) Fix bug in convertFromString --- roseus_bt/include/roseus_bt/basic_types.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/basic_types.cpp b/roseus_bt/include/roseus_bt/basic_types.cpp index ecc74616f..be37ce3d1 100644 --- a/roseus_bt/include/roseus_bt/basic_types.cpp +++ b/roseus_bt/include/roseus_bt/basic_types.cpp @@ -38,9 +38,9 @@ NodeType convertFromString(BT::StringView str) 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; - if( str == "RemoteAction" ) return NodeType::ACTION; - if( str == "RemoteCondition" ) return NodeType::CONDITION; return NodeType::UNDEFINED; } From 9daa78accbe847f455ba1be37e2ad68e30ebe243 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 29 Apr 2022 00:10:36 +0900 Subject: [PATCH 137/176] (roseus_bt) Explicitly add subscriber and subtree ports in model files --- roseus_bt/sample/models/t04_subscriber.xml | 5 ++++- roseus_bt/sample/models/t06_reactive.xml | 9 +++++++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/roseus_bt/sample/models/t04_subscriber.xml b/roseus_bt/sample/models/t04_subscriber.xml index 479a43163..e233921ea 100644 --- a/roseus_bt/sample/models/t04_subscriber.xml +++ b/roseus_bt/sample/models/t04_subscriber.xml @@ -44,7 +44,10 @@ - + + + + diff --git a/roseus_bt/sample/models/t06_reactive.xml b/roseus_bt/sample/models/t06_reactive.xml index 38b3f2c01..93c372e15 100644 --- a/roseus_bt/sample/models/t06_reactive.xml +++ b/roseus_bt/sample/models/t06_reactive.xml @@ -39,7 +39,9 @@ - + + + @@ -63,7 +65,10 @@ - + + + + From a9d7a184cc6472b767d10726a975122f655632c1 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 6 Jan 2022 13:48:31 +0900 Subject: [PATCH 138/176] Add single-threaded resumable-action-node --- roseus_bt/euslisp/nodes.l | 54 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 52 insertions(+), 2 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 4968ad560..4b63add8d 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -3,7 +3,7 @@ (in-package "ROSEUS_BT") -(export '(action-node condition-node spin-once spin)) +(export '(action-node resumable-action-node condition-node spin-once spin)) ;; import relevant ros::simple-action-server symbols (import '(ros::name-space ros::status ros::execute-cb ros::goal ros::goal-id)) @@ -40,6 +40,13 @@ (setf (third fn) 0) (setf (fourth fn) 0)))))) +(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)) @@ -98,6 +105,49 @@ (ros::spin-once monitor-groupname) (send self :spin-once)))) +(defclass resumable-action-node :super action-node :slots (resume-if timeout cancel-cb)) +(defmethod resumable-action-node + (:init (ns spec &key execute-cb preempt-cb accept-cb groupname + ((:monitor-groupname monitor-gn)) + ((:resume-if res-if) 'check-goal) ((:timeout tmout) 10)) + (check-fn-closure res-if) + (assert (numberp tmout)) + (setq resume-if res-if) + (setq timeout tmout) + (setq cancel-cb preempt-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..."))))))) + (send-super :execute-cb))))) (defclass condition-node :super propertied-object :slots (execute-cb groupname)) (defmethod condition-node @@ -131,6 +181,6 @@ (send ac :execute-cb))) (defun spin () - (while t + (while (ros::ok) (spin-once) (ros::sleep))) From d41c1724cdfeaf5a79f9fa9a8a5eda499b80bc56 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 6 Jan 2022 19:25:56 +0900 Subject: [PATCH 139/176] Add resume-cb option to resumable-action-nodes --- roseus_bt/euslisp/nodes.l | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 4b63add8d..ce8b4659d 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -107,7 +107,7 @@ (defclass resumable-action-node :super action-node :slots (resume-if timeout cancel-cb)) (defmethod resumable-action-node - (:init (ns spec &key execute-cb preempt-cb accept-cb groupname + (: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)) (check-fn-closure res-if) @@ -115,6 +115,7 @@ (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 @@ -146,7 +147,8 @@ (ros::ros-warn ";; Resume conditions were not satisfied!") (throw :execute-cb nil)) (t - (ros::ros-info ";; Resuming..."))))))) + (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)) From 037ab99fce062510316f234fcbb7996d53a75dce Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 20:26:18 +0900 Subject: [PATCH 140/176] (roseus_bt) Default action-node's preemption callback to 'signals-cancel' --- roseus_bt/euslisp/nodes.l | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index ce8b4659d..24e4f3da8 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -3,7 +3,8 @@ (in-package "ROSEUS_BT") -(export '(action-node resumable-action-node condition-node spin-once spin)) +(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)) @@ -51,7 +52,7 @@ ;; classes (defclass action-node :super ros::simple-action-server :slots (monitor-groupname)) (defmethod action-node - (:init (ns spec &key execute-cb preempt-cb accept-cb groupname + (:init (ns spec &key execute-cb (preempt-cb 'signals-cancel) accept-cb groupname ((:monitor-groupname monitor-gn))) (check-fn-closure execute-cb) (check-fn-closure preempt-cb) From a5fcbe3d24888bd2ca876bc0a0488377671a734a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 21:14:16 +0900 Subject: [PATCH 141/176] (roseus_bt) Update the reactive sample code and documentation to use conditions --- roseus_bt/include/roseus_bt/tutorial_parser.h | 9 +++++++-- roseus_bt/sample/README.md | 2 +- roseus_bt/sample/sample-task.l | 5 +++-- 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/roseus_bt/include/roseus_bt/tutorial_parser.h b/roseus_bt/include/roseus_bt/tutorial_parser.h index 337613599..9a9fcf1e8 100644 --- a/roseus_bt/include/roseus_bt/tutorial_parser.h +++ b/roseus_bt/include/roseus_bt/tutorial_parser.h @@ -54,8 +54,6 @@ std::string TutorialParser::format_node_body(const XMLElement* node, int padding if (id == "PlaceBottle") return fmt::format("{0}(place-sushi-bottle)\n{0}(reset-pose)", pad); - if (id == "SweepFloor") - return fmt::format("{0}(sweep-floor #'send server :ok)\n{0}(reset-pose)", pad); if (id == "MoveTo") return fmt::format("{}(go-to-spot {})", pad, param_list.at(0)); if (id == "PickBottleAt") @@ -66,6 +64,13 @@ std::string TutorialParser::format_node_body(const XMLElement* node, int padding {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); } diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index 04a1e5b95..8af543714 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -179,7 +179,7 @@ The sixth example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseu 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. Because in such scenario the condition nodes must be evaluated alongside the running action we prepare two distinct roseus servers -- one for actions and one for conditions. -On the action side it is also necessary to check for the presence of interruption requests with the `(roseus_bt:ok)` function. +On the action side it is also necessary to catch any interruption requests signalized by the `roseus_bt:cancel-action` condition. It is also possible to actively check for the presence of interruption requests with the `(send server :ok)`, when using custom preemption callbacks. #### Run the code diff --git a/roseus_bt/sample/sample-task.l b/roseus_bt/sample/sample-task.l index 5cf7669a6..414cd4063 100644 --- a/roseus_bt/sample/sample-task.l +++ b/roseus_bt/sample/sample-task.l @@ -82,12 +82,13 @@ ;; Sweep task -(defun sweep-floor (&optional (check-fn #'(lambda () t)) &rest args) +(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 (apply check-fn args) + (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)) From a27c107ae547eff9a795a42eeeb65d2396a502ab Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 21:30:12 +0900 Subject: [PATCH 142/176] (roseus_bt) Fix typo in warning --- roseus_bt/euslisp/nodes.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 24e4f3da8..5d1484003 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -72,7 +72,7 @@ :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 reacitivity is required.")) + (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))) From efa07d2cf51c707462158170715dd855d5c2f548 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 21:38:30 +0900 Subject: [PATCH 143/176] (roseus_bt) Log and return final tree status on completion --- roseus_bt/include/roseus_bt/gen_template.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index c07cf0918..55fee97b0 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -464,7 +464,8 @@ int main(int argc, char **argv) } std::cout << "Writed log to file: " << log_filename << std::endl; - return 0; + std::cout << "Behavior Tree execution finished with " << toStr(status, true).c_str() << std::endl; + return (status != NodeStatus::SUCCESS); } )"; From 4749d6a3361e723097dc183fd20bf6bb3f557c7b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 23:17:39 +0900 Subject: [PATCH 144/176] (roseus_bt) Add received_port in Subscriber nodes to differentiate received messages from ones that have only been initialized --- roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h | 2 ++ roseus_bt/include/roseus_bt/eus_subscriber_node.h | 4 +++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h b/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h index 08d5ab630..3d917eee4 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h @@ -35,6 +35,7 @@ class EusRemoteSubscriberNode: public BT::ActionNodeBase 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") }; @@ -56,6 +57,7 @@ class EusRemoteSubscriberNode: public BT::ActionNodeBase 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) diff --git a/roseus_bt/include/roseus_bt/eus_subscriber_node.h b/roseus_bt/include/roseus_bt/eus_subscriber_node.h index 31e1e0564..78bcc40d2 100644 --- a/roseus_bt/include/roseus_bt/eus_subscriber_node.h +++ b/roseus_bt/include/roseus_bt/eus_subscriber_node.h @@ -26,7 +26,8 @@ class EusSubscriberNode: public BT::ActionNodeBase static PortsList providedPorts() { return { InputPort("topic_name", "name of the subscribed topic"), - OutputPort("output_port", "port to where messages are redirected") + OutputPort("output_port", "port to where messages are redirected"), + OutputPort("received_port", "port set to true every time a message is received") }; } @@ -47,6 +48,7 @@ class EusSubscriberNode: public BT::ActionNodeBase protected: virtual void callback(MessageT msg) { setOutput("output_port", msg); + setOutput("received_port", (uint8_t)true); } }; From 46031e61c716d037ca7cac4d615150f40acb763e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 23:21:11 +0900 Subject: [PATCH 145/176] (roseus_bt) Update samples to use received_port in subscribers --- roseus_bt/include/roseus_bt/tutorial_parser.h | 2 +- roseus_bt/sample/models/t04_subscriber.xml | 7 ++++--- roseus_bt/sample/models/t06_reactive.xml | 3 ++- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/roseus_bt/include/roseus_bt/tutorial_parser.h b/roseus_bt/include/roseus_bt/tutorial_parser.h index 9a9fcf1e8..50ef0628f 100644 --- a/roseus_bt/include/roseus_bt/tutorial_parser.h +++ b/roseus_bt/include/roseus_bt/tutorial_parser.h @@ -42,7 +42,7 @@ std::string TutorialParser::format_node_body(const XMLElement* node, int padding if (id == "atSpot" || id == "atTableSpot" || id == "atBroomSpot") return fmt::format("{}(at-spot {})", pad, param_list.at(0)); if (id == "CheckCoords") return fmt::format( - "{}(not (equal (instance geometry_msgs::Pose :init) {}))", pad, param_list.at(0)); + "{}{}", pad, param_list.at(0)); // Actions if (id == "Init") return std::string("(init nil t)").insert(0, padding, ' '); diff --git a/roseus_bt/sample/models/t04_subscriber.xml b/roseus_bt/sample/models/t04_subscriber.xml index e233921ea..8a1a478a4 100644 --- a/roseus_bt/sample/models/t04_subscriber.xml +++ b/roseus_bt/sample/models/t04_subscriber.xml @@ -4,7 +4,7 @@ - + @@ -13,7 +13,7 @@ + found_coords="${found_coords}"/> @@ -38,7 +38,7 @@ - + @@ -46,6 +46,7 @@ + diff --git a/roseus_bt/sample/models/t06_reactive.xml b/roseus_bt/sample/models/t06_reactive.xml index 93c372e15..68d056e81 100644 --- a/roseus_bt/sample/models/t06_reactive.xml +++ b/roseus_bt/sample/models/t06_reactive.xml @@ -5,7 +5,7 @@ - + @@ -67,6 +67,7 @@ + From 3a934edfe11ad40f96a3be9c640a4a03ec72ec78 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 23:31:50 +0900 Subject: [PATCH 146/176] (roseus_bt) Update the subscriber documentation --- roseus_bt/sample/README.md | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index 8af543714..040497479 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -107,10 +107,9 @@ The fourth example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/rose Such port variables are initialized with an empty message instance and updated every time a new topic message arrives. -To do this we add an action with the `topic_name` and `output_port` fields in the `` section, and declare it as a `` and specify `message_type` and optionally `message_field` in the `` section. -Only proper ROS message types are supported by subscriber nodes (e.g. `std_msgs/Int64` instead of `int64`). +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. Optionally, `message_field` can also be assigned. -Note how we also add a step to verify and wait for messages. +Only proper ROS message types are supported by subscriber nodes (e.g. `std_msgs/Int64` instead of `int64`). #### Run the code @@ -178,8 +177,8 @@ The sixth example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseu 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. -Because in such scenario the condition nodes must be evaluated alongside the running action we prepare two distinct roseus servers -- one for actions and one for conditions. -On the action side it is also necessary to catch any interruption requests signalized by the `roseus_bt:cancel-action` condition. It is also possible to actively check for the presence of interruption requests with the `(send server :ok)`, when using custom preemption callbacks. +Because in such scenario the condition nodes must be evaluated alongside the running action, we prepare two distinct roseus servers -- one for actions and the other for conditions. +On the action side it is also necessary to check for the presence of interruption requests with the `(roseus_bt:ok)` function. #### Run the code From 67a8098b72dbbd8fad11f80256f616d87f9c604f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 28 May 2022 00:13:54 +0900 Subject: [PATCH 147/176] (roseus_bt) Add more documentation on reactive nodes --- roseus_bt/sample/README.md | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index 040497479..c379bcd8a 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -177,8 +177,16 @@ The sixth example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseu 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. -Because in such scenario the condition nodes must be evaluated alongside the running action, we prepare two distinct roseus servers -- one for actions and the other for conditions. -On the action side it is also necessary to check for the presence of interruption requests with the `(roseus_bt:ok)` function. +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 From c31655bd7292e2b1cac0ca8657272b10ad3e1111 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 28 May 2022 00:45:20 +0900 Subject: [PATCH 148/176] (roseus_bt) Update the remote tutorial documentation --- roseus_bt/sample/README.md | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index c379bcd8a..ba6a38992 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -243,11 +243,15 @@ https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_bt/sample/models 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 -roslaunch rosbridge_server rosbridge_websocket.launch +# source package before running this +roslaunch rosbridge_server rosbridge_websocket.launch unregister_timeout:=100000 ``` Run the first roseus server: @@ -258,8 +262,9 @@ 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 +roslaunch rosbridge_server rosbridge_websocket.launch port:=9091 unregister_timeout:=100000 ``` Run the second roseus server: From 64d0a8e49c24d16573503c3df203637bf6067b33 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 7 Jun 2022 20:30:24 +0900 Subject: [PATCH 149/176] (roseus_smach) Adapt to updated assoc key (see EusLisp/436) --- roseus_smach/src/state-machine.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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) From 6a5c9437c5f721eb5cf800a80171cdf3eef2b5fc Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 19 Oct 2022 16:21:55 +0900 Subject: [PATCH 150/176] set userdata as same as mydata --- roseus_smach/src/state-machine-utils.l | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/roseus_smach/src/state-machine-utils.l b/roseus_smach/src/state-machine-utils.l index e2ee985f7..6d98ee8a7 100644 --- a/roseus_smach/src/state-machine-utils.l +++ b/roseus_smach/src/state-machine-utils.l @@ -16,7 +16,8 @@ Args: Returns: the last active state " - (let ((insp (instance state-machine-inspector :init sm :root-name root-name :srv-name srv-name))) + (let ((insp (instance state-machine-inspector :init sm :root-name root-name :srv-name srv-name)) + (userdata mydata)) (unix::usleep (round (* 0.5 1e6))) (send sm :reset-state) (send insp :publish-structure) ;; publish once and latch From e5e739c9fccd45a6a3c047043ef21400f1884ae1 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 20 Oct 2022 14:17:31 +0900 Subject: [PATCH 151/176] pass userdata as arguments --- roseus_smach/src/state-machine-utils.l | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/roseus_smach/src/state-machine-utils.l b/roseus_smach/src/state-machine-utils.l index 6d98ee8a7..8272d4691 100644 --- a/roseus_smach/src/state-machine-utils.l +++ b/roseus_smach/src/state-machine-utils.l @@ -16,8 +16,7 @@ Args: Returns: the last active state " - (let ((insp (instance state-machine-inspector :init sm :root-name root-name :srv-name srv-name)) - (userdata mydata)) + (let ((insp (instance state-machine-inspector :init sm :root-name root-name :srv-name srv-name))) (unix::usleep (round (* 0.5 1e6))) (send sm :reset-state) (send insp :publish-structure) ;; publish once and latch @@ -44,9 +43,9 @@ Returns: (ros::ros-warn "aborting...") (return)) (t (error "value of key :iterate must be t or function")))))) - (if before-hook-func (funcall before-hook-func)) + (if before-hook-func (funcall before-hook-func mydata)) (send sm :execute mydata :step -1) - (if after-hook-func (funcall after-hook-func)) + (if after-hook-func (funcall after-hook-func mydata)) (ros::sleep)) (send sm :active-state)))) From 5802b7585f59adcec45cced1c0b931d2229fc132 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 11 Aug 2022 20:31:06 +0900 Subject: [PATCH 152/176] Allow optional argument on set-logger-level for backward compability --- roseus/roseus.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index daf911c16..1e092d471 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -1786,8 +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(1); - int log_level = intval(argv[0]); + 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: @@ -1811,6 +1814,7 @@ pointer ROSEUS_SET_LOGGER_LEVEL(register context *ctx, int n, pointer *argv) // 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) { From cbb18718c94b9f22aac07bdac9c81ee1acf778b3 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Sep 2022 00:08:00 +0900 Subject: [PATCH 153/176] Fix typos in ros::roseus warning --- roseus/roseus.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index 1e092d471..ccae884da 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -643,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(); } } From f05e39ad9b3672d606c35d6155cd5feb343a28e7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 26 Sep 2022 18:21:34 +0900 Subject: [PATCH 154/176] (roseus_bt) Update create_bt_tutorials help description --- roseus_bt/src/create_bt_tutorials.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus_bt/src/create_bt_tutorials.cpp b/roseus_bt/src/create_bt_tutorials.cpp index ec9ab824e..5c0450958 100644 --- a/roseus_bt/src/create_bt_tutorials.cpp +++ b/roseus_bt/src/create_bt_tutorials.cpp @@ -38,7 +38,7 @@ int main(int argc, char** argv) // Help if (args.count("help")) { - std::cout << "\n" << "Create behavior tree package." << "\n"; + std::cout << "\n" << "Create a behavior tree package with tutorial code." << "\n"; std::cout << desc << std::endl; return 0; } From 8b6a8b0f1ac38c54438fd128db9ffb22e05f5029 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 26 Sep 2022 18:22:18 +0900 Subject: [PATCH 155/176] (roseus_bt) Remove check-fn-closure --- roseus_bt/euslisp/nodes.l | 33 --------------------------------- 1 file changed, 33 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 5d1484003..4b2b20b6c 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -13,34 +13,6 @@ (defvar *condition-list*) -;; utility -(defun get-fn-sym (fn) - (when (functionp fn) - (cond - ((symbolp fn) fn) - ((and (listp fn) (memq (car fn) '(lambda lambda-closure))) - (cadr fn)) - ((derivedp fn compiled-code) - (send fn :name)) - (t nil)))) - -(defun check-fn-closure (fn) - (when (and (consp fn) - (eql (car fn) 'lambda-closure) - (or (not (zerop (third fn))) - (not (zerop (fourth fn))))) - (let ((name (get-fn-sym fn))) - (if name - (ros::ros-warn - (format nil "Possibly harmful lambda-closure in #'~A! Try using '~A instead." - name - name)) - (ros::ros-warn - (format nil "Possibly harmful lambda context in ~A! Reseting to 0..." - (append (subseq fn 0 5) '(...))) - (setf (third fn) 0) - (setf (fourth fn) 0)))))) - (defcondition cancel-action :slots (server goal)) (defun signals-cancel (server goal) @@ -54,9 +26,6 @@ (defmethod action-node (:init (ns spec &key execute-cb (preempt-cb 'signals-cancel) accept-cb groupname ((:monitor-groupname monitor-gn))) - (check-fn-closure execute-cb) - (check-fn-closure preempt-cb) - (check-fn-closure accept-cb) (send-super :init ns spec :execute-cb execute-cb @@ -111,7 +80,6 @@ (: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)) - (check-fn-closure res-if) (assert (numberp tmout)) (setq resume-if res-if) (setq timeout tmout) @@ -155,7 +123,6 @@ (defclass condition-node :super propertied-object :slots (execute-cb groupname)) (defmethod condition-node (:init (ns spec &key ((:execute-cb exec-cb)) ((:groupname gn))) - (check-fn-closure exec-cb) (setq execute-cb exec-cb) (setq groupname gn) (if groupname From 62135c44aa285d95c85851b67b2982f1084be31b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 26 Sep 2022 22:50:00 +0900 Subject: [PATCH 156/176] (roseus_bt) Add missing resume-cb slot in resumable-action-node --- roseus_bt/euslisp/nodes.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 4b2b20b6c..f4c6c486c 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -75,7 +75,7 @@ (ros::spin-once monitor-groupname) (send self :spin-once)))) -(defclass resumable-action-node :super action-node :slots (resume-if timeout cancel-cb)) +(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)) From a9ef96648165a15ef34b7b1eef4fb7d29cd00ae9 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 10 Oct 2022 12:58:23 +0900 Subject: [PATCH 157/176] (roseus_bt) Always wait for result in RemoteCondition --- roseus_bt/include/roseus_bt/eus_remote_condition_node.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_remote_condition_node.h b/roseus_bt/include/roseus_bt/eus_remote_condition_node.h index b63d2d561..493c6ec47 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_condition_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_condition_node.h @@ -76,9 +76,8 @@ class EusRemoteConditionNode : public BT::ActionNodeBase service_client_.call(request); } - if (service_client_.isActive()) { - return NodeStatus::RUNNING; - } + // Conditions cannot operate asynchronously + service_client_.waitForResult(); return onResponse(service_client_.getResult()); } From 335c75cc2cd8f0a988d0996e74b07190ba55d511 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 10 Oct 2022 23:36:44 +0900 Subject: [PATCH 158/176] (roseus_bt) Fix misspell: SetUInt -> SetUint --- roseus_bt/include/roseus_bt/xml_parser.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 65b090ecf..c571a4ad9 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -606,11 +606,11 @@ std::string XMLParser::format_get_remote_input(const XMLElement* node, const std 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")); + 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")); + 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") From 642ba9e315404b9529879d8321c35052d2833588 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 20 Oct 2022 16:25:36 +0900 Subject: [PATCH 159/176] (roseus_bt) Generate server launch files --- roseus_bt/include/roseus_bt/gen_template.h | 18 +++++++++ .../include/roseus_bt/package_generator.h | 37 ++++++++++++++++--- roseus_bt/include/roseus_bt/xml_parser.h | 32 ++++++++++++++++ 3 files changed, 82 insertions(+), 5 deletions(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 55fee97b0..271340f1a 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -19,6 +19,7 @@ class 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, @@ -86,6 +87,23 @@ bool success } +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 diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 569a8c25c..8379b0f04 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -61,10 +61,11 @@ class PackageGenerator private: Query query; PkgTemplate pkg_template; - std::vector parser_vector; + 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; @@ -75,6 +76,8 @@ class PackageGenerator 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); @@ -159,6 +162,22 @@ void PackageGenerator::write_service_files(Parser* parser) { } } +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, @@ -190,9 +209,12 @@ void PackageGenerator::write_eus_action_server(Parser* parser, for (auto it=server_files.begin(); it!=server_files.end(); ++it) { std::string remote_host = it->first; std::string body = it->second; - std::string dest_file = fmt::format("{}/{}{}-action-server.l", - base_dir, target_filename, remote_host); + std::string euslisp_filename = fmt::format("{}{}-action-server", + target_filename, remote_host); + 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 << "..."; @@ -213,9 +235,12 @@ void PackageGenerator::write_eus_condition_server(Parser* parser, for (auto it=server_files.begin(); it!=server_files.end(); ++it) { std::string remote_host = it->first; std::string body = it->second; - std::string dest_file = fmt::format("{}/{}{}-condition-server.l", - base_dir, target_filename, remote_host); + std::string euslisp_filename = fmt::format("{}{}-condition-server", + target_filename, remote_host); + 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 << "..."; @@ -274,6 +299,7 @@ void PackageGenerator::write_all_files() { Parser* parser = &parser_vector.at(i); std::string xml_filename = xml_filenames.at(i); std::string target_filename = target_filenames.at(i); + euslisp_filenames.clear(); BOOST_LOG_TRIVIAL(info) << "Generating " << xml_filename << " files..."; @@ -286,6 +312,7 @@ void PackageGenerator::write_all_files() { 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); diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index c571a4ad9..a5a0528ce 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -78,6 +78,8 @@ class XMLParser 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_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); @@ -107,6 +109,8 @@ class XMLParser std::string generate_cpp_file(const std::string package_name, const std::string roscpp_node_name, 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); @@ -640,6 +644,24 @@ std::string XMLParser::format_get_remote_input(const XMLElement* node, const std format_setvalue(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; @@ -1276,6 +1298,16 @@ std::string XMLParser::generate_cpp_file(const std::string package_name, 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) { From 17676bd957db417e938b4e52ed861b86345949da Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 20 Oct 2022 18:19:33 +0900 Subject: [PATCH 160/176] (roseus_bt) Force hyphens in euslisp file names --- .../include/roseus_bt/package_generator.h | 2 ++ roseus_bt/sample/README.md | 18 +++++++++--------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 8379b0f04..19e9a4206 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -211,6 +211,7 @@ void PackageGenerator::write_eus_action_server(Parser* parser, 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; @@ -237,6 +238,7 @@ void PackageGenerator::write_eus_condition_server(Parser* parser, 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; diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index ba6a38992..d7ab7e18d 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -27,7 +27,7 @@ Both the `` tag in the xml model file and the euslisp server are Run the roseus server ```bash roscd roseus_bt_tutorials/euslisp -roseus t01_simple_tree-action-server.l +roseus t01-simple-tree-action-server.l ``` Run the cpp client @@ -52,7 +52,7 @@ Every `` tag in the `` must be provided with an arb Run the roseus server ```bash roscd roseus_bt_tutorials/euslisp -roseus t02_conditions-action-server.l +roseus t02-conditions-action-server.l ``` Run the cpp client @@ -87,7 +87,7 @@ Conditions only support input ports, as they are not meant to do any changes in Run the roseus server ```bash roscd roseus_bt_tutorials/euslisp -roseus t03_ports-action-server.l +roseus t03-ports-action-server.l ``` Run the cpp client @@ -130,7 +130,7 @@ orientation: Run the roseus server ```bash roscd roseus_bt_tutorials/euslisp -roseus t04_subscriber-action-server.l +roseus t04-subscriber-action-server.l ``` Run the cpp client @@ -157,7 +157,7 @@ Each subtree inherits a separate blackboard and accepts remaps in the `inner_nam Run the roseus server ```bash roscd roseus_bt_tutorials/euslisp -roseus t05_subtrees-action-server.l +roseus t05-subtrees-action-server.l ``` Run the cpp client @@ -193,13 +193,13 @@ It is also possible to manually spin the monitor groupname with `(send server :s Run the roseus action server ```bash roscd roseus_bt_tutorials/euslisp -roseus t06_reactive-action-server.l +roseus t06-reactive-action-server.l ``` Run the roseus condition server ```bash roscd roseus_bt_tutorials/euslisp -roseus t06_reactive-condition-server.l +roseus t06-reactive-condition-server.l ``` Run the cpp client @@ -257,7 +257,7 @@ 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 +roseus t08-multimaster-localhost9090-action-server.l ``` Run the second rosbridge_server: @@ -271,7 +271,7 @@ 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 +roseus t08-multimaster-localhost9091-action-server.l ``` Run the cpp client From 3f9bc2aee27fad96edb7fc60fe977d473c848685 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 10:45:56 +0900 Subject: [PATCH 161/176] (roseus_bt) Add CopyDocument class to avoid unnecessary dumping and parsing --- roseus_bt/include/roseus_bt/copy_document.h | 66 +++++++++++++++++++ .../roseus_bt/eus_remote_action_node.h | 8 +-- roseus_bt/include/roseus_bt/gen_template.h | 3 +- roseus_bt/include/roseus_bt/xml_parser.h | 9 ++- 4 files changed, 74 insertions(+), 12 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/copy_document.h 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..1cfd75ae2 --- /dev/null +++ b/roseus_bt/include/roseus_bt/copy_document.h @@ -0,0 +1,66 @@ +#ifndef JSON_COPY_DOCUMENT_ +#define JSON_COPY_DOCUMENT_ + + +#include "rapidjson/include/rapidjson/document.h" +#include "rapidjson/include/rapidjson/writer.h" +#include "rapidjson/include/rapidjson/stringbuffer.h" + + +namespace rapidjson +{ +class CopyDocument : public rapidjson::Document +{ +public: + CopyDocument() : Document() {} + + 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()); + } +}; + +} + +#endif // JSON_COPY_DOCUMENT diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h index 86b354646..d12b47e1f 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -4,6 +4,7 @@ #include #include #include +#include namespace BT @@ -123,12 +124,9 @@ class EusRemoteActionNode : public BT::ActionNodeBase { if (message["update_field_name"].GetString() != name) return; - rapidjson::StringBuffer strbuf; - rapidjson::Writer writer(strbuf); - rapidjson::Document document; + rapidjson::CopyDocument document; document.CopyFrom(message[name.c_str()], document.GetAllocator()); - document.Accept(writer); - setOutput(name, strbuf.GetString()); + setOutput(name, document); } }; diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 271340f1a..0d1bcd3cb 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -188,8 +188,7 @@ std::string GenTemplate::remote_action_class_template( { auto format_send_goal = [](const std::string body) { std::string decl = 1 + R"( - std::string json; - rapidjson::Document document; + rapidjson::CopyDocument document; GoalType ros_msg; )"; if (!body.empty()) return fmt::format("{}{}", decl, body); diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index a5a0528ce..095bde339 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -628,8 +628,7 @@ std::string XMLParser::format_get_remote_input(const XMLElement* node, const std if (msg_type.find('/') != std::string::npos) // parse ros message from json string return fmt::format(R"( - getInput("{0}", json); - document.Parse(json.c_str()); + getInput("{0}", document); rapidjson::Value {0}(document, document.GetAllocator()); {1}->AddMember("{0}", {0}, {1}->GetAllocator());)", node->Attribute("name"), @@ -827,14 +826,14 @@ std::string XMLParser::generate_remote_action_class(const XMLElement* node, cons auto format_input_port = [](const XMLElement* node) { std::string msg_type = node->Attribute("type"); if (msg_type.find('/') != std::string::npos) - // ros messages are parsed from json - return fmt::format(" InputPort(\"{0}\")", + // 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) { - return fmt::format(" OutputPort(\"{0}\")", + return fmt::format(" OutputPort(\"{0}\")", node->Attribute("name")); }; auto format_set_output = [](const XMLElement* node) { From 600bbe9f5eb128d76456d90f3c06d6e178581cab Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 13:15:08 +0900 Subject: [PATCH 162/176] (roseus_bt) Allow to parse to basic ros types in remote action --- .../roseus_bt/eus_remote_action_node.h | 1 + roseus_bt/include/roseus_bt/xml_parser.h | 50 ++++++++++++++++--- 2 files changed, 44 insertions(+), 7 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h index d12b47e1f..d139ebde9 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -34,6 +34,7 @@ class EusRemoteActionNode : public BT::ActionNodeBase 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; diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 095bde339..95020faf5 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -78,6 +78,7 @@ class XMLParser 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); @@ -626,7 +627,6 @@ std::string XMLParser::format_get_remote_input(const XMLElement* node, const std std::string msg_type = node->Attribute("type"); if (msg_type.find('/') != std::string::npos) - // parse ros message from json string return fmt::format(R"( getInput("{0}", document); rapidjson::Value {0}(document, document.GetAllocator()); @@ -643,6 +643,42 @@ std::string XMLParser::format_get_remote_input(const XMLElement* node, const std 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"( @@ -833,13 +869,13 @@ std::string XMLParser::generate_remote_action_class(const XMLElement* node, cons node->Attribute("name")); }; auto format_output_port = [](const XMLElement* node) { - return fmt::format(" OutputPort(\"{0}\")", + 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")); }; - auto format_set_output = [](const XMLElement* node) { - return fmt::format(" setOutputFromMessage(\"{0}\", feedback);", - node->Attribute("name")); - }; std::vector provided_input_ports; std::vector provided_output_ports; @@ -863,7 +899,7 @@ std::string XMLParser::generate_remote_action_class(const XMLElement* node, cons } 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)); + set_outputs.push_back(format_set_remote_output(port_node)); } } From 8c07fa5035cbeee0ab8c3fe53719c2844212445d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 19:48:17 +0900 Subject: [PATCH 163/176] (roseus_bt) Add command line arguments for initializing blackboard variables --- .../roseus_bt/command_line_argument_mapping.h | 62 +++++++++++++++++++ roseus_bt/include/roseus_bt/gen_template.h | 18 ++++-- .../include/roseus_bt/package_generator.h | 3 +- roseus_bt/include/roseus_bt/xml_parser.h | 13 +++- 4 files changed, 88 insertions(+), 8 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/command_line_argument_mapping.h 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..ceab8cd30 --- /dev/null +++ b/roseus_bt/include/roseus_bt/command_line_argument_mapping.h @@ -0,0 +1,62 @@ +#include +#include +#include +#include +#include +#include +#include + + +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::parsed_options parsed_options = po::command_line_parser(argc, argv).options(desc).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) +{ + 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); + } +} diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 0d1bcd3cb..1a5dcc282 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -42,6 +42,7 @@ class GenTemplate 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, @@ -110,6 +111,7 @@ std::string GenTemplate::headers_template(std::vector headers) { #define DEBUG // rosbridgecpp logging #include +#include #include #include #include @@ -441,6 +443,7 @@ class %1%: public EusRemoteSubscriberNode<%2%> 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, @@ -455,16 +458,22 @@ std::string GenTemplate::main_function_template(std::string roscpp_node_name, std::string fmt_string = 1 + R"( int main(int argc, char **argv) { -%1% + std::map init_variables; + if (!parse_command_line(argc, argv, "%1%", init_variables)) { + return 1; + } + +%2% ros::NodeHandle nh; BehaviorTreeFactory factory; -%3%%4%%5% -%2% +%4%%5%%6% +%3% + register_blackboard_variables(&tree, init_variables); std::string timestamp = std::to_string(ros::Time::now().toNSec()); - std::string log_filename(fmt::format("%6%", timestamp)); + std::string log_filename(fmt::format("%7%", timestamp)); StdCoutLogger logger_cout(tree); FileLogger logger_file(tree, log_filename.c_str()); @@ -494,6 +503,7 @@ int main(int argc, char **argv) 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") % diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 19e9a4206..21108232c 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -186,6 +186,7 @@ void PackageGenerator::write_cpp_file(Parser* parser, 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)) @@ -193,7 +194,7 @@ void PackageGenerator::write_cpp_file(Parser* parser, BOOST_LOG_TRIVIAL(info) << "Writing " << dest_file << "..."; std::ofstream output_file(dest_file); - output_file << parser->generate_cpp_file(package_name, roscpp_node_name, + output_file << parser->generate_cpp_file(package_name, roscpp_node_name, program_description, boost::filesystem::absolute(xml_filename).c_str()); output_file.close(); } diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 95020faf5..d8d8902c4 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -90,7 +90,9 @@ class XMLParser 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 xml_filename); + 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); @@ -109,6 +111,7 @@ class XMLParser 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); @@ -1045,6 +1048,7 @@ std::string XMLParser::generate_remote_subscriber_class(const XMLElement* node) } 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);", @@ -1118,7 +1122,9 @@ std::string XMLParser::generate_main_function(const std::string roscpp_node_name register_subscribers.push_back(format_remote_subscriber_node(subscriber_node)); } - return gen_template.main_function_template(roscpp_node_name, xml_filename, + return gen_template.main_function_template(roscpp_node_name, + program_description, + xml_filename, register_actions, register_conditions, register_subscribers); @@ -1274,6 +1280,7 @@ std::map XMLParser::generate_all_eus_condition_servers 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; @@ -1328,7 +1335,7 @@ std::string XMLParser::generate_cpp_file(const std::string package_name, output.append("\n\n"); } - output.append(generate_main_function(roscpp_node_name, xml_filename)); + output.append(generate_main_function(roscpp_node_name, program_description, xml_filename)); return output; } From ee09f2bf8e5562b57af03675fd77946427747a39 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 21:31:50 +0900 Subject: [PATCH 164/176] (roseus_bt) Standardize namespaces and loadguards --- roseus_bt/include/roseus_bt/basic_types.h | 6 +++--- .../roseus_bt/command_line_argument_mapping.h | 13 +++++++++++++ roseus_bt/include/roseus_bt/copy_document.h | 9 ++++----- roseus_bt/include/roseus_bt/eus_action_node.h | 6 +++--- roseus_bt/include/roseus_bt/eus_condition_node.h | 6 +++--- roseus_bt/include/roseus_bt/eus_nodes.h | 6 +++--- .../include/roseus_bt/eus_remote_action_node.h | 10 ++++------ .../include/roseus_bt/eus_remote_condition_node.h | 8 ++++---- .../include/roseus_bt/eus_remote_subscriber_node.h | 2 +- roseus_bt/include/roseus_bt/eus_subscriber_node.h | 8 +++----- roseus_bt/include/roseus_bt/gen_template.h | 5 ++--- roseus_bt/include/roseus_bt/tutorial_parser.h | 3 ++- roseus_bt/include/roseus_bt/ws_action_client.h | 13 +++++++++---- roseus_bt/include/roseus_bt/ws_service_client.h | 13 +++++++++---- roseus_bt/include/roseus_bt/ws_subscriber_client.h | 12 +++++++++--- roseus_bt/include/roseus_bt/xml_exceptions.h | 6 +++--- roseus_bt/include/roseus_bt/xml_parser.h | 3 ++- 17 files changed, 77 insertions(+), 52 deletions(-) diff --git a/roseus_bt/include/roseus_bt/basic_types.h b/roseus_bt/include/roseus_bt/basic_types.h index 513c61a28..b85a4d141 100644 --- a/roseus_bt/include/roseus_bt/basic_types.h +++ b/roseus_bt/include/roseus_bt/basic_types.h @@ -1,5 +1,5 @@ -#ifndef BEHAVIOR_TREE_EUS_NODE_TYPE_HPP_ -#define BEHAVIOR_TREE_EUS_NODE_TYPE_HPP_ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_NODE_TYPE_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_NODE_TYPE_HPP_ #include @@ -27,4 +27,4 @@ std::ostream& operator<<(std::ostream& os, const NodeType& type); } // namespace roseus_bt -#endif // BEHAVIOR_TREE_EUS_NODE_TYPE_HPP_ +#endif // BEHAVIOR_TREE_ROSEUS_BT_EUS_NODE_TYPE_HPP_ diff --git a/roseus_bt/include/roseus_bt/command_line_argument_mapping.h b/roseus_bt/include/roseus_bt/command_line_argument_mapping.h index ceab8cd30..922104a3a 100644 --- a/roseus_bt/include/roseus_bt/command_line_argument_mapping.h +++ b/roseus_bt/include/roseus_bt/command_line_argument_mapping.h @@ -1,3 +1,6 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_COMMAND_LINE_ARGUMENT_MAPPING_ +#define BEHAVIOR_TREE_ROSEUS_BT_COMMAND_LINE_ARGUMENT_MAPPING_ + #include #include #include @@ -7,6 +10,9 @@ #include +namespace roseus_bt +{ + namespace po = boost::program_options; bool parse_command_line(int argc, char** argv, @@ -52,6 +58,9 @@ bool parse_command_line(int argc, char** argv, 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 << @@ -60,3 +69,7 @@ void register_blackboard_variables(BT::Tree* tree, 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/copy_document.h b/roseus_bt/include/roseus_bt/copy_document.h index 1cfd75ae2..b29214f26 100644 --- a/roseus_bt/include/roseus_bt/copy_document.h +++ b/roseus_bt/include/roseus_bt/copy_document.h @@ -1,6 +1,5 @@ -#ifndef JSON_COPY_DOCUMENT_ -#define JSON_COPY_DOCUMENT_ - +#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" @@ -61,6 +60,6 @@ class CopyDocument : public rapidjson::Document } }; -} +} // namespace rapidjson -#endif // JSON_COPY_DOCUMENT +#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 index 5b6ca5687..68bcdd321 100644 --- a/roseus_bt/include/roseus_bt/eus_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_action_node.h @@ -1,5 +1,5 @@ -#ifndef BEHAVIOR_TREE_EUS_ACTION_NODE_HPP_ -#define BEHAVIOR_TREE_EUS_ACTION_NODE_HPP_ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_ACTION_NODE_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_ACTION_NODE_HPP_ #include @@ -111,4 +111,4 @@ class EusActionNode: public BT::RosActionNode } // namespace BT -#endif // BEHAVIOR_TREE_EUS_ACTION_NODE_HPP_ +#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 index f4c1d2a9d..fc61d13a7 100644 --- a/roseus_bt/include/roseus_bt/eus_condition_node.h +++ b/roseus_bt/include/roseus_bt/eus_condition_node.h @@ -1,5 +1,5 @@ -#ifndef BEHAVIOR_TREE_EUS_CONDITION_NODE_HPP_ -#define BEHAVIOR_TREE_EUS_CONDITION_NODE_HPP_ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_CONDITION_NODE_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_CONDITION_NODE_HPP_ #include @@ -30,4 +30,4 @@ class EusConditionNode : public BT::RosServiceNode } // namespace BT -#endif // BEHAVIOR_TREE_EUS_CONDITION_NODE_HPP_ +#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 index 601e6d676..6f1c57e3c 100644 --- a/roseus_bt/include/roseus_bt/eus_nodes.h +++ b/roseus_bt/include/roseus_bt/eus_nodes.h @@ -1,5 +1,5 @@ -#ifndef BEHAVIOR_TREE_EUS_NODES_HPP_ -#define BEHAVIOR_TREE_EUS_NODES_HPP_ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_NODES_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_NODES_HPP_ #include "eus_action_node.h" #include "eus_condition_node.h" @@ -8,4 +8,4 @@ #include "eus_remote_condition_node.h" #include "eus_remote_subscriber_node.h" -#endif // BEHAVIOR_TREE_EUS_NODES_HPP_ +#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 index d139ebde9..2ef3b095c 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -1,5 +1,5 @@ -#ifndef BEHAVIOR_TREE_EUS_REMOTE_ACTION_NODE_HPP_ -#define BEHAVIOR_TREE_EUS_REMOTE_ACTION_NODE_HPP_ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_REMOTE_ACTION_NODE_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_REMOTE_ACTION_NODE_HPP_ #include #include @@ -68,7 +68,7 @@ class EusRemoteActionNode : public BT::ActionNodeBase } protected: - RosbridgeActionClient action_client_; + roseus_bt::RosbridgeActionClient action_client_; std::string server_name_; std::string goal_topic_; @@ -150,8 +150,6 @@ template static factory.registerBuilder( manifest, builder ); } - } // namespace BT - -#endif // BEHAVIOR_TREE_BT_REMOTE_ACTION_NODE_HPP_ +#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 index 493c6ec47..ee15b9208 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_condition_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_condition_node.h @@ -1,5 +1,5 @@ -#ifndef BEHAVIOR_TREE_EUS_REMOTE_CONDITION_NODE_HPP_ -#define BEHAVIOR_TREE_EUS_REMOTE_CONDITION_NODE_HPP_ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_REMOTE_CONDITION_NODE_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_REMOTE_CONDITION_NODE_HPP_ #include #include @@ -63,7 +63,7 @@ class EusRemoteConditionNode : public BT::ActionNodeBase } protected: - RosbridgeServiceClient service_client_; + roseus_bt::RosbridgeServiceClient service_client_; BT::NodeStatus tick() override { @@ -105,4 +105,4 @@ template static } // namespace BT -#endif // BEHAVIOR_TREE_EUS_REMOTE_CONDITION_NODE_HPP_ +#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 index 3d917eee4..b1285143a 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h @@ -52,7 +52,7 @@ class EusRemoteSubscriberNode: public BT::ActionNodeBase } protected: - RosbridgeSubscriberClient subscriber_client_; + roseus_bt::RosbridgeSubscriberClient subscriber_client_; protected: virtual void callback(const rapidjson::Value& msg) { diff --git a/roseus_bt/include/roseus_bt/eus_subscriber_node.h b/roseus_bt/include/roseus_bt/eus_subscriber_node.h index 78bcc40d2..8cb259c18 100644 --- a/roseus_bt/include/roseus_bt/eus_subscriber_node.h +++ b/roseus_bt/include/roseus_bt/eus_subscriber_node.h @@ -1,5 +1,5 @@ -#ifndef BEHAVIOR_TREE_EUS_SUBSCRIBER_NODE_HPP_ -#define BEHAVIOR_TREE_EUS_SUBSCRIBER_NODE_HPP_ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_SUBSCRIBER_NODE_HPP_ +#define BEHAVIOR_TREE_ROSEUS_BT_EUS_SUBSCRIBER_NODE_HPP_ namespace BT @@ -75,6 +75,4 @@ template static } // namespace BT -#endif // BEHAVIOR_TREE_EUS_SUBSCRIBER_NODE_HPP_ - - +#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 index 1a5dcc282..5c52ca7b3 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -459,7 +459,7 @@ std::string GenTemplate::main_function_template(std::string roscpp_node_name, int main(int argc, char **argv) { std::map init_variables; - if (!parse_command_line(argc, argv, "%1%", init_variables)) { + if (!roseus_bt::parse_command_line(argc, argv, "%1%", init_variables)) { return 1; } @@ -470,7 +470,7 @@ int main(int argc, char **argv) %4%%5%%6% %3% - register_blackboard_variables(&tree, init_variables); + 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)); @@ -563,7 +563,6 @@ std::string GenTemplate::eus_server_template(std::string server_type, return bfmt.str(); } - } // namespace RoseusBT #endif // BEHAVIOR_TREE_ROSEUS_BT_GEN_TEMPLATE_ diff --git a/roseus_bt/include/roseus_bt/tutorial_parser.h b/roseus_bt/include/roseus_bt/tutorial_parser.h index 50ef0628f..5202f7700 100644 --- a/roseus_bt/include/roseus_bt/tutorial_parser.h +++ b/roseus_bt/include/roseus_bt/tutorial_parser.h @@ -6,7 +6,8 @@ namespace RoseusBT { - using namespace tinyxml2; + +using namespace tinyxml2; class TutorialParser : public XMLParser { diff --git a/roseus_bt/include/roseus_bt/ws_action_client.h b/roseus_bt/include/roseus_bt/ws_action_client.h index 2ec0333ee..21ad7148e 100644 --- a/roseus_bt/include/roseus_bt/ws_action_client.h +++ b/roseus_bt/include/roseus_bt/ws_action_client.h @@ -1,9 +1,13 @@ -#ifndef WS_ACTION_CLIENT_ -#define WS_ACTION_CLIENT_ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_WS_ACTION_CLIENT_ +#define BEHAVIOR_TREE_ROSEUS_BT_WS_ACTION_CLIENT_ #include #include + +namespace roseus_bt +{ + class RosbridgeActionClient { public: @@ -108,7 +112,8 @@ class RosbridgeActionClient is_active_ = false; } - }; -#endif // WS_ACTION_CLIENT_ +} // 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 index f8ca850f4..366570ba7 100644 --- a/roseus_bt/include/roseus_bt/ws_service_client.h +++ b/roseus_bt/include/roseus_bt/ws_service_client.h @@ -1,9 +1,13 @@ -#ifndef WS_SERVICE_CLIENT_ -#define WS_SERVICE_CLIENT_ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_WS_SERVICE_CLIENT_ +#define BEHAVIOR_TREE_ROSEUS_BT_WS_SERVICE_CLIENT_ #include #include + +namespace roseus_bt +{ + class RosbridgeServiceClient { public: @@ -71,7 +75,8 @@ class RosbridgeServiceClient is_active_ = false; connection->send_close(1000); } - }; -#endif // WS_SERVICE_CLIENT_ +} // 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 index 1cd90ee44..f8f01d22f 100644 --- a/roseus_bt/include/roseus_bt/ws_subscriber_client.h +++ b/roseus_bt/include/roseus_bt/ws_subscriber_client.h @@ -1,9 +1,13 @@ -#ifndef WS_SUBSCRIBER_CLIENT_ -#define WS_SUBSCRIBER_CLIENT_ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_WS_SUBSCRIBER_CLIENT_ +#define BEHAVIOR_TREE_ROSEUS_BT_WS_SUBSCRIBER_CLIENT_ #include #include + +namespace roseus_bt +{ + class RosbridgeSubscriberClient { public: @@ -27,4 +31,6 @@ class RosbridgeSubscriberClient std::string topic_name_; }; -#endif // WS_SUBSCRIBER_CLIENT_ +} // 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 index b924ace12..23edb6121 100644 --- a/roseus_bt/include/roseus_bt/xml_exceptions.h +++ b/roseus_bt/include/roseus_bt/xml_exceptions.h @@ -9,7 +9,8 @@ namespace XMLError { - using namespace tinyxml2; + +using namespace tinyxml2; class XMLError: public std::exception { public: @@ -81,7 +82,6 @@ class InvalidPortType: public XMLError { XMLError(fmt::format("Invalid port type {}{}", type, get_place(node))) {}; }; - -} // namespace RoseusBT +} // 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 index d8d8902c4..e66919036 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -18,7 +18,8 @@ void push_new(std::string elem, std::vector* vec) { namespace RoseusBT { - using namespace tinyxml2; + +using namespace tinyxml2; class XMLParser { From d4cd7d2f8bab35584da4be36f73a32118a9321bb Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 22:31:22 +0900 Subject: [PATCH 165/176] (roseus_bt) Add positional argument check and negative number support in command line parser --- .../include/roseus_bt/command_line_argument_mapping.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/command_line_argument_mapping.h b/roseus_bt/include/roseus_bt/command_line_argument_mapping.h index 922104a3a..7c4f4e3d0 100644 --- a/roseus_bt/include/roseus_bt/command_line_argument_mapping.h +++ b/roseus_bt/include/roseus_bt/command_line_argument_mapping.h @@ -28,7 +28,12 @@ bool parse_command_line(int argc, char** argv, ("arg", po::value>()->multitoken(), "Initial blackboard variable-value pairs"); - po::parsed_options parsed_options = po::command_line_parser(argc, argv).options(desc).run(); + 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); From 077a337ea4da0d654435cbd7d66d734e12b01c19 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 22:32:32 +0900 Subject: [PATCH 166/176] (roseus_bt) Add implicit blackboard conversions for int8, int16, and bool --- .../include/roseus_bt/convert_from_string.h | 39 +++++++++++++++++++ roseus_bt/include/roseus_bt/eus_nodes.h | 2 + 2 files changed, 41 insertions(+) create mode 100644 roseus_bt/include/roseus_bt/convert_from_string.h 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/eus_nodes.h b/roseus_bt/include/roseus_bt/eus_nodes.h index 6f1c57e3c..5d97b4301 100644 --- a/roseus_bt/include/roseus_bt/eus_nodes.h +++ b/roseus_bt/include/roseus_bt/eus_nodes.h @@ -1,6 +1,8 @@ #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" From b3645fa781f1c31558f41575087286519277e00f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 22 Oct 2022 16:21:33 +0900 Subject: [PATCH 167/176] (roseus_bt) Decrease sleep time in remote nodes --- roseus_bt/include/roseus_bt/ws_action_client.h | 2 +- roseus_bt/include/roseus_bt/ws_service_client.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/ws_action_client.h b/roseus_bt/include/roseus_bt/ws_action_client.h index 21ad7148e..6797f1ea3 100644 --- a/roseus_bt/include/roseus_bt/ws_action_client.h +++ b/roseus_bt/include/roseus_bt/ws_action_client.h @@ -76,7 +76,7 @@ class RosbridgeActionClient void waitForResult() { std::cout << "RemoteAction: waiting for result: " << result_topic_ << std::endl; while (is_active_) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } } diff --git a/roseus_bt/include/roseus_bt/ws_service_client.h b/roseus_bt/include/roseus_bt/ws_service_client.h index 366570ba7..dbe3e8734 100644 --- a/roseus_bt/include/roseus_bt/ws_service_client.h +++ b/roseus_bt/include/roseus_bt/ws_service_client.h @@ -44,7 +44,7 @@ class RosbridgeServiceClient void waitForResult() { std::cout << "RemoteService: waiting for result: " << service_name_ << std::endl; while (is_active_) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } } From 84cef5d53f74c11395f23fa092da2d5890279e19 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 10:02:57 +0900 Subject: [PATCH 168/176] (roseus_bt) Use CopyDocument in remote subscriber --- roseus_bt/include/roseus_bt/eus_remote_action_node.h | 2 -- .../include/roseus_bt/eus_remote_subscriber_node.h | 10 ++++------ 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_remote_action_node.h b/roseus_bt/include/roseus_bt/eus_remote_action_node.h index 2ef3b095c..441e89688 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -102,8 +102,6 @@ class EusRemoteActionNode : public BT::ActionNodeBase } // port related - // TODO: avoid unnecessary dumping & parsing - // cannot use rapidjson::Value directly because it is not `CopyConstructible' // TODO: translate to ROS message: how to loop through slots? void feedbackCallback(std::shared_ptr connection, diff --git a/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h b/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h index b1285143a..a736114d4 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h @@ -4,6 +4,7 @@ #include #include #include +#include namespace BT { @@ -34,7 +35,7 @@ class EusRemoteSubscriberNode: public BT::ActionNodeBase static PortsList providedPorts() { return { InputPort("topic_name", "name of the subscribed topic"), - OutputPort("output_port", "port to where messages are redirected"), + 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") @@ -76,12 +77,9 @@ class EusRemoteSubscriberNode: public BT::ActionNodeBase void setOutputFromMessage(const std::string& name, const rapidjson::Value& message) { - rapidjson::StringBuffer strbuf; - rapidjson::Writer writer(strbuf); - rapidjson::Document document; + rapidjson::CopyDocument document; document.CopyFrom(message, document.GetAllocator()); - document.Accept(writer); - setOutput(name, strbuf.GetString()); + setOutput(name, document); } }; From 1918a4d62a0a6617b71cf79103a80a8dbf1e7602 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 12:27:09 +0900 Subject: [PATCH 169/176] (roseus_bt) Inform topic type in remote subscribers --- .../include/roseus_bt/eus_remote_subscriber_node.h | 3 ++- roseus_bt/include/roseus_bt/ws_subscriber_client.h | 10 +++++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h b/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h index a736114d4..37fe5f5d0 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h @@ -17,7 +17,8 @@ class EusRemoteSubscriberNode: public BT::ActionNodeBase BT::ActionNodeBase(name, conf), subscriber_client_(getInput("host_name").value(), getInput("host_port").value(), - getInput("topic_name").value()) + getInput("topic_name").value(), + getInput("message_type").value()) { auto cb = std::bind(&EusRemoteSubscriberNode::topicCallback, this, std::placeholders::_1, diff --git a/roseus_bt/include/roseus_bt/ws_subscriber_client.h b/roseus_bt/include/roseus_bt/ws_subscriber_client.h index f8f01d22f..324cbe2ee 100644 --- a/roseus_bt/include/roseus_bt/ws_subscriber_client.h +++ b/roseus_bt/include/roseus_bt/ws_subscriber_client.h @@ -11,9 +11,12 @@ namespace roseus_bt class RosbridgeSubscriberClient { public: - RosbridgeSubscriberClient(const std::string& master, int port, const std::string& topic_name): + 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_name_(topic_name), + topic_type_(topic_type) { rbc_.addClient("topic_subscriber"); } @@ -23,12 +26,13 @@ class RosbridgeSubscriberClient } void registerCallback(auto callback) { - rbc_.subscribe("topic_subscriber", topic_name_, callback); + rbc_.subscribe("topic_subscriber", topic_name_, callback, "", topic_type_); } protected: RosbridgeWsClient rbc_; std::string topic_name_; + std::string topic_type_; }; } // namespace roseus_bt From 2aa29279c731af1b0f53b32a559a8d9b881b9a1e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 15:26:45 +0900 Subject: [PATCH 170/176] (roseus_bt) Add single variable constructor in CopyDocument --- roseus_bt/include/roseus_bt/copy_document.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/roseus_bt/include/roseus_bt/copy_document.h b/roseus_bt/include/roseus_bt/copy_document.h index b29214f26..e3cb57cdb 100644 --- a/roseus_bt/include/roseus_bt/copy_document.h +++ b/roseus_bt/include/roseus_bt/copy_document.h @@ -3,6 +3,7 @@ #include "rapidjson/include/rapidjson/document.h" #include "rapidjson/include/rapidjson/writer.h" +#include "rapidjson/include/rapidjson/prettywriter.h" #include "rapidjson/include/rapidjson/stringbuffer.h" @@ -13,6 +14,8 @@ class CopyDocument : public rapidjson::Document public: CopyDocument() : Document() {} + CopyDocument(Type type) : Document(type) {} + CopyDocument(const CopyDocument& document) { _clone(document); } From 775e813b72a599315b0825cccb1ee5d1225d1182 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 17:13:12 +0900 Subject: [PATCH 171/176] (roseus_bt) Avoid duplicated header generation --- roseus_bt/include/roseus_bt/xml_parser.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index e66919036..459c88e32 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -767,42 +767,42 @@ std::string XMLParser::generate_headers(const std::string package_name) { action_node != nullptr; action_node = action_node->NextSiblingElement("Action")) { - headers.push_back(format_action_node(action_node, package_name)); + 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")) { - headers.push_back(format_action_node(action_node, package_name)); + 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")) { - headers.push_back(format_condition_node(condition_node, package_name)); + 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")) { - headers.push_back(format_condition_node(condition_node, package_name)); + 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")) { - headers.push_back(format_subscriber_node(subscriber_node)); + push_new(format_subscriber_node(subscriber_node), &headers); } for (auto subscriber_node = root->FirstChildElement("RemoteSubscriber"); subscriber_node != nullptr; subscriber_node = subscriber_node->NextSiblingElement("RemoteSubscriber")) { - headers.push_back(format_subscriber_node(subscriber_node)); + push_new(format_subscriber_node(subscriber_node), &headers); } return gen_template.headers_template(headers); From 80c86c9a6ee50c27337bceec02142b8b73090c2b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 20:06:55 +0900 Subject: [PATCH 172/176] (roseus_bt) Drop support for type unsafe message_field subscriber option --- roseus_bt/include/roseus_bt/gen_template.h | 19 +------------------ roseus_bt/include/roseus_bt/xml_parser.h | 5 ----- roseus_bt/sample/README.md | 2 +- 3 files changed, 2 insertions(+), 24 deletions(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 5c52ca7b3..85f540591 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -37,7 +37,6 @@ class GenTemplate std::vector provided_ports, std::vector get_inputs); std::string subscriber_class_template(std::string nodeID, std::string message_type, - std::string message_field, std::vector provided_ports); std::string remote_subscriber_class_template(std::string nodeID, std::string message_type, std::vector provided_ports); @@ -352,23 +351,9 @@ class %2%: public EusRemoteConditionNode<%1%::%2%> std::string GenTemplate::subscriber_class_template(std::string nodeID, std::string message_type, - std::string message_field, std::vector provided_ports) { std::string provided_ports_body; - if (!message_field.empty()) { - std::string fmt_string = R"( - virtual void callback(%1% msg) { - setOutput("output_port", msg.%2%); - })"; - - boost::format bfmt = boost::format(fmt_string) % - message_type % - message_field; - - message_field = bfmt.str(); - } - if (!provided_ports.empty()) { std::string fmt_string = R"( static PortsList providedPorts() @@ -391,14 +376,12 @@ class %1%: public EusSubscriberNode<%2%> %1%(ros::NodeHandle& handle, const std::string& name, const NodeConfiguration& conf) : EusSubscriberNode<%2%>(handle, name, conf) {} %3% -%4% }; )"; boost::format bfmt = boost::format(fmt_string) % nodeID % message_type % - provided_ports_body % - message_field; + provided_ports_body; return bfmt.str(); } diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 459c88e32..c2edf456a 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -983,10 +983,6 @@ std::string XMLParser::generate_subscriber_class(const XMLElement* node) { } return fmt::format("{}::{}", type.substr(0, pos), type.substr(1+pos)); }; - auto format_field = [](const XMLElement* node) { - if (!node->Attribute("message_field")) return ""; - return node->Attribute("message_field"); - }; auto format_port = [](const XMLElement* port_node) { return fmt::format( " InputPort(\"topic_name\", \"{0}\", \"name of the subscribed topic\")", @@ -1007,7 +1003,6 @@ std::string XMLParser::generate_subscriber_class(const XMLElement* node) { return gen_template.subscriber_class_template(node->Attribute("ID"), format_type(node), - format_field(node), provided_ports); } diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index d7ab7e18d..43e9cad30 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -107,7 +107,7 @@ The fourth example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/rose 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. Optionally, `message_field` can also be assigned. +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`). From 5fcea9a347985c9fffb0a42fcc98702356d9d9a1 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 22:24:08 +0900 Subject: [PATCH 173/176] (roseus_bt) Increase server unregister_timeout (https://github.com/knorth55/jsk_robot/pull/230) --- roseus_bt/include/roseus_bt/gen_template.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 85f540591..931fdf5c9 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -91,7 +91,9 @@ std::string GenTemplate::launch_file_template(std::vector launch_no { std::string fmt_string = 1 + R"( - + + + %1% From 551b51e11bb6e803bf5bcc63cdf17fe909574dfa Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 11 Nov 2022 22:29:21 +0900 Subject: [PATCH 174/176] add dynamic-reconfigure-server originally written by @YoheiKakiuchi --- roseus/euslisp/dynamic-reconfigure-server.l | 306 ++++++++++++++++++++ 1 file changed, 306 insertions(+) create mode 100644 roseus/euslisp/dynamic-reconfigure-server.l 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)) +|# From 6ceae78aa799c3db70dcdc2818fb0b8197e2d1d1 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 14 Nov 2022 17:12:50 +0900 Subject: [PATCH 175/176] add dynamic_reconfigure_server test --- roseus/CMakeLists.txt | 1 + roseus/test/test-dynamic-reconfigure-server.l | 130 ++++++++++++++++++ .../test/test-dynamic-reconfigure-server.test | 3 + 3 files changed, 134 insertions(+) create mode 100644 roseus/test/test-dynamic-reconfigure-server.l create mode 100644 roseus/test/test-dynamic-reconfigure-server.test diff --git a/roseus/CMakeLists.txt b/roseus/CMakeLists.txt index e732689e3..f880db98b 100644 --- a/roseus/CMakeLists.txt +++ b/roseus/CMakeLists.txt @@ -206,4 +206,5 @@ 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) endif() 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 @@ + + + From 42e0c20bf326cb903d9077adb51466102d0b9a42 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 14 Nov 2022 17:47:31 +0900 Subject: [PATCH 176/176] add dynamic reconfigure client test --- roseus/CMakeLists.txt | 1 + .../test/simple-dynamic-reconfigure-server.l | 50 ++++++++ roseus/test/test-dynamic-reconfigure-client.l | 121 ++++++++++++++++++ .../test/test-dynamic-reconfigure-client.test | 4 + 4 files changed, 176 insertions(+) create mode 100644 roseus/test/simple-dynamic-reconfigure-server.l create mode 100644 roseus/test/test-dynamic-reconfigure-client.l create mode 100644 roseus/test/test-dynamic-reconfigure-client.test diff --git a/roseus/CMakeLists.txt b/roseus/CMakeLists.txt index f880db98b..4812d008a 100644 --- a/roseus/CMakeLists.txt +++ b/roseus/CMakeLists.txt @@ -207,4 +207,5 @@ if(CATKIN_ENABLE_TESTING) 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/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 @@ + + + +