From 0b0c563668e69624e27bc2100ff80b536b9a4979 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 28 May 2021 13:34:17 +0900 Subject: [PATCH 001/195] 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 017869c23..a30df5de4 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -871,10 +871,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 ffb810e45dafa59e7846561e65541326f8d2bce0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 26 Oct 2022 23:14:21 +0900 Subject: [PATCH 002/195] Fix possible memory faults when registering service servers and timer functions --- roseus/roseus.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index a30df5de4..b71c44337 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -1239,13 +1239,16 @@ pointer ROSEUS_ADVERTISE_SERVICE(register context *ctx,int n,pointer *argv) EuslispMessage message(emessage); vpush(message._message); // to avoid GC in csend + vpush(args); pointer request(csend(ctx,emessage,K_ROSEUS_GET,1,K_ROSEUS_REQUEST)); pointer response(csend(ctx,emessage,K_ROSEUS_GET,1,K_ROSEUS_RESPONSE)); - vpop(); // pop message._message boost::shared_ptr *callback = (new boost::shared_ptr (new EuslispServiceCallbackHelper(fncallback, args, message.__getMD5Sum(), message.__getDataType(), request, response))); + vpop(); // pop args + vpop(); // pop message._message + AdvertiseServiceOptions aso; aso.service.assign(service); aso.datatype = (*callback->get()).getDataType(); @@ -1976,8 +1979,10 @@ pointer ROSEUS_CREATE_TIMER(register context *ctx,int n,pointer *argv) for (int i=n-1;i>=2;i--) args=cons(ctx,argv[i],args); // avoid gc + vpush(args); 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)); + vpop(); // ;; store mapTimered ROS_DEBUG("create timer %s at %f (oneshot=%d) (groupname=%s)", fncallname.c_str(), period, oneshot, groupname.c_str()); From d31accff9ea0c1a3f698ea712103e84324ad31e0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 26 Oct 2022 21:24:35 +0900 Subject: [PATCH 003/195] Ensure stack safety in StoreConnectionHeader --- roseus/roseus.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index b71c44337..d2113c450 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -310,6 +310,7 @@ void StoreConnectionHeader(EuslispMessage *eus_msg) { return; } context *ctx = current_ctx; + vpush(eus_msg->_message); // store conection headers register pointer ret, header; ret = cons(ctx, NIL, NIL); @@ -323,7 +324,8 @@ void StoreConnectionHeader(EuslispMessage *eus_msg) { /* (setslot obj class index newval) */ pointer slot_args[4] = {eus_msg->_message, classof(eus_msg->_message), K_ROSEUS_CONNECTION_HEADER, ccdr(header)}; SETSLOT(ctx, 4, slot_args); - vpop(); + vpop(); // ret + vpop(); // eus_msg->_message } namespace ros{ From 1bbc21aafea71e7708db126b0c44b57ec4617371 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 15:40:25 +0900 Subject: [PATCH 004/195] 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 51bc26e571e177dfdc925a08baeca6b2421ef814 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 15:55:32 +0900 Subject: [PATCH 005/195] (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 a95c10dbf1e8dbdd654d98b7827de830243488c7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 15:56:42 +0900 Subject: [PATCH 006/195] (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 96fab9f347ccfbdf8d427bf9f7cf1a9e84b6cee0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 15:57:40 +0900 Subject: [PATCH 007/195] (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 bafc6338c0cdf72850ae86fc06511b45cc031c81 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 18:02:46 +0900 Subject: [PATCH 008/195] (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 f9dd76db4188f0bc8836f543f96c5e6e9d4b135a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 19:13:31 +0900 Subject: [PATCH 009/195] (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 abff74f6575cca8f6e6c40ba5195b55195d65d52 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 20:55:11 +0900 Subject: [PATCH 010/195] (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 56670ad574321891dd6527675938e0c3943b8698 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 20:57:34 +0900 Subject: [PATCH 011/195] (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 e0a7e9a7879ab473f14861c1ab671f2267da739f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 21:04:55 +0900 Subject: [PATCH 012/195] (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 2f9088f752964068b913e2422f2cf74cdab2d22d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 21:14:51 +0900 Subject: [PATCH 013/195] (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 d9669a527776476cc59317e45a470919facfd300 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 11:11:07 +0900 Subject: [PATCH 014/195] (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 67b10ed2857938216887b702a0f4a3f4ec787c83 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 12:56:52 +0900 Subject: [PATCH 015/195] (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 417085f249b8352f940ec7581a2ef8eb7bdbdab2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 16:44:03 +0900 Subject: [PATCH 016/195] (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 9065274b63798d25c201da0d182bfa3330703fe2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 17:02:44 +0900 Subject: [PATCH 017/195] (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 eb21bf9b86a65bbdf8dd2354b72b2717ab97a7ba Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 17:48:30 +0900 Subject: [PATCH 018/195] (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 3f3a63c62e1a18a179ea28846fe8d1a6c1b56ff9 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 17:49:17 +0900 Subject: [PATCH 019/195] (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 1c41cd37af2883f0adfff5d2c428146bf2051141 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 20:09:16 +0900 Subject: [PATCH 020/195] (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 4621ff790f4d5460bfdba55d30a7311da3342296 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 18 Jun 2021 13:20:23 +0900 Subject: [PATCH 021/195] (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 10e955846c7559ea49953f7ccb6dabb072de7098 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 18 Jun 2021 13:44:37 +0900 Subject: [PATCH 022/195] (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 71830a69e00480790f7c005c63a56361975ef14b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 18 Jun 2021 15:56:38 +0900 Subject: [PATCH 023/195] (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 13fb4819d6aea3f3a18a85ae0716f399c361efe4 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 11:11:26 +0900 Subject: [PATCH 024/195] (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 bea85017e2c7a608841a4ec8b2f3823791db8c52 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 11:12:37 +0900 Subject: [PATCH 025/195] (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 65a3ad552b4d91d3c402df5069ef5be104166fc4 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 13:16:53 +0900 Subject: [PATCH 026/195] (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 ff899abb86b6a97c78a1b3e7f09daddafb72a261 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 14:37:40 +0900 Subject: [PATCH 027/195] (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 5c5f316be49ff827e2a2fde702ca6c2c8315adfe Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 14:44:08 +0900 Subject: [PATCH 028/195] (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 f3604b5031aef7d55206b9d04a884bfe84b97685 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 14:47:52 +0900 Subject: [PATCH 029/195] (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 d9340f1fb55627abbbb76ccef250676a483c07e2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 17:28:15 +0900 Subject: [PATCH 030/195] (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 6dc320e4f1bebfc319938fa06df0463d48d4d6fd Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 17:47:39 +0900 Subject: [PATCH 031/195] (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 e3a07d20711f872ac512c86e28cfcd49f777d8b0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 19:05:11 +0900 Subject: [PATCH 032/195] (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 c34455f822a7fb826b0b710207191fd8ff6c628b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 19:11:56 +0900 Subject: [PATCH 033/195] (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 863ab21d630f97c2f3b0bbac5eed550c7f3a1ed2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Jun 2021 13:49:24 +0900 Subject: [PATCH 034/195] (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 0025a1c34b98210302dc2bf6d42802f268cef9f9 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Jun 2021 13:50:22 +0900 Subject: [PATCH 035/195] (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 98f7fe6a4de12795c62495ae5679716f5df3beae Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 14:17:03 +0900 Subject: [PATCH 036/195] (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 a492167956038fcb186bbc7908e978a76bd11129 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 14:29:27 +0900 Subject: [PATCH 037/195] (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 03f0b83a6e9f304fe01fb6a4bd9c6c6a67fff3e4 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 14:31:25 +0900 Subject: [PATCH 038/195] (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 80e2c3957a6e76645553196fa8e846f1726aa8f7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 14:46:47 +0900 Subject: [PATCH 039/195] (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 b9fa96b11384ca04fce1d3a96cf9f33ada65b581 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 15:43:39 +0900 Subject: [PATCH 040/195] (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 618091930be48ad36545da8f361b2ce590208ca4 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 15:50:07 +0900 Subject: [PATCH 041/195] (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 b3241e84b97d20092bdde6c9a1c95113a7abb57b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 6 Jul 2021 16:29:41 +0900 Subject: [PATCH 042/195] (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 948496b2b526c40429f798dc390618bd1ea3aa49 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 6 Jul 2021 20:03:21 +0900 Subject: [PATCH 043/195] (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 671a86c0acb66fcbe012c623618e4b28f5dbe118 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 6 Jul 2021 20:04:19 +0900 Subject: [PATCH 044/195] (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 ce8baea39dbefbb935acfd580aff9807f5bbda4d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 19:44:04 +0900 Subject: [PATCH 045/195] (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 abf8a812a6afb39059d55ea2e5fef1736db17605 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 19:44:27 +0900 Subject: [PATCH 046/195] (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 1602ac304e620d2e3db6687a1342c22d87c766d8 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 20:58:17 +0900 Subject: [PATCH 047/195] (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 3768b9a895f9b9f440b8c07b9fc09ed4d282623f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:01:02 +0900 Subject: [PATCH 048/195] (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 5f1d43483d9d93d5c2b67e63e240e08858edf930 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:15:53 +0900 Subject: [PATCH 049/195] (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 91369b54ff8d017d1eff28eb08cb2544d1807a4f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:17:47 +0900 Subject: [PATCH 050/195] (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 171b619fc4e37a2dae3155e03501c7eff323a030 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:32:52 +0900 Subject: [PATCH 051/195] (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 be4f6c5f19ed5dad3dd44da1a0cf045a0f4fbe0f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:33:34 +0900 Subject: [PATCH 052/195] (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 85afe523526108b6d4402b4f9ca00865047f0191 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:45:37 +0900 Subject: [PATCH 053/195] (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 bb990b28e4afba020f83e6e81be9ea8423b4ae7d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 13:55:23 +0900 Subject: [PATCH 054/195] (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 7a870f135844bf5b0f13b9624f988010d02f500d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 14:03:23 +0900 Subject: [PATCH 055/195] (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 1bda54c57a2031e7cee4b5e7eb861750473c4099 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 14:03:36 +0900 Subject: [PATCH 056/195] (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 a3ee38e26528a9aa838883008e4b0b287de1544d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 14:04:00 +0900 Subject: [PATCH 057/195] (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 3370cf05c19d7b2abf790d1f7568abfdbb636101 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 19:17:22 +0900 Subject: [PATCH 058/195] (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 c466877b5b6da119a9411a3d39fd0c94e5b72cb1 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 19:20:57 +0900 Subject: [PATCH 059/195] (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 ad8302050cc4a1a2ced1cbf75ab59ad6d5780ab1 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 20:41:08 +0900 Subject: [PATCH 060/195] (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 9b42bbac3fe9fd90628556e7810f05d6fa27535f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 22:01:38 +0900 Subject: [PATCH 061/195] (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 1ebbf81d6c7e469d5172b0303c25826ba50c8ce5 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 9 Jul 2021 16:10:04 +0900 Subject: [PATCH 062/195] (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 5c76756a330a54df0ca0b296ce82de010b24ad9f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 9 Jul 2021 18:10:26 +0900 Subject: [PATCH 063/195] (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 b2b82ef2e1d6e2245a304a0847eb55afd39c1816 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 9 Jul 2021 23:40:15 +0900 Subject: [PATCH 064/195] (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 ef7b03c5bd818d45a4cd0876a5e694a869324593 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 01:51:46 +0900 Subject: [PATCH 065/195] (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 c67d29a25680ef38e79c637f1791444c97bebf9b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 10:42:13 +0900 Subject: [PATCH 066/195] (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 8b6d0b8cbcc22a51fc101b5969eed9676c817e68 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 11:22:08 +0900 Subject: [PATCH 067/195] (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 312234c865e625329f59f175c91160444a3e1655 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 11:26:34 +0900 Subject: [PATCH 068/195] (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 96bdff8b8f2b0af2d117a746573419cd0c6b897e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 11:56:54 +0900 Subject: [PATCH 069/195] (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 9f6391878a8f8e1c9c9bdc282b17de67ed0e4544 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 12 Jul 2021 11:52:12 +0900 Subject: [PATCH 070/195] (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 503e5da346c04aa363951b542694a98cb254129d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 12 Jul 2021 13:40:21 +0900 Subject: [PATCH 071/195] (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 dab791fd799b43db678b1c113e40773643da7be1 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 12 Jul 2021 13:50:03 +0900 Subject: [PATCH 072/195] (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 964227ec55ca0d97ccbeb207017afe65762f090a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 13 Aug 2021 15:51:00 +0900 Subject: [PATCH 073/195] (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 0553584532680fe6ab92ce9cc6d08bb351d5f403 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 13 Aug 2021 17:03:10 +0900 Subject: [PATCH 074/195] (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 78b66da9eefe86a43fc89b6744825aacfaaccbdb Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 16 Aug 2021 13:48:49 +0900 Subject: [PATCH 075/195] (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 b8e66aac19f2c88f36686c896cd9c0dfacd1cfbd Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 16 Aug 2021 15:13:34 +0900 Subject: [PATCH 076/195] (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 b7abedc95f2193e283e3c0f328f0d84476685c03 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 16 Aug 2021 16:20:51 +0900 Subject: [PATCH 077/195] (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 304a7c304b53c7f61526ecbe535253da484e9f2d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 24 Aug 2021 14:08:12 +0900 Subject: [PATCH 078/195] (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 a8b5ed41a1de98611ab15d9c1fc432e109ea52f0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 6 Mar 2019 18:30:49 +0900 Subject: [PATCH 079/195] 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 99fc13e3a..b6d9a2305 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 1dc4a2939577b7d385e58d6cc6f3ea73e85e0a59 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 14 Apr 2021 14:10:11 +0900 Subject: [PATCH 080/195] 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 10c3a9ecdf3a100df2401c0652e22493432527d6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 14 Apr 2021 14:18:16 +0900 Subject: [PATCH 081/195] 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 1a14594404486a56fd41ecf8eb5666b5205cecb4 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 14 Apr 2021 14:31:36 +0900 Subject: [PATCH 082/195] 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 d2113c450..8e9f6687b 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -1754,11 +1754,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: @@ -1780,7 +1777,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(); @@ -2127,7 +2126,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 6b4d1336bfa11ef61847affcd5699b1af8d2b77c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 14 Apr 2021 15:00:13 +0900 Subject: [PATCH 083/195] 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 8e9f6687b..6348075a6 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -690,7 +690,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); } @@ -724,6 +724,7 @@ pointer ROSEUS_SPINONCE(register context *ctx,int n,pointer *argv) ckarg2(0, 1); // ;; arguments ;; // [ groupname ] + CallbackQueue* queue; if ( n > 0 ) { string groupname; @@ -732,18 +733,20 @@ pointer ROSEUS_SPINONCE(register context *ctx,int n,pointer *argv) map >::iterator it = s_mapHandle.find(groupname); if( it == s_mapHandle.end() ) { - ROS_ERROR("Groupname %s is missing", groupname.c_str()); - return (T); + ROS_ERROR("Groupname \"%s\" is missing", groupname.c_str()); + error(E_USER, "groupname not found"); } boost::shared_ptr hdl = (it->second); - // spin this nodehandle - ((CallbackQueue *)hdl->getCallbackQueue())->callAvailable(); - - return (NIL); + queue = (CallbackQueue *)hdl->getCallbackQueue(); } else { - ros::spinOnce(); - return (NIL); + queue = ros::getGlobalCallbackQueue(); } + if (queue->isEmpty()) { + return (NIL);} + else { + // execute callbacks + queue->callAvailable();} + return (T); } pointer ROSEUS_TIME_NOW(register context *ctx,int n,pointer *argv) @@ -858,7 +861,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 faf2d583b7591e79afdb7753bbc4d50e3ee0c288 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 14 Apr 2021 15:37:17 +0900 Subject: [PATCH 084/195] Allow interruptions on ROS sleep functions --- roseus/roseus.cpp | 42 ++++++++++++++++++++++++++++++++++++++---- 1 file changed, 38 insertions(+), 4 deletions(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index 6348075a6..f48618776 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -566,6 +566,7 @@ void roseusSignalHandler(int sig) // memoize for euslisp handler... context *ctx=euscontexts[thr_self()]; ctx->intsig = sig; + ros::Time::shutdown(); } /************************************************************ @@ -711,7 +712,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(); } @@ -786,7 +788,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); } @@ -1088,6 +1102,7 @@ pointer ROSEUS_GETTOPICPUBLISHER(register context *ctx,int n,pointer *argv) pointer ROSEUS_WAIT_FOR_SERVICE(register context *ctx,int n,pointer *argv) { isInstalledCheck; + ros::Time start_time = ros::Time::now(); string service; numunion nu; @@ -1100,9 +1115,28 @@ pointer ROSEUS_WAIT_FOR_SERVICE(register context *ctx,int n,pointer *argv) if( n > 1 && argv[1] != NIL) timeout = ckfltval(argv[1]); - bool bSuccess = service::waitForService(service, ros::Duration(timeout)); + // Overwrite definition on service.cpp L87 to check for signal interruptions + // http://docs.ros.org/electric/api/roscpp/html/service_8cpp_source.html + bool printed = false; + bool result = false; + ros::Duration timeout_duration = ros::Duration(timeout); + while (ctx->intsig==0 && ros::ok()) { + if (service::exists(service, !printed)) { + result = true; + break;} + else { + printed = true; + if (timeout >= 0) { + ros::Time current_time = ros::Time::now(); + if ((current_time - start_time) >= timeout_duration) + return(NIL);} + ros::Duration(0.02).sleep();} + } - return (bSuccess?T:NIL); + if (result && printed && ros::ok()) + ROS_INFO("waitForService: Service [%s] is now available.", service.c_str()); + + return (result?T:NIL); } pointer ROSEUS_SERVICE_EXISTS(register context *ctx,int n,pointer *argv) From 0ff260d00728eb33457449838c9f0ea61f30702e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 16 Aug 2021 16:14:54 +0900 Subject: [PATCH 085/195] 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 f48618776..ae7d51ea1 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -839,12 +839,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 dd9de9e9ac9277e8814451ee19bf9452dba67df1 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 12 Sep 2022 17:10:39 +0900 Subject: [PATCH 086/195] (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 f792479d1c79dab20cd1780b350d895f48599be0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 21 Sep 2021 21:59:42 +0900 Subject: [PATCH 087/195] (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 2d6cdd4749271d7257f77b134e0968bf7832cf33 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 22 Sep 2021 16:43:39 +0900 Subject: [PATCH 088/195] (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 351058b89c2492873834a316227fc67a5bd40026 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 22 Sep 2021 17:02:37 +0900 Subject: [PATCH 089/195] (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 96d170dbbe5252a59f976be9980a56ce386d1bbe Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 8 Oct 2021 16:32:34 +0900 Subject: [PATCH 090/195] (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 83353cc5a8cb3ac15b6f2d4eccdff1bc904bd07d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 8 Oct 2021 16:40:28 +0900 Subject: [PATCH 091/195] (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 dd1649f8a0d74950330a9e88599e5ab37d03d0cb Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 8 Oct 2021 16:59:34 +0900 Subject: [PATCH 092/195] (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 b6cd7740b1f86ac15f794f1b81040d4dacec204a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 8 Oct 2021 17:04:38 +0900 Subject: [PATCH 093/195] (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 58db1ee0743f25238ee6dfc353b49fc45a165ded Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 22 Sep 2021 15:30:09 +0900 Subject: [PATCH 094/195] (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 08f7da89be73e46bb4f3ae14b8425351110ff15d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 24 Feb 2022 14:20:46 +0900 Subject: [PATCH 095/195] (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 46ccea6b66787373f447fad0b75d9679e4cee059 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 13:29:09 +0900 Subject: [PATCH 096/195] (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 ea3ec60241b5111599f3207cf7b43785a353c95b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 17:35:41 +0900 Subject: [PATCH 097/195] (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 c4fb029b7ed4c4e66356d513e4fc7fbcbcd2ea01 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 17:39:40 +0900 Subject: [PATCH 098/195] (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 3fff3875e07fda255778f6ed264170fc3d5ccaec Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 17:41:36 +0900 Subject: [PATCH 099/195] (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 c8979f3f61c3fd178928d81d5666a41fb7fe5ddc Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 20:24:57 +0900 Subject: [PATCH 100/195] (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 5fb4262942ec27ba3963702cc569efc4c4bf302f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 23:36:21 +0900 Subject: [PATCH 101/195] (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 28f0fc3d4c27a8769be89c9a4cd7e7a002d4233b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 23:38:56 +0900 Subject: [PATCH 102/195] (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 86542969fca145a18ec336363a6a86a6612160d6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 19 Apr 2022 07:37:18 +0900 Subject: [PATCH 103/195] (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 99161ad2abd228d3440f6ce3d7c5fd9d8390b226 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 19 Apr 2022 08:28:22 +0900 Subject: [PATCH 104/195] (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 76f9f46f8cefc6a4148947829aa3cdec4840f7ba Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 21 Apr 2022 17:39:43 +0900 Subject: [PATCH 105/195] (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 13d19513166e065b573224bc1dbcde0e772ebb40 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 21 Apr 2022 21:02:39 +0900 Subject: [PATCH 106/195] (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 072127bc1436f65c189fc781dddbd041dcf57fb5 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 21 Apr 2022 21:35:14 +0900 Subject: [PATCH 107/195] (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 b6a31368968d70d7978e186ff7f7152849ac38e4 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 13:50:24 +0900 Subject: [PATCH 108/195] (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 3a31050f9f6ed7f52bfaf83d402dbc1d747b32ac Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 13:53:23 +0900 Subject: [PATCH 109/195] (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 1aedf61f3342a81c3e0be1f372d251fb450d7a17 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 14:59:14 +0900 Subject: [PATCH 110/195] (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 fed80fe71a8a229428b2829810d410bbece1fdec Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 17:24:49 +0900 Subject: [PATCH 111/195] (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 0bcd89ec7e6de0ed96e4319c4becb071cabce0cf Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 17:25:18 +0900 Subject: [PATCH 112/195] (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 651a3638d33a2306f86cd792101fc936f730af14 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 17:39:46 +0900 Subject: [PATCH 113/195] (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 24df8b73ee2a862cbc6f5ef9f1686a8fb696e720 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 18:47:59 +0900 Subject: [PATCH 114/195] (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 0cef7d5e7ad53b7f70e3647f4ceb1d5a1822386c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 19:21:57 +0900 Subject: [PATCH 115/195] (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 ec9608e3b7f241b7da0a767c155394e7c1cbeb7b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 19:30:18 +0900 Subject: [PATCH 116/195] (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 5607ca8e455cac089d665ae02dfd0224c402a8fc Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 20:04:58 +0900 Subject: [PATCH 117/195] (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 8b7c18dfb0798bfca9001150ff689e97e117d938 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 21:31:22 +0900 Subject: [PATCH 118/195] (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 80b7f1049abafd0f8f8b9ac1fdd0e780b99a6de3 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 21:48:21 +0900 Subject: [PATCH 119/195] (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 e7ff1bad842b68aed29ce502c783882ad00084bd Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 22:26:05 +0900 Subject: [PATCH 120/195] (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 7575c020bf46824cc3077ed587f2e1846eb38314 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 11:56:21 +0900 Subject: [PATCH 121/195] (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 1aab7cd4c2e0aade26205bf6cad41c93b1c97461 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 12:07:03 +0900 Subject: [PATCH 122/195] (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 52ea9e90d4a39678fb042baa8db8a620d63ed95a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 12:18:02 +0900 Subject: [PATCH 123/195] (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 130ef883fce976c8102a99174f88ee0a921299d2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 13:36:36 +0900 Subject: [PATCH 124/195] (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 82674329e3c62933a9fa31d7da1295e9da0ab8dc Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 14:06:41 +0900 Subject: [PATCH 125/195] (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 2aaceeabedcb0ce80ea7316d8d12ed2d5accf1c2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 27 Apr 2022 20:39:44 +0900 Subject: [PATCH 126/195] (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 c5fa11ab7505a50928de3eb9fa11e9abcbf1e512 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 28 Apr 2022 11:18:17 +0900 Subject: [PATCH 127/195] (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 794abfc75314ef9e97a0c0d1a2ec3b8e5c87e236 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 28 Apr 2022 11:28:32 +0900 Subject: [PATCH 128/195] (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 399dd0d63243711f34870549924c72bd82b51cbc Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 28 Apr 2022 15:42:54 +0900 Subject: [PATCH 129/195] (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 1e32632d04e703d2964ce73421ad485ef2e061c2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 28 Apr 2022 23:08:45 +0900 Subject: [PATCH 130/195] (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 1db0bd05283cb2d883bb9e42d34e39e0e67f73be Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 29 Apr 2022 00:10:36 +0900 Subject: [PATCH 131/195] (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 449fd34ab2284fe3f86a039ba3176682cdb1292e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 20:26:18 +0900 Subject: [PATCH 132/195] (roseus_bt) Default action-node's preemption callback to 'signals-cancel' --- 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 4968ad560..042228c12 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 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)) @@ -44,7 +44,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 e00a5c32cede87ab87575fed62d55d14d8876ae7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 21:14:16 +0900 Subject: [PATCH 133/195] (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 0cb7edd0ee0df29f26906d495d8fe4c73fc82912 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 28 May 2022 00:13:54 +0900 Subject: [PATCH 134/195] (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 8af543714..4bbebe170 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -178,8 +178,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 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. +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 5cfa4c62f1f5788896b9948048d1026c160d7c74 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 21:30:12 +0900 Subject: [PATCH 135/195] (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 042228c12..8a3a69070 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -64,7 +64,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 19e480ed097aeade543d09e6bd46a48c1ef5d8e3 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 21:38:30 +0900 Subject: [PATCH 136/195] (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 4fb3a9b54e46e8941658218680f0f768c270b478 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 23:17:39 +0900 Subject: [PATCH 137/195] (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 571d273d9477244f17a906c0aba7217c5e06db06 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 23:21:11 +0900 Subject: [PATCH 138/195] (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 c95d128c6130fd282ac64e715ed38b660614b72b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 23:31:50 +0900 Subject: [PATCH 139/195] (roseus_bt) Update the subscriber documentation --- roseus_bt/sample/README.md | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index 4bbebe170..c379bcd8a 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 From 1af595bf2fd339842edfb26afa068bee51417564 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 28 May 2022 00:45:20 +0900 Subject: [PATCH 140/195] (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 45ba1dbebb6351dc85a78dc06989ed27b844f00c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 7 Jun 2022 20:30:24 +0900 Subject: [PATCH 141/195] (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 318e528b8..169f6a4da 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 7ef087695587ca478e090cbe1efae34b2b780bc2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 11 Aug 2022 20:31:06 +0900 Subject: [PATCH 142/195] 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 ae7d51ea1..409bc263a 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -1791,8 +1791,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: @@ -1816,6 +1819,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 b6267cd4930fecb89d3eacb5b963d28e15e6a365 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Sep 2022 00:08:00 +0900 Subject: [PATCH 143/195] 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 409bc263a..60563da7b 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -645,12 +645,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 e329a6ae254cd245aaaa4daf99dabcd6ad6751e8 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 26 Sep 2022 18:21:34 +0900 Subject: [PATCH 144/195] (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 efef44d76631c09dd7068f957f094c5f56d8a6ec Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 26 Sep 2022 18:22:18 +0900 Subject: [PATCH 145/195] (roseus_bt) Remove check-fn-closure --- roseus_bt/euslisp/nodes.l | 35 +++++------------------------------ 1 file changed, 5 insertions(+), 30 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 8a3a69070..2d0ba587d 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -12,43 +12,19 @@ (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)))) +(defcondition cancel-action :slots (server goal)) -(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)))))) +(defun signals-cancel (server goal) + (signals cancel-action :server server :goal goal)) +(defun check-goal (server new-goal old-goal) + (equal (send new-goal :goal) (send old-goal :goal))) ;; classes (defclass action-node :super ros::simple-action-server :slots (monitor-groupname)) (defmethod action-node (:init (ns spec &key execute-cb (preempt-cb 'signals-cancel) accept-cb groupname ((:monitor-groupname monitor-gn))) - (check-fn-closure execute-cb) - (check-fn-closure preempt-cb) - (check-fn-closure accept-cb) (send-super :init ns spec :execute-cb execute-cb @@ -102,7 +78,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 5d9ca9448f9a10b29a9fe684ae2aa5574ca4ca30 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 10 Oct 2022 12:58:23 +0900 Subject: [PATCH 146/195] (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 af980935c75e4f6d6afde8b6efd49aba0d98356f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 10 Oct 2022 23:36:44 +0900 Subject: [PATCH 147/195] (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 7037b16a38e81f62f8865f261562fd6de211efaf Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 20 Oct 2022 16:25:36 +0900 Subject: [PATCH 148/195] (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 57982d57eec3018642adc6a2b3e7667268bd0612 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 20 Oct 2022 18:19:33 +0900 Subject: [PATCH 149/195] (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 3c1332c7798d57e55553d4beaab99057ae599803 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 10:45:56 +0900 Subject: [PATCH 150/195] (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 49efe4f2bc3c2d7c0d55bcf4ff006ad991a5a8c8 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 13:15:08 +0900 Subject: [PATCH 151/195] (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 f4e6cc244e0009ef32b9f19e7727be42d5dfa7dc Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 19:48:17 +0900 Subject: [PATCH 152/195] (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 58c26a0e192f8822ebb51486a2655e73c377ae8d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 21:31:50 +0900 Subject: [PATCH 153/195] (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 e48db95b94577fc666aacdc07ebce6a892a33c0b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 22:31:22 +0900 Subject: [PATCH 154/195] (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 0b2787a1de8493e338c26fa07751d6885903e5ea Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 22:32:32 +0900 Subject: [PATCH 155/195] (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 b076c6fa753aac207bdd2546b197efe3d756d601 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 22 Oct 2022 16:21:33 +0900 Subject: [PATCH 156/195] (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 163ba7df05df9abeb4e02755ee3e893a970f7ea4 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 10:02:57 +0900 Subject: [PATCH 157/195] (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 e14418505de32b18efd65256b228168dbfbf1d16 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 12:27:09 +0900 Subject: [PATCH 158/195] (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 5ad1ea6cd721e229f2def0586b08d04b7888b048 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 15:26:45 +0900 Subject: [PATCH 159/195] (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 bd8e49b5995278f64e851a1f4de1617326055201 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 17:13:12 +0900 Subject: [PATCH 160/195] (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 245d10d2c4f79dc2cc4262d219544573e0fc2666 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 20:06:55 +0900 Subject: [PATCH 161/195] (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 e717c2248604bdef2d4bc42e374dbe25018b5216 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 22:24:08 +0900 Subject: [PATCH 162/195] (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 eb31cd904e878572c5114a0c16b5aafd3d3e0adc Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 25 Oct 2022 19:32:57 +0900 Subject: [PATCH 163/195] (roseus_bt) Remove comma separation in package.xml generator --- roseus_bt/include/roseus_bt/pkg_template.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/roseus_bt/include/roseus_bt/pkg_template.h b/roseus_bt/include/roseus_bt/pkg_template.h index 909f9a620..532a38c77 100644 --- a/roseus_bt/include/roseus_bt/pkg_template.h +++ b/roseus_bt/include/roseus_bt/pkg_template.h @@ -160,8 +160,8 @@ std::string PkgTemplate::package_xml_template(std::string package_name, package_name % author_name % author_email % - boost::algorithm::join(build_dependencies, ",\n") % - boost::algorithm::join(exec_dependencies, ",\n"); + boost::algorithm::join(build_dependencies, "\n") % + boost::algorithm::join(exec_dependencies, "\n"); return bfmt.str(); } From b5746a5fbd4b24ef8392781f37450fad0b3724d4 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 25 Oct 2022 23:08:38 +0900 Subject: [PATCH 164/195] (roseus_bt) Enforce '/' on remote topics --- roseus_bt/include/roseus_bt/ws_action_client.h | 3 +++ roseus_bt/include/roseus_bt/ws_service_client.h | 6 +++++- roseus_bt/include/roseus_bt/ws_subscriber_client.h | 3 +++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/ws_action_client.h b/roseus_bt/include/roseus_bt/ws_action_client.h index 6797f1ea3..5bfe1936f 100644 --- a/roseus_bt/include/roseus_bt/ws_action_client.h +++ b/roseus_bt/include/roseus_bt/ws_action_client.h @@ -16,6 +16,9 @@ class RosbridgeActionClient server_name_(server_name), is_active_(false) { + if (server_name_.front() != '/') { + 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_); diff --git a/roseus_bt/include/roseus_bt/ws_service_client.h b/roseus_bt/include/roseus_bt/ws_service_client.h index dbe3e8734..88f1a355a 100644 --- a/roseus_bt/include/roseus_bt/ws_service_client.h +++ b/roseus_bt/include/roseus_bt/ws_service_client.h @@ -15,7 +15,11 @@ class RosbridgeServiceClient rbc_(fmt::format("{}:{}", master, std::to_string(port))), service_name_(service_name), is_active_(false) - {} + { + if (service_name_.front() != '/') { + service_name_ = '/' + service_name_; + } + } ~RosbridgeServiceClient() {} diff --git a/roseus_bt/include/roseus_bt/ws_subscriber_client.h b/roseus_bt/include/roseus_bt/ws_subscriber_client.h index 324cbe2ee..e85693204 100644 --- a/roseus_bt/include/roseus_bt/ws_subscriber_client.h +++ b/roseus_bt/include/roseus_bt/ws_subscriber_client.h @@ -18,6 +18,9 @@ class RosbridgeSubscriberClient topic_name_(topic_name), topic_type_(topic_type) { + if (topic_name_.front() != '/') { + topic_name_ = '/' + topic_name_; + } rbc_.addClient("topic_subscriber"); } From c75ce1a7f03c34137835edb0e8eda8e090ee6ba6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 26 Oct 2022 02:04:08 +0900 Subject: [PATCH 165/195] (roseus_bt) Raise runtime errors when blackboard entry is not found --- roseus_bt/include/roseus_bt/gen_template.h | 22 ++++++++++++++++++++-- roseus_bt/include/roseus_bt/xml_parser.h | 18 ++++++++++++------ 2 files changed, 32 insertions(+), 8 deletions(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 931fdf5c9..04edd9725 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -132,6 +132,14 @@ std::string GenTemplate::action_class_template(std::string package_name, std::st std::vector provided_ports, std::vector get_inputs, std::vector set_outputs) { + auto format_send_goal = [](const std::string body) { + std::string decl = 1 + R"( + BT::Result res; +)"; + if (!body.empty()) return fmt::format("{}{}", decl, body); + return body; + }; + std::string fmt_string = 1 + R"( class %2%: public EusActionNode<%1%::%2%Action> { @@ -176,7 +184,7 @@ EusActionNode<%1%::%2%Action>(handle, 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(); @@ -193,6 +201,7 @@ std::string GenTemplate::remote_action_class_template( std::string decl = 1 + R"( rapidjson::CopyDocument document; GoalType ros_msg; + BT::Result res; )"; if (!body.empty()) return fmt::format("{}{}", decl, body); return body; @@ -251,6 +260,14 @@ EusRemoteActionNode("%1%/%2%Action", name, conf) {} std::string GenTemplate::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"( + BT::Result res; +)"; + if (!body.empty()) return fmt::format("{}{}", decl, body); + return body; + }; + std::string fmt_string = 1 + R"( class %2%: public EusConditionNode<%1%::%2%> { @@ -289,7 +306,7 @@ class %2%: public EusConditionNode<%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(); } @@ -303,6 +320,7 @@ std::string GenTemplate::remote_condition_class_template(std::string package_nam std::string json; rapidjson::Document document; RequestType ros_msg; + BT::Result res; )"; if (!body.empty()) return fmt::format("{}{}", decl, body); return body; diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index c2edf456a..be0bcc9ac 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -632,13 +632,15 @@ 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) return fmt::format(R"( - getInput("{0}", document); + res = getInput("{0}", document); + if (!res) throw BT::RuntimeError(res.error()); 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}); + res = getInput("{0}", ros_msg.{0}); + if (!res) throw BT::RuntimeError(res.error()); rapidjson::Value {0}; {0}.{2}; {1}->AddMember("{0}", {0}, {1}->GetAllocator());)", @@ -818,8 +820,10 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const std:: node->Attribute("name")); }; auto format_get_input = [](const XMLElement* node) { - return fmt::format(" getInput(\"{0}\", goal.{0});", - node->Attribute("name")); + return fmt::format(1 + R"( + res = getInput("{0}", goal.{0}); + if (!res) throw BT::RuntimeError(res.error());)", + node->Attribute("name")); }; auto format_set_output = [](const XMLElement* node) { return fmt::format( @@ -926,8 +930,10 @@ std::string XMLParser::generate_condition_class(const XMLElement* node, const st node->Attribute("name")); }; auto format_get_input = [](const XMLElement* node) { - return fmt::format(" getInput(\"{0}\", request.{0});", - node->Attribute("name")); + return fmt::format(1 + R"( + res = getInput("{0}", request.{0}); + if (!res) throw BT::RuntimeError(res.error());)", + node->Attribute("name")); }; std::vector provided_ports; From 255da5a7ae0cb8ab09d9637999c6931ffef21b08 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 26 Oct 2022 02:04:32 +0900 Subject: [PATCH 166/195] (roseus_bt) Initialize subscriber ports --- roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h | 5 ++++- roseus_bt/include/roseus_bt/eus_subscriber_node.h | 4 +++- 2 files changed, 7 insertions(+), 2 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 37fe5f5d0..ddc8791f5 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_subscriber_node.h @@ -20,6 +20,9 @@ class EusRemoteSubscriberNode: public BT::ActionNodeBase getInput("topic_name").value(), getInput("message_type").value()) { + setOutput("received_port", false); + setOutput("output_port", + rapidjson::CopyDocument(rapidjson::kObjectType)); auto cb = std::bind(&EusRemoteSubscriberNode::topicCallback, this, std::placeholders::_1, std::placeholders::_2); @@ -59,7 +62,7 @@ class EusRemoteSubscriberNode: public BT::ActionNodeBase protected: virtual void callback(const rapidjson::Value& msg) { setOutputFromMessage("output_port", msg); - setOutput("received_port", (uint8_t)true); + setOutput("received_port", 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 8cb259c18..f5a37cab9 100644 --- a/roseus_bt/include/roseus_bt/eus_subscriber_node.h +++ b/roseus_bt/include/roseus_bt/eus_subscriber_node.h @@ -12,6 +12,8 @@ class EusSubscriberNode: public BT::ActionNodeBase EusSubscriberNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration& conf): BT::ActionNodeBase(name, conf), node_(nh) { + setOutput("received_port", false); + setOutput("output_port", MessageT()); const std::string topic_name = getInput("topic_name").value(); sub_ = node_.subscribe(topic_name, 1000, &EusSubscriberNode::callback, this); } @@ -48,7 +50,7 @@ class EusSubscriberNode: public BT::ActionNodeBase protected: virtual void callback(MessageT msg) { setOutput("output_port", msg); - setOutput("received_port", (uint8_t)true); + setOutput("received_port", true); } }; From eb42df36c77cf79f24cd537eaf1021be3a4c20e4 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 26 Oct 2022 23:22:34 +0900 Subject: [PATCH 167/195] Add store_pointer helper to conceal stack manipulation --- roseus/roseus.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index 60563da7b..a76e6ecc5 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -207,6 +207,16 @@ int getInteger(pointer message, pointer method) { return 0; } +void store_pointer(context *ctx, pointer value) { + // Stores a callback list as a symbol in the lisp package. + // This prevents that the callable gets collected by the gc, + // even if there are no other references to it. + // TODO: cleanup when unsubscribed + vpush(value); + pointer p=gensym(ctx); + defconst(ctx, (char*)(p->c.sym.pname->c.str.chars), vpop(), lisppkg); +} + class EuslispMessage { public: @@ -371,8 +381,7 @@ class EuslispSubscriptionCallbackHelper : public ros::SubscriptionCallbackHelper ROS_ERROR("subscription callback function install error"); } // 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)); + store_pointer(ctx, cons(ctx,scb,args)); } ~EuslispSubscriptionCallbackHelper() { ROS_ERROR("subscription gc"); @@ -454,8 +463,7 @@ class EuslispServiceCallbackHelper : public ros::ServiceCallbackHelper { ROS_ERROR("service callback function install error"); } // 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)); + store_pointer(ctx, cons(ctx,scb,args)); requestDataType = _req.__getDataType(); responseDataType = _res.__getDataType(); @@ -2021,10 +2029,7 @@ pointer ROSEUS_CREATE_TIMER(register context *ctx,int n,pointer *argv) for (int i=n-1;i>=2;i--) args=cons(ctx,argv[i],args); // avoid gc - vpush(args); - 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)); - vpop(); + store_pointer(ctx, cons(ctx,fncallback,args)); // ;; store mapTimered ROS_DEBUG("create timer %s at %f (oneshot=%d) (groupname=%s)", fncallname.c_str(), period, oneshot, groupname.c_str()); From 9c5bfba88af254d1add8d55b426e340f92c1a807 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 26 Oct 2022 23:24:22 +0900 Subject: [PATCH 168/195] Remove redundant stack operations in roseus.cpp --- roseus/roseus.cpp | 33 ++++++--------------------------- 1 file changed, 6 insertions(+), 27 deletions(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index a76e6ecc5..d7fe16c79 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -227,19 +227,17 @@ class EuslispMessage } EuslispMessage(const EuslispMessage &r) { context *ctx = current_ctx; + vpush(r._message); if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self()); if ( isclass(r._message) ) { //ROS_ASSERT(isclass(r._message)); - vpush(r._message); _message = makeobject(r._message); - vpush(_message); csend(ctx,_message,K_ROSEUS_INIT,0); - vpop(); // _message - vpop(); // r._message } else { ROS_WARN("r._message must be class");prinx(ctx,r._message,ERROUT);flushstream(ERROUT);terpri(ERROUT); _message = r._message; } + vpop(); } virtual ~EuslispMessage() { } @@ -277,9 +275,7 @@ class EuslispMessage context *ctx = current_ctx; if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self()); pointer a,curclass; - vpush(_message); // to avoid GC uint32_t len = serializationLength(); - vpop(); // pop _message a = (pointer)findmethod(ctx,K_ROSEUS_SERIALIZE,classof(_message),&curclass); ROS_ASSERT(a!=NIL); pointer r = csend(ctx,_message,K_ROSEUS_SERIALIZE,0); @@ -410,7 +406,6 @@ class EuslispSubscriptionCallbackHelper : public ros::SubscriptionCallbackHelper int argc=0; //ROS_WARN("func");prinx(ctx,_scb,ERROUT);flushstream(ERROUT);terpri(ERROUT); //ROS_WARN("argc");prinx(ctx,argp,ERROUT);flushstream(ERROUT);terpri(ERROUT); - vpush(eus_msg->_message); // to avoid GC if ( ! ( issymbol(_scb) || piscode(_scb) || ccar(_scb)==LAMCLOSURE ) ) { ROS_ERROR("%s : can't find callback function", __PRETTY_FUNCTION__); } @@ -423,7 +418,6 @@ class EuslispSubscriptionCallbackHelper : public ros::SubscriptionCallbackHelper ufuncall(ctx,(ctx->callfp?ctx->callfp->form:NIL),_scb,(pointer)(ctx->vsp-argc),NULL,argc); while(argc-->0)vpop(); - vpop(); // remove eus_msg._message } virtual const std::type_info& getTypeInfo() { return typeid(EuslispMessage); @@ -504,7 +498,6 @@ class EuslispServiceCallbackHelper : public ros::ServiceCallbackHelper { } // Deserialization EuslispMessage eus_msg(_req); - vpush(eus_msg._message); // _res._message, _req._message, eus_msg._message eus_msg.deserialize(params.request.message_start, params.request.num_bytes); // store connection header @@ -517,11 +510,9 @@ class EuslispServiceCallbackHelper : public ros::ServiceCallbackHelper { r = ufuncall(ctx, (ctx->callfp?ctx->callfp->form:NIL), _scb, (pointer)(ctx->vsp-argc), NULL, argc); - while(argc-->0)vpop();// _res._message, _req._message, eus_msg._message - vpush(r); // _res._message, _req._message, eus_msg._message, r, - // Serializaion - EuslispMessage eus_res(_res); - eus_res.replaceContents(r); + while(argc-->0)vpop();// _res._message, _req._message + // Serialization + EuslispMessage eus_res(r); // check return value is valid pointer ret_serialize_method, ret_class; if (ispointer(r)) { @@ -534,7 +525,6 @@ class EuslispServiceCallbackHelper : public ros::ServiceCallbackHelper { vpop(); // _res._message return false; } - vpush(eus_res._message); // _res._message, _req._message, eus_msg._message, r, eus_res._message uint32_t serialized_length = eus_res.serializationLength(); params.response.num_bytes = serialized_length + 5; // add 5 bytes of message header params.response.buf.reset (new uint8_t[params.response.num_bytes]); @@ -560,9 +550,6 @@ class EuslispServiceCallbackHelper : public ros::ServiceCallbackHelper { ROS_INFO("%X", tmp[i]); } #endif - vpop(); // _res._message, _req._message, eus_msg._message, r, eus_res._message - vpop(); // _res._message, _req._message, eus_msg._message, r - vpop(); // _res._message, _req._message, eus_msg._message vpop(); // _res._message, _req._message, vpop(); // _res._message return true; @@ -766,10 +753,8 @@ pointer ROSEUS_TIME_NOW(register context *ctx,int n,pointer *argv) ros::Time t = ros::Time::now(); timevec=makevector(C_INTVECTOR,2); - vpush(timevec); timevec->c.ivec.iv[0]=t.sec; timevec->c.ivec.iv[1]=t.nsec; - vpop(); return (timevec); } @@ -898,12 +883,10 @@ 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); @@ -1284,9 +1267,8 @@ pointer ROSEUS_ADVERTISE_SERVICE(register context *ctx,int n,pointer *argv) return (NIL); } - EuslispMessage message(emessage); - vpush(message._message); // to avoid GC in csend vpush(args); + EuslispMessage message(emessage); pointer request(csend(ctx,emessage,K_ROSEUS_GET,1,K_ROSEUS_REQUEST)); pointer response(csend(ctx,emessage,K_ROSEUS_GET,1,K_ROSEUS_RESPONSE)); boost::shared_ptr *callback = @@ -1294,7 +1276,6 @@ pointer ROSEUS_ADVERTISE_SERVICE(register context *ctx,int n,pointer *argv) (new EuslispServiceCallbackHelper(fncallback, args, message.__getMD5Sum(), message.__getDataType(), request, response))); vpop(); // pop args - vpop(); // pop message._message AdvertiseServiceOptions aso; aso.service.assign(service); @@ -1899,10 +1880,8 @@ pointer ROSEUS_GET_TOPICS(register context *ctx,int n,pointer *argv) for (ros::master::V_TopicInfo::iterator it = topics.begin() ; it != topics.end(); it++) { const ros::master::TopicInfo& info = *it; pointer tmp = cons(ctx,makestring((char*)info.name.c_str(), info.name.length()), makestring((char*)info.datatype.c_str(), info.datatype.length())); - vpush(tmp); ccdr(ret) = cons(ctx, tmp, NIL); ret = ccdr(ret); - vpop(); // vpush(tmp) } vpop(); // vpush(ret) From 4242c9502926d94f7153025e8e02fdc01a53aa9a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 4 Nov 2022 14:50:19 +0900 Subject: [PATCH 169/195] Use sys::*gc-debug* instead of gc-hook in tests --- roseus/test/fibonacci-client.l | 2 +- roseus/test/fibonacci-server.l | 2 +- roseus/test/listener.l | 2 +- roseus/test/say.l | 2 +- roseus/test/simple-client-cancel-test.l | 2 +- roseus/test/simple-client-test.l | 2 +- roseus/test/simple-client-wait-test.l | 2 +- roseus/test/test-actionlib.l | 2 +- roseus/test/test-add-two-ints-server-groupname.l | 2 +- roseus/test/test-add-two-ints-server.l | 2 +- roseus/test/test-add-two-ints.l | 2 +- roseus/test/test-compile-message.l | 2 +- roseus/test/test-namespace.l | 2 +- roseus/test/test-roseus.l | 2 +- roseus/test/test-talker-listener.l | 2 +- roseus/test/test-tf.l | 2 +- roseus/test/test-timer.l | 2 +- 17 files changed, 17 insertions(+), 17 deletions(-) diff --git a/roseus/test/fibonacci-client.l b/roseus/test/fibonacci-client.l index ade547427..e6121d59d 100755 --- a/roseus/test/fibonacci-client.l +++ b/roseus/test/fibonacci-client.l @@ -6,7 +6,7 @@ (ros::load-ros-manifest "actionlib_tutorials") (ros::roseus "fibonacci_client") -(setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A~%" a b))) +(setq sys::*gc-debug* t) (ros::service-call "~set_logger_level" (instance roscpp::SetLoggerLevelRequest diff --git a/roseus/test/fibonacci-server.l b/roseus/test/fibonacci-server.l index 6a12c7d97..10ef02fea 100755 --- a/roseus/test/fibonacci-server.l +++ b/roseus/test/fibonacci-server.l @@ -8,7 +8,7 @@ ;;; ;;; -(setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A~%" a b))) +(setq sys::*gc-debug* t) (ros::roseus "fibonacci") (defun fibonacci-execute-cb (server goal) diff --git a/roseus/test/listener.l b/roseus/test/listener.l index d02a17fa7..a6caae105 100755 --- a/roseus/test/listener.l +++ b/roseus/test/listener.l @@ -9,7 +9,7 @@ ;;; ;;; (ros::roseus "listener") -;;(setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A~%" a b))) +;(setq sys::*gc-debug* t) ;; callback function ;(defun string-cb (msg) (print (list 'cb (sys::thread-self) (send msg :data)))) diff --git a/roseus/test/say.l b/roseus/test/say.l index dbce503aa..127b829be 100644 --- a/roseus/test/say.l +++ b/roseus/test/say.l @@ -6,7 +6,7 @@ (setq ros::*debug* t) ;;; ;;; -(setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A ~A~%" (sys::thread-self) a b))) +(setq sys::*gc-debug* t) (ros::roseus "clear") (ros::advertise "robotsound" sound_play::SoundRequest 5) diff --git a/roseus/test/simple-client-cancel-test.l b/roseus/test/simple-client-cancel-test.l index 9c332befd..f7a78f02e 100755 --- a/roseus/test/simple-client-cancel-test.l +++ b/roseus/test/simple-client-cancel-test.l @@ -12,7 +12,7 @@ ;;; ;;; -(setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A~%" a b))) +(setq sys::*gc-debug* t) (deftest test-client-cancel-by-myself () (let (c1 g1) diff --git a/roseus/test/simple-client-test.l b/roseus/test/simple-client-test.l index 2c613f5c2..0d9e74aef 100755 --- a/roseus/test/simple-client-test.l +++ b/roseus/test/simple-client-test.l @@ -12,7 +12,7 @@ ;;; ;;; -(setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A~%" a b))) +(setq sys::*gc-debug* t) (deftest test-simple-client () (let (c goal) diff --git a/roseus/test/simple-client-wait-test.l b/roseus/test/simple-client-wait-test.l index 1f02ad00e..ec1144008 100755 --- a/roseus/test/simple-client-wait-test.l +++ b/roseus/test/simple-client-wait-test.l @@ -12,7 +12,7 @@ ;;; ;;; -(setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A~%" a b))) +(setq sys::*gc-debug* t) (deftest test-client () (let (c goal) diff --git a/roseus/test/test-actionlib.l b/roseus/test/test-actionlib.l index 5b3a27ff4..649d7297d 100755 --- a/roseus/test/test-actionlib.l +++ b/roseus/test/test-actionlib.l @@ -6,7 +6,7 @@ (ros::load-ros-manifest "actionlib_tutorials") (ros::roseus "test_actionlib") -(setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A~%" a b))) +(setq sys::*gc-debug* t) (require :unittest "lib/llib/unittest.l") (init-unit-test) diff --git a/roseus/test/test-add-two-ints-server-groupname.l b/roseus/test/test-add-two-ints-server-groupname.l index 2b5debc4c..6544affaa 100644 --- a/roseus/test/test-add-two-ints-server-groupname.l +++ b/roseus/test/test-add-two-ints-server-groupname.l @@ -2,7 +2,7 @@ ;;; ;;; -(setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A~%" a b))) +(setq sys::*gc-debug* t) (require :unittest "lib/llib/unittest.l") (ros::load-ros-manifest "roseus") diff --git a/roseus/test/test-add-two-ints-server.l b/roseus/test/test-add-two-ints-server.l index 04116eae6..fe9ebbbf2 100644 --- a/roseus/test/test-add-two-ints-server.l +++ b/roseus/test/test-add-two-ints-server.l @@ -2,7 +2,7 @@ ;;; ;;; -(setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A~%" a b))) +(setq sys::*gc-debug* t) (require :unittest "lib/llib/unittest.l") (ros::load-ros-manifest "roseus") diff --git a/roseus/test/test-add-two-ints.l b/roseus/test/test-add-two-ints.l index 59d5e4fea..a9e41f709 100755 --- a/roseus/test/test-add-two-ints.l +++ b/roseus/test/test-add-two-ints.l @@ -3,7 +3,7 @@ ;;; euslisp version of ros_tutorials/rospy_tutorials/test_add_two_ints.py ;;; -(setq sys::*gc-hook* #'(lambda (a b) (format t ";; gc ~A ~A~%" a b))) +(setq sys::*gc-debug* t) (require :unittest "lib/llib/unittest.l") (ros::load-ros-manifest "roseus") diff --git a/roseus/test/test-compile-message.l b/roseus/test/test-compile-message.l index ef96fa9e4..dc552d04d 100644 --- a/roseus/test/test-compile-message.l +++ b/roseus/test/test-compile-message.l @@ -4,7 +4,7 @@ (require :unittest "lib/llib/unittest.l") (ros::roseus "test-compile-message") -;(setq sys::*gc-hook* #'(lambda (a b) (format *error-output* ";; gc ~A ~A~%" a b))) +;(setq sys::*gc-debug* t) (init-unit-test) diff --git a/roseus/test/test-namespace.l b/roseus/test/test-namespace.l index 47fe0f849..1d7ebe998 100755 --- a/roseus/test/test-namespace.l +++ b/roseus/test/test-namespace.l @@ -4,7 +4,7 @@ (require :unittest "lib/llib/unittest.l") (ros::roseus "test-namespace") -;(setq sys::*gc-hook* #'(lambda (a b) (format *error-output* ";; gc ~A ~A~%" a b))) +;(setq sys::*gc-debug* t) (init-unit-test) diff --git a/roseus/test/test-roseus.l b/roseus/test/test-roseus.l index 18fcfa859..5fc989567 100644 --- a/roseus/test/test-roseus.l +++ b/roseus/test/test-roseus.l @@ -4,7 +4,7 @@ (require :unittest "lib/llib/unittest.l") (ros::roseus "test-roseus") -;(setq sys::*gc-hook* #'(lambda (a b) (format *error-output* ";; gc ~A ~A~%" a b))) +;(setq sys::*gc-debug* t) (init-unit-test) diff --git a/roseus/test/test-talker-listener.l b/roseus/test/test-talker-listener.l index 2f6960d9a..2e83f6e02 100755 --- a/roseus/test/test-talker-listener.l +++ b/roseus/test/test-talker-listener.l @@ -9,7 +9,7 @@ ;;; ;;; -(setq sys::*gc-hook* #'(lambda (a b) (format *error-output* ";; gc ~A ~A~%" a b))) +(setq sys::*gc-debug* t) (init-unit-test) ;; method call diff --git a/roseus/test/test-tf.l b/roseus/test/test-tf.l index 99c154bf2..80396b8e4 100755 --- a/roseus/test/test-tf.l +++ b/roseus/test/test-tf.l @@ -8,7 +8,7 @@ ;;(unix::putenv "ROSCONSOLE_CONFIG_FILE=/home/k-okada/ros/electric/jsk-ros-pkg/roseus/roseus.config") (ros::roseus "test-tf") -;;(setq sys::*gc-hook* #'(lambda (a b) (format *error-output* ";; gc ~A ~A~%" a b))) +;(setq sys::*gc-debug* t) (init-unit-test) diff --git a/roseus/test/test-timer.l b/roseus/test/test-timer.l index 843e0abf5..77bef0171 100755 --- a/roseus/test/test-timer.l +++ b/roseus/test/test-timer.l @@ -1,6 +1,6 @@ #!/usr/bin/env roseus -(setq sys::*gc-hook* #'(lambda (a b) (format *error-output* ";; gc ~A ~A~%" a b))) +(setq sys::*gc-debug* t) ;; (ros::roseus "test_timer") From f18a5318409dd26a3c70fcbcbcef3778a03488f9 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 7 Nov 2022 11:47:34 +0900 Subject: [PATCH 170/195] (roseus_bt) Better euslisip function formatting with underscores --- roseus_bt/include/roseus_bt/xml_parser.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index be0bcc9ac..c01228e6c 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -541,8 +541,9 @@ void XMLParser::maybe_push_message_package(const std::string message_type, } std::string XMLParser::format_eus_name(const std::string input) { - std::regex e ("([^A-Z]+)([A-Z]+)"); + std::regex e ("([a-z]+)([A-Z]+)"); std::string out = std::regex_replace(input, e, "$1-$2"); + std::replace(out.begin(), out.end(), '_', '-'); std::transform(out.begin(), out.end(), out.begin(), [](unsigned char c){ return std::tolower(c); }); return out; From 41f7dab9c16aa3a2dec262ae1a1cd60228d39060 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 11 Nov 2022 16:53:24 +0900 Subject: [PATCH 171/195] Unregister oneshot timers after execution --- roseus/roseus.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index d7fe16c79..836f28c58 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -1891,8 +1891,11 @@ pointer ROSEUS_GET_TOPICS(register context *ctx,int n,pointer *argv) class TimerFunction { pointer _scb, _args; + string _funcname; + bool _oneshot; public: - TimerFunction(pointer scb, pointer args) : _scb(scb), _args(args) { + TimerFunction(pointer scb, pointer args, string funcname, bool oneshot) : + _scb(scb), _args(args), _funcname(funcname), _oneshot(oneshot) { //context *ctx = current_ctx; //ROS_WARN("func");prinx(ctx,scb,ERROUT);flushstream(ERROUT);terpri(ERROUT); //ROS_WARN("argc");prinx(ctx,args,ERROUT);flushstream(ERROUT);terpri(ERROUT); @@ -1932,6 +1935,16 @@ class TimerFunction ufuncall(ctx,(ctx->callfp?ctx->callfp->form:NIL),_scb,(pointer)(ctx->vsp-argc),NULL,argc); while(argc-->0)vpop(); + // unregister callback when finished + if (_oneshot) { + bool bSuccess = s_mapTimered.erase(_funcname)>0; + if (bSuccess) { + ROS_DEBUG("one shot timer successfully exited: %s", _funcname.c_str()); + } + else { + ROS_ERROR("could not unregister oneshot timer: %s", _funcname.c_str()); + } + } } }; @@ -2012,7 +2025,8 @@ pointer ROSEUS_CREATE_TIMER(register context *ctx,int n,pointer *argv) // ;; store mapTimered ROS_DEBUG("create timer %s at %f (oneshot=%d) (groupname=%s)", fncallname.c_str(), period, oneshot, groupname.c_str()); - s_mapTimered[fncallname] = lnode->createTimer(ros::Duration(period), TimerFunction(fncallback, args), oneshot); + TimerFunction t_fn(fncallback, args, fncallname, oneshot); + s_mapTimered[fncallname] = lnode->createTimer(ros::Duration(period), t_fn, oneshot); return (T); } From 591df5651fdf6ff5f826bc4217d452dd0aa423e9 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 9 Nov 2022 23:28:30 +0900 Subject: [PATCH 172/195] Allow to use :groupname nil for default groupname in ros functions --- roseus/roseus.cpp | 39 ++++++++++++++++++++++----------------- 1 file changed, 22 insertions(+), 17 deletions(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index 836f28c58..06f1d8c09 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -859,8 +859,9 @@ pointer ROSEUS_SUBSCRIBE(register context *ctx,int n,pointer *argv) if (isstring(argv[0])) topicname = ros::names::resolve((char *)get_string(argv[0])); else error(E_NOSTRING); - if (n > 1 && issymbol(argv[n-2]) && isstring(argv[n-1])) { - if (argv[n-2] == K_ROSEUS_GROUPNAME) { + if (n > 1 && issymbol(argv[n-2]) && argv[n-2] == K_ROSEUS_GROUPNAME) { + if (argv[n-1] != NIL) { + if (!isstring(argv[n-1])) error(E_NOSTRING); string groupname; groupname.assign((char *)get_string(argv[n-1])); map >::iterator it = s_mapHandle.find(groupname); @@ -872,8 +873,8 @@ pointer ROSEUS_SUBSCRIBE(register context *ctx,int n,pointer *argv) groupname.c_str(), topicname.c_str(), groupname.c_str()); return (NIL); } - n -= 2; } + n -= 2; } if (isint(argv[n-1])) {queuesize = ckintval(argv[n-1]);n--;} ROS_DEBUG("subscribe %s queuesize=%d", topicname.c_str(), queuesize); @@ -1243,8 +1244,9 @@ pointer ROSEUS_ADVERTISE_SERVICE(register context *ctx,int n,pointer *argv) emessage = argv[1]; fncallback = argv[2]; - if (n >= 5 && issymbol(argv[n-2]) && isstring(argv[n-1])) { - if (argv[n-2] == K_ROSEUS_GROUPNAME) { + if (n >= 5 && issymbol(argv[n-2]) && argv[n-2] == K_ROSEUS_GROUPNAME) { + if (argv[n-1] != NIL) { + if (!isstring(argv[n-1])) error(E_NOSTRING); string groupname; groupname.assign((char *)get_string(argv[n-1])); map >::iterator it = s_mapHandle.find(groupname); @@ -1256,8 +1258,8 @@ pointer ROSEUS_ADVERTISE_SERVICE(register context *ctx,int n,pointer *argv) groupname.c_str(), service.c_str(), groupname.c_str()); return (NIL); } - n -= 2; } + n -= 2; } args=NIL; @@ -1965,23 +1967,26 @@ pointer ROSEUS_CREATE_TIMER(register context *ctx,int n,pointer *argv) { if (n > 1 && issymbol(argv[n-2])) { // ;; oneshot ;; - if (argv[n-2] == K_ROSEUS_ONESHOT && issymbol(argv[n-1])) { + if (argv[n-2] == K_ROSEUS_ONESHOT) { if ( argv[n-1] != NIL ) { oneshot = true; } n -= 2; } // ;; groupname ;; - else if (argv[n-2] == K_ROSEUS_GROUPNAME && isstring(argv[n-1])) { - groupname.assign((char *)get_string(argv[n-1])); - map >::iterator it = s_mapHandle.find(groupname); - if( it != s_mapHandle.end() ) { - ROS_DEBUG("create-timer with groupname=%s", groupname.c_str()); - lnode = (it->second).get(); - } else { - ROS_ERROR("Groupname %s is missing. Call (ros::create-nodehandle \"%s\") first.", - groupname.c_str(), groupname.c_str()); - return (NIL); + else if (argv[n-2] == K_ROSEUS_GROUPNAME) { + if (argv[n-1] != NIL) { + if (!isstring(argv[n-1])) error(E_NOSTRING); + groupname.assign((char *)get_string(argv[n-1])); + map >::iterator it = s_mapHandle.find(groupname); + if( it != s_mapHandle.end() ) { + ROS_DEBUG("create-timer with groupname=%s", groupname.c_str()); + lnode = (it->second).get(); + } else { + ROS_ERROR("Groupname %s is missing. Call (ros::create-nodehandle \"%s\") first.", + groupname.c_str(), groupname.c_str()); + return (NIL); + } } n -= 2; } From f00abc5cbb04c1251f2fcaa4834bba4e5ab14429 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 9 Nov 2022 23:30:13 +0900 Subject: [PATCH 173/195] Fix Wmisleading-indentation compilation warning --- roseus/roseus.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index 06f1d8c09..a6db4ff65 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -98,7 +98,8 @@ extern "C" { byte *get_string(register pointer s){ if (isstring(s)) return(s->c.str.chars); if (issymbol(s)) return(s->c.sym.pname->c.str.chars); - else error(E_NOSTRING); return NULL; } + else error(E_NOSTRING); + return NULL; } } #undef class From c8f9992fa3caa22f5e25ef8469fce7e86a44aa69 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 9 Nov 2022 17:49:44 +0900 Subject: [PATCH 174/195] Supress thread warnings --- roseus/roseus.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index a6db4ff65..72c45a4dc 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -229,7 +229,7 @@ class EuslispMessage EuslispMessage(const EuslispMessage &r) { context *ctx = current_ctx; vpush(r._message); - if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self()); + // if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self()); if ( isclass(r._message) ) { //ROS_ASSERT(isclass(r._message)); _message = makeobject(r._message); @@ -264,7 +264,7 @@ class EuslispMessage uint32_t serializationLength() const { context *ctx = current_ctx; - if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self()); + // if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self()); pointer a,curclass; a = (pointer)findmethod(ctx,K_ROSEUS_SERIALIZATION_LENGTH,classof(_message),&curclass); ROS_ASSERT(a!=NIL); @@ -274,7 +274,7 @@ class EuslispMessage virtual uint8_t *serialize(uint8_t *writePtr, uint32_t seqid) const { context *ctx = current_ctx; - if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self()); + // if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self()); pointer a,curclass; uint32_t len = serializationLength(); a = (pointer)findmethod(ctx,K_ROSEUS_SERIALIZE,classof(_message),&curclass); @@ -290,7 +290,7 @@ class EuslispMessage virtual uint8_t *deserialize(uint8_t *readPtr, uint32_t sz) { context *ctx = current_ctx; - if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self()); + // if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self()); pointer a,curclass; if ( sz == 0 ) { From cc8e381cafbf3bf36d8f8a4e7c3152f2fb1c6d16 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 11 Nov 2022 14:31:18 +0900 Subject: [PATCH 175/195] (roseus_bt) Avoid CMake errors on bloom release --- roseus_bt/CMakeLists.txt | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/roseus_bt/CMakeLists.txt b/roseus_bt/CMakeLists.txt index 37e0e7176..d51ae9dd2 100644 --- a/roseus_bt/CMakeLists.txt +++ b/roseus_bt/CMakeLists.txt @@ -21,15 +21,19 @@ catkin_package( ) # 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}") +# only when not in bloom build +if (NOT $ENV{DH_OPTIONS}) + find_package(Git QUIET) + option(GIT_SUBMODULE "Check submodules during build" ON) + if(Git_FOUND AND 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(WARNING "git submodule update failed with ${GIT_SUBMOD_RESULT}") + message(WARNING "please update submodules manually with: git submodule update --init --remote include/rosbridgecpp") + endif() endif() endif() From 599c4660fc174762048bdee637463c5735a98fa6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 11 Nov 2022 18:35:18 +0900 Subject: [PATCH 176/195] Add mutex locks for static data assignment --- roseus/roseus.cpp | 68 +++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 57 insertions(+), 11 deletions(-) diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index 72c45a4dc..0fbdb0a2a 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -124,6 +124,52 @@ using namespace std; #define isInstalledCheck \ if( ! ros::ok() ) { error(E_USER,"You must call (ros::roseus \"name\") before creating the first NodeHandle"); } + +template +class SharedMap +{ +public: + Value get(const Key& key) { + boost::shared_lock lock(_mutex); + return(_map[key]); + } + + void set(const Key& key, const Value& value) { + boost::unique_lock lock(_mutex); + _map[key] = value; + } + + size_t erase(const Key& key) { + boost::unique_lock lock(_mutex); + return _map.erase(key); + } + + void clear() { + boost::unique_lock lock(_mutex); + _map.clear(); + } + + typename map::iterator find(const Key& key) { + boost::shared_lock lock(_mutex); + return(_map.find(key)); + } + + typename map::iterator begin() { + boost::shared_lock lock(_mutex); + return(_map.end()); + } + + typename map::iterator end() { + boost::shared_lock lock(_mutex); + return(_map.end()); + } + +private: + map _map; + boost::shared_mutex _mutex; +}; + + class RoseusStaticData { public: @@ -132,12 +178,12 @@ class RoseusStaticData } boost::shared_ptr node; boost::shared_ptr rate; - map > mapAdvertised; ///< advertised topics - map > mapSubscribed; ///< subscribed topics - map > mapServiced; ///< subscribed topics - map mapTimered; ///< subscribed timers + SharedMap > mapAdvertised; ///< advertised topics + SharedMap > mapSubscribed; ///< subscribed topics + SharedMap > mapServiced; ///< subscribed topics + SharedMap mapTimered; ///< subscribed timers - map > mapHandle; ///< for grouping nodehandle + SharedMap > mapHandle; ///< for grouping nodehandle }; static RoseusStaticData s_staticdata; @@ -694,10 +740,10 @@ pointer ROSEUS_CREATE_NODEHANDLE(register context *ctx,int n,pointer *argv) boost::shared_ptr hd; if ( n > 1 ) { hd = boost::shared_ptr (new NodeHandle(ns)); - s_mapHandle[groupname] = hd; + s_mapHandle.set(groupname, hd); } else { hd = boost::shared_ptr(new NodeHandle()); - s_mapHandle[groupname] = hd; + s_mapHandle.set(groupname, hd); } //add callbackqueue to hd hd->setCallbackQueue( new CallbackQueue() ); @@ -894,7 +940,7 @@ pointer ROSEUS_SUBSCRIBE(register context *ctx,int n,pointer *argv) Subscriber subscriber = lnode->subscribe(so); boost::shared_ptr sub = boost::shared_ptr(new Subscriber(subscriber)); if ( !!sub ) { - s_mapSubscribed[topicname] = sub; + s_mapSubscribed.set(topicname, sub); } else { ROS_ERROR("s_mapSubscribed"); } @@ -992,7 +1038,7 @@ pointer ROSEUS_ADVERTISE(register context *ctx,int n,pointer *argv) Publisher publisher = s_node->advertise(ao); boost::shared_ptr pub = boost::shared_ptr(new Publisher(publisher)); if ( !!pub ) { - s_mapAdvertised[topicname] = pub; + s_mapAdvertised.set(topicname, pub); } else { ROS_ERROR("s_mapAdvertised"); } @@ -1290,7 +1336,7 @@ pointer ROSEUS_ADVERTISE_SERVICE(register context *ctx,int n,pointer *argv) ServiceServer server = lnode->advertiseService(aso); boost::shared_ptr ser = boost::shared_ptr(new ServiceServer(server)); if ( !!ser ) { - s_mapServiced[service] = ser; + s_mapServiced.set(service, ser); } else { ROS_ERROR("s_mapServiced"); } @@ -2032,7 +2078,7 @@ pointer ROSEUS_CREATE_TIMER(register context *ctx,int n,pointer *argv) // ;; store mapTimered ROS_DEBUG("create timer %s at %f (oneshot=%d) (groupname=%s)", fncallname.c_str(), period, oneshot, groupname.c_str()); TimerFunction t_fn(fncallback, args, fncallname, oneshot); - s_mapTimered[fncallname] = lnode->createTimer(ros::Duration(period), t_fn, oneshot); + s_mapTimered.set(fncallname, lnode->createTimer(ros::Duration(period), t_fn, oneshot)); return (T); } From 866e89b9734acf79bae381d8505e9655cc98830a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 12 Nov 2022 01:52:51 +0900 Subject: [PATCH 177/195] Remove compile warnings in roseus_c_util.c --- roseus/roseus_c_util.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roseus/roseus_c_util.c b/roseus/roseus_c_util.c index e99eb23ee..9081851cf 100644 --- a/roseus/roseus_c_util.c +++ b/roseus/roseus_c_util.c @@ -18,7 +18,7 @@ extern pointer ___roseus_c_util(); #define ismatrix(p) ((isarray(p) && \ p->c.ary.rank==makeint(2) && \ elmtypeof(p->c.ary.entity)==ELM_FLOAT)) -#define ckvsize(a,b) ((a->c.vec.size==b->c.vec.size)?vecsize(a):(int)error(E_VECINDEX)) +#define ckvsize(a,b) ((a->c.vec.size==b->c.vec.size)?vecsize(a):error(E_VECINDEX)) pointer CONV_MSG2_PC (ctx,n,argv) register context *ctx; From 5fcecafdacda293f7f73f36f16e791d6b48f463f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 12 Nov 2022 01:55:44 +0900 Subject: [PATCH 178/195] Avoid memory leaks when removing function helpers --- roseus/euslisp/roseus.l | 11 ++++++++-- roseus/roseus.cpp | 47 ++++++++++++++++++++++++++++++----------- 2 files changed, 44 insertions(+), 14 deletions(-) diff --git a/roseus/euslisp/roseus.l b/roseus/euslisp/roseus.l index 50d70e5ef..7bde6579f 100644 --- a/roseus/euslisp/roseus.l +++ b/roseus/euslisp/roseus.l @@ -14,6 +14,15 @@ (in-package "ROS") +(defvar ros::*compile-message* nil) +(defvar ros::*static-data-vector* + (vector + (make-hash-table :size 32 :test #'string=) ;; s_mapSubscribed + (make-hash-table :size 32 :test #'string=) ;; s_mapServiced + (make-hash-table :size 16 :test #'string=) ;; s_mapTimered + )) + + (defun ros::roseus-sigint-handler (sig) (ros::ros-error "Received shutdown request (SIGINT)") (exit 1)) @@ -55,8 +64,6 @@ (ros::set-logger-level level) ))) -(setq ros::*compile-message* nil) - (shadow 'object *package*) (defclass ros::object :super propertied-object diff --git a/roseus/roseus.cpp b/roseus/roseus.cpp index 0fbdb0a2a..163efd8f7 100644 --- a/roseus/roseus.cpp +++ b/roseus/roseus.cpp @@ -196,7 +196,12 @@ static bool s_bInstalled = false; #define s_mapTimered s_staticdata.mapTimered #define s_mapHandle s_staticdata.mapHandle +#define s_mapSubscribedIndex 0 +#define s_mapServicedIndex 1 +#define s_mapTimeredIndex 2 + pointer K_ROSEUS_MD5SUM,K_ROSEUS_DATATYPE,K_ROSEUS_DEFINITION,K_ROSEUS_CONNECTION_HEADER,K_ROSEUS_SERIALIZATION_LENGTH,K_ROSEUS_SERIALIZE,K_ROSEUS_DESERIALIZE,K_ROSEUS_INIT,K_ROSEUS_GET,K_ROSEUS_REQUEST,K_ROSEUS_RESPONSE,K_ROSEUS_GROUPNAME,K_ROSEUS_ONESHOT,K_ROSEUS_LAST_EXPECTED,K_ROSEUS_LAST_REAL,K_ROSEUS_CURRENT_EXPECTED,K_ROSEUS_CURRENT_REAL,K_ROSEUS_LAST_DURATION,K_ROSEUS_SEC,K_ROSEUS_NSEC,QANON,QNOOUT,QREPOVERSION,QROSDEBUG,QROSINFO,QROSWARN,QROSERROR,QROSFATAL; +pointer QSTATICDATAVECTOR, K_ROSEUS_ENTER, K_ROSEUS_DELETE; extern pointer LAMCLOSURE; /*********************************************************** @@ -254,14 +259,21 @@ int getInteger(pointer message, pointer method) { return 0; } -void store_pointer(context *ctx, pointer value) { - // Stores a callback list as a symbol in the lisp package. +void store_pointer(context *ctx, string name, int index, pointer value) { + // Stores a callback list in ros::*static-data-vector* // This prevents that the callable gets collected by the gc, // even if there are no other references to it. - // TODO: cleanup when unsubscribed vpush(value); - pointer p=gensym(ctx); - defconst(ctx, (char*)(p->c.sym.pname->c.str.chars), vpop(), lisppkg); + pointer p=makestring((char *)name.c_str(), name.size()); + csend(ctx,speval(QSTATICDATAVECTOR)->c.vec.v[index],K_ROSEUS_ENTER,2,p,vpop()); +} + +void erase_pointer(context *ctx, string name, int index) { + // Removes the entry for ros::*static-data-vector*, + // meaning that the pointers can be deallocated if + // there are no other references to them. + pointer p=makestring((char *)name.c_str(), name.size()); + csend(ctx,speval(QSTATICDATAVECTOR)->c.vec.v[index],K_ROSEUS_DELETE,1,p); } class EuslispMessage @@ -408,7 +420,9 @@ class EuslispSubscriptionCallbackHelper : public ros::SubscriptionCallbackHelper pointer _scb,_args; EuslispMessage _msg; - EuslispSubscriptionCallbackHelper(pointer scb, pointer args,pointer tmpl) : _args(args), _msg(tmpl) { + EuslispSubscriptionCallbackHelper(string name, pointer scb, pointer args, pointer tmpl) : + _args(args), _msg(tmpl) + { context *ctx = current_ctx; //ROS_WARN("func");prinx(ctx,scb,ERROUT);flushstream(ERROUT);terpri(ERROUT); //ROS_WARN("argc");prinx(ctx,args,ERROUT);flushstream(ERROUT);terpri(ERROUT); @@ -424,7 +438,7 @@ class EuslispSubscriptionCallbackHelper : public ros::SubscriptionCallbackHelper ROS_ERROR("subscription callback function install error"); } // avoid gc - store_pointer(ctx, cons(ctx,scb,args)); + store_pointer(ctx, name, s_mapSubscribedIndex, cons(ctx,scb,args)); } ~EuslispSubscriptionCallbackHelper() { ROS_ERROR("subscription gc"); @@ -487,7 +501,9 @@ class EuslispServiceCallbackHelper : public ros::ServiceCallbackHelper { string md5, datatype, requestDataType, responseDataType, requestMessageDefinition, responseMessageDefinition; - EuslispServiceCallbackHelper(pointer scb, pointer args, string smd5, string sdatatype, pointer reqclass, pointer resclass) : _args(args), _req(reqclass), _res(resclass), md5(smd5), datatype(sdatatype) { + EuslispServiceCallbackHelper(string name, pointer scb, pointer args, string smd5, string sdatatype, pointer reqclass, pointer resclass) : + _args(args), _req(reqclass), _res(resclass), md5(smd5), datatype(sdatatype) + { context *ctx = current_ctx; //ROS_WARN("func");prinx(ctx,scb,ERROUT);flushstream(ERROUT);terpri(ERROUT); //ROS_WARN("argc");prinx(ctx,args,ERROUT);flushstream(ERROUT);terpri(ERROUT); @@ -504,7 +520,7 @@ class EuslispServiceCallbackHelper : public ros::ServiceCallbackHelper { ROS_ERROR("service callback function install error"); } // avoid gc - store_pointer(ctx, cons(ctx,scb,args)); + store_pointer(ctx, name, s_mapServicedIndex, cons(ctx,scb,args)); requestDataType = _req.__getDataType(); responseDataType = _res.__getDataType(); @@ -665,6 +681,8 @@ pointer ROSEUS(register context *ctx,int n,pointer *argv) K_ROSEUS_LAST_DURATION = defkeyword(ctx,"LAST-DURATION"); K_ROSEUS_SEC = defkeyword(ctx,"SEC"); K_ROSEUS_NSEC = defkeyword(ctx,"NSEC"); + K_ROSEUS_ENTER = defkeyword(ctx,"ENTER"); + K_ROSEUS_DELETE = defkeyword(ctx,"DELETE"); s_mapAdvertised.clear(); s_mapSubscribed.clear(); @@ -934,7 +952,7 @@ pointer ROSEUS_SUBSCRIBE(register context *ctx,int n,pointer *argv) EuslispMessage msg(message); boost::shared_ptr *callback = (new boost::shared_ptr - (new EuslispSubscriptionCallbackHelper(fncallback, args, message))); + (new EuslispSubscriptionCallbackHelper(topicname, fncallback, args, message))); SubscribeOptions so(topicname, queuesize, msg.__getMD5Sum(), msg.__getDataType()); so.helper = *callback; Subscriber subscriber = lnode->subscribe(so); @@ -957,6 +975,7 @@ pointer ROSEUS_UNSUBSCRIBE(register context *ctx,int n,pointer *argv) else error(E_NOSTRING); bool bSuccess = s_mapSubscribed.erase(topicname)>0; + erase_pointer(ctx, topicname, s_mapSubscribedIndex); return (bSuccess?T:NIL); } @@ -1322,7 +1341,7 @@ pointer ROSEUS_ADVERTISE_SERVICE(register context *ctx,int n,pointer *argv) pointer response(csend(ctx,emessage,K_ROSEUS_GET,1,K_ROSEUS_RESPONSE)); boost::shared_ptr *callback = (new boost::shared_ptr - (new EuslispServiceCallbackHelper(fncallback, args, message.__getMD5Sum(), + (new EuslispServiceCallbackHelper(service, fncallback, args, message.__getMD5Sum(), message.__getDataType(), request, response))); vpop(); // pop args @@ -1354,6 +1373,7 @@ pointer ROSEUS_UNADVERTISE_SERVICE(register context *ctx,int n,pointer *argv) ROS_DEBUG("unadvertise %s", service.c_str()); bool bSuccess = s_mapServiced.erase(service)>0; + erase_pointer(ctx, service, s_mapServicedIndex); return (bSuccess?T:NIL); } @@ -1987,6 +2007,7 @@ class TimerFunction // unregister callback when finished if (_oneshot) { bool bSuccess = s_mapTimered.erase(_funcname)>0; + erase_pointer(ctx, _funcname, s_mapTimeredIndex); if (bSuccess) { ROS_DEBUG("one shot timer successfully exited: %s", _funcname.c_str()); } @@ -2073,7 +2094,7 @@ pointer ROSEUS_CREATE_TIMER(register context *ctx,int n,pointer *argv) for (int i=n-1;i>=2;i--) args=cons(ctx,argv[i],args); // avoid gc - store_pointer(ctx, cons(ctx,fncallback,args)); + store_pointer(ctx, fncallname, s_mapTimeredIndex, cons(ctx,fncallback,args)); // ;; store mapTimered ROS_DEBUG("create timer %s at %f (oneshot=%d) (groupname=%s)", fncallname.c_str(), period, oneshot, groupname.c_str()); @@ -2109,6 +2130,8 @@ pointer ___roseus(register context *ctx, int n, pointer *argv, pointer env) QROSWARN=defvar(ctx,"*ROSWARN*",makeint(3),rospkg); QROSERROR=defvar(ctx,"*ROSERROR*",makeint(4),rospkg); QROSFATAL=defvar(ctx,"*ROSFATAL*",makeint(5),rospkg); + QSTATICDATAVECTOR=intern(ctx,"*STATIC-DATA-VECTOR*",20,rospkg); + defun(ctx,"SPIN",argv[0],(pointer (*)())ROSEUS_SPIN, "Enter simple event loop"); defun(ctx,"SPIN-ONCE",argv[0],(pointer (*)())ROSEUS_SPINONCE, From 0302c13f88f29ada88e9b82f127c5d39e6caa7ef Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 14 Nov 2022 10:19:41 +0900 Subject: [PATCH 179/195] (roseus_bt) define result_ member as Document to avoid having the allocator go out of scope --- roseus_bt/include/roseus_bt/ws_action_client.h | 8 +++----- roseus_bt/include/roseus_bt/ws_service_client.h | 10 +++++----- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/roseus_bt/include/roseus_bt/ws_action_client.h b/roseus_bt/include/roseus_bt/ws_action_client.h index 5bfe1936f..9455e5e1e 100644 --- a/roseus_bt/include/roseus_bt/ws_action_client.h +++ b/roseus_bt/include/roseus_bt/ws_action_client.h @@ -14,6 +14,7 @@ class RosbridgeActionClient 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), + result_(rapidjson::kObjectType), is_active_(false) { if (server_name_.front() != '/') { @@ -89,7 +90,7 @@ class RosbridgeActionClient RosbridgeWsClient rbc_; bool is_active_; - rapidjson::Value result_; + rapidjson::Document result_; std::string server_name_; std::string goal_topic_; @@ -108,10 +109,7 @@ class RosbridgeActionClient std::cout << "resultCallback(): Message Received: " << message << std::endl; #endif - rapidjson::Document document(rapidjson::kObjectType); - document.Parse(message.c_str()); - rapidjson::Value res(document, document.GetAllocator()); - result_ = res; + result_.Parse(message.c_str()); is_active_ = false; } diff --git a/roseus_bt/include/roseus_bt/ws_service_client.h b/roseus_bt/include/roseus_bt/ws_service_client.h index 88f1a355a..f52d05561 100644 --- a/roseus_bt/include/roseus_bt/ws_service_client.h +++ b/roseus_bt/include/roseus_bt/ws_service_client.h @@ -14,6 +14,7 @@ class RosbridgeServiceClient RosbridgeServiceClient(const std::string& master, int port, const std::string& service_name): rbc_(fmt::format("{}:{}", master, std::to_string(port))), service_name_(service_name), + result_(rapidjson::kObjectType), is_active_(false) { if (service_name_.front() != '/') { @@ -46,7 +47,9 @@ class RosbridgeServiceClient } void waitForResult() { +#ifdef DEBUG std::cout << "RemoteService: waiting for result: " << service_name_ << std::endl; +#endif while (is_active_) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); } @@ -58,7 +61,7 @@ class RosbridgeServiceClient RosbridgeWsClient rbc_; bool is_active_; - rapidjson::Value result_; + rapidjson::Document result_; std::string service_name_; @@ -71,10 +74,7 @@ class RosbridgeServiceClient std::cout << "serviceResponseCallback(): Message Received: " << message << std::endl; #endif - rapidjson::Document document(rapidjson::kObjectType); - document.Parse(message.c_str()); - rapidjson::Value res(document, document.GetAllocator()); - result_ = res; + result_.Parse(message.c_str()); is_active_ = false; connection->send_close(1000); From e3f5f3fb920262f497d140533c2bce498dba920f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 15 Nov 2022 14:10:49 +0900 Subject: [PATCH 180/195] (roseus_bt) Throw errors on failed requests --- roseus_bt/include/roseus_bt/eus_action_node.h | 10 ++++++- .../include/roseus_bt/eus_condition_node.h | 6 ++++ roseus_bt/include/roseus_bt/gen_template.h | 29 +++++++++---------- 3 files changed, 28 insertions(+), 17 deletions(-) diff --git a/roseus_bt/include/roseus_bt/eus_action_node.h b/roseus_bt/include/roseus_bt/eus_action_node.h index 68bcdd321..d8721fb8c 100644 --- a/roseus_bt/include/roseus_bt/eus_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_action_node.h @@ -40,10 +40,18 @@ class EusActionNode: public BT::RosActionNode virtual NodeStatus onResult( const typename BT::RosActionNode::ResultType& res) = 0; virtual void onFeedback( const typename FeedbackType::ConstPtr& feedback) = 0; + virtual NodeStatus onFailedRequest(enum BT::RosActionNode::FailureCause failure) override + { + const std::string server_name = BT::TreeNode::getInput("server_name").value(); + throw BT::RuntimeError("Lost connection to action server at: ", server_name); + } + virtual void halt() override { BT::RosActionNode::halt(); - BT::RosActionNode::action_client_->waitForResult(); + if (BT::RosActionNode::action_client_->isServerConnected()) { + BT::RosActionNode::action_client_->waitForResult(); + } } protected: diff --git a/roseus_bt/include/roseus_bt/eus_condition_node.h b/roseus_bt/include/roseus_bt/eus_condition_node.h index fc61d13a7..ed34935df 100644 --- a/roseus_bt/include/roseus_bt/eus_condition_node.h +++ b/roseus_bt/include/roseus_bt/eus_condition_node.h @@ -26,6 +26,12 @@ class EusConditionNode : public BT::RosServiceNode public: EusConditionNode() = delete; virtual ~EusConditionNode() = default; + + virtual NodeStatus onFailedRequest(enum BT::RosServiceNode::FailureCause failure) override + { + const std::string server_name = BT::TreeNode::getInput("service_name").value(); + throw BT::RuntimeError("Lost connection to service server at: ", server_name); + } }; } // namespace BT diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 04edd9725..7f7874a0b 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -173,11 +173,6 @@ EusActionNode<%1%::%2%Action>(handle, name, conf) {} return NodeStatus::FAILURE; } - virtual NodeStatus onFailedRequest(FailureCause failure) override - { - return NodeStatus::FAILURE; - } - }; )"; boost::format bfmt = boost::format(fmt_string) % @@ -294,11 +289,6 @@ class %2%: public EusConditionNode<%1%::%2%> return NodeStatus::FAILURE; } - virtual NodeStatus onFailedRequest(FailureCause failure) override - { - return NodeStatus::FAILURE; - } - }; )"; @@ -484,12 +474,19 @@ int main(int argc, char **argv) NodeStatus status = NodeStatus::IDLE; - while( ros::ok() && (status == NodeStatus::IDLE || status == NodeStatus::RUNNING)) - { - ros::spinOnce(); - status = tree.tickRoot(); - ros::Duration sleep_time(0.005); - sleep_time.sleep(); + std::cout << "Writing log to file: " << log_filename << std::endl; + + try { + while( ros::ok() && (status == NodeStatus::IDLE || status == NodeStatus::RUNNING)) + { + ros::spinOnce(); + status = tree.tickRoot(); + ros::Duration sleep_time(0.005); + sleep_time.sleep(); + } + } + catch(BT::RuntimeError& err) { + std::cerr << "Behavior Tree execution terminated after throwing an instance of 'BT::RuntimeError'" << "\n what(): " << err.what() << std::endl; } std::cout << "Writed log to file: " << log_filename << std::endl; From d1c519b3bdf05337f120e8bb7aa15671a5fc4e9e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 15 Nov 2022 21:58:39 +0900 Subject: [PATCH 181/195] (roseus_bt) Throw errors on failed remote service calls --- roseus_bt/include/roseus_bt/ws_service_client.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/roseus_bt/include/roseus_bt/ws_service_client.h b/roseus_bt/include/roseus_bt/ws_service_client.h index f52d05561..6be87babc 100644 --- a/roseus_bt/include/roseus_bt/ws_service_client.h +++ b/roseus_bt/include/roseus_bt/ws_service_client.h @@ -2,6 +2,7 @@ #define BEHAVIOR_TREE_ROSEUS_BT_WS_SERVICE_CLIENT_ #include +#include #include @@ -42,6 +43,14 @@ class RosbridgeServiceClient } rapidjson::Value getResult() { + if (!(result_["result"].GetBool())) { + std::string err = "Error calling remote service: " + service_name_; + if (result_["values"].IsString()) { + err += "\n what(): "; + err += result_["values"].GetString(); + } + throw BT::RuntimeError(err); + } // TODO: reset result after getting return result_["values"].GetObject(); } From 2c81c8edb1a3098c538d3f2b7c4671286edece4c Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 21 Nov 2022 18:31:33 +0900 Subject: [PATCH 182/195] add common_argument_mapping.h --- .../roseus_bt/command_line_argument_mapping.h | 16 +--------- .../roseus_bt/common_argument_mapping.h | 29 +++++++++++++++++++ 2 files changed, 30 insertions(+), 15 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/common_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 index 7c4f4e3d0..e3f424a58 100644 --- a/roseus_bt/include/roseus_bt/command_line_argument_mapping.h +++ b/roseus_bt/include/roseus_bt/command_line_argument_mapping.h @@ -9,6 +9,7 @@ #include #include +#include namespace roseus_bt { @@ -60,21 +61,6 @@ bool parse_command_line(int argc, char** argv, return true; } -void register_blackboard_variables(BT::Tree* tree, - const std::map& argument_map) -{ - // New variables are registered as strings. - // However, if the port has already been registered - // in the tree, it can be default-casted. - for (const auto it: argument_map) { -#ifdef DEBUG - std::cout << "Initializing blackboard variable: " << it.first << - " with value: " << it.second << "\n"; -#endif - tree->rootBlackboard()->set(it.first, it.second); - } -} - } // namespace roseus_bt #endif // BEHAVIOR_TREE_ROSEUS_BT_COMMAND_LINE_ARGUMENT_MAPPING_ diff --git a/roseus_bt/include/roseus_bt/common_argument_mapping.h b/roseus_bt/include/roseus_bt/common_argument_mapping.h new file mode 100644 index 000000000..71f8d4ce0 --- /dev/null +++ b/roseus_bt/include/roseus_bt/common_argument_mapping.h @@ -0,0 +1,29 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_COMMON_ARGUMENT_MAPPING_ +#define BEHAVIOR_TREE_ROSEUS_BT_COMMON_ARGUMENT_MAPPING_ + +#include +#include +#include + + +namespace roseus_bt +{ + +void register_blackboard_variables(BT::Tree* tree, + const std::map& argument_map) +{ + // New variables are registered as strings. + // However, if the port has already been registered + // in the tree, it can be default-casted. + for (const auto it: argument_map) { +#ifdef DEBUG + std::cout << "Initializing blackboard variable: " << it.first << + " with value: " << it.second << "\n"; +#endif + tree->rootBlackboard()->set(it.first, it.second); + } +} + +} // namespace roseus_bt + +#endif // BEHAVIOR_TREE_ROSEUS_BT_COMMON_ARGUMENT_MAPPING_ From 603d3b2af7e2c87e849defef5281dc7fbc03c46c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 11 Nov 2022 19:52:03 +0900 Subject: [PATCH 183/195] (roseus_bt) Throttle actions/status topic for performance --- roseus_bt/euslisp/nodes.l | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/roseus_bt/euslisp/nodes.l b/roseus_bt/euslisp/nodes.l index 2d0ba587d..6e137ae48 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -38,6 +38,13 @@ (ros::subscribe (format nil "~A/cancel" ns) actionlib_msgs::GoalID #'send self :cancel-callback 50 :groupname monitor-groupname)) + + ;; Publishing the status topic is required to make a + ;; connection with the action client, but doing so on + ;; high frequencies is slow. Throttle it to 5Hz. + (ros::create-timer 0.2 #'(lambda (event) (send self :publish-status)) + :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 reactivity is required.")) @@ -58,10 +65,7 @@ (let ((res (funcall execute-cb self goal))) (when (send self :is-active) (send self :set-succeeded - (send self :result :success (not (not res))))))) - ;; TODO: publishing status at high frequencies is slow, but - ;; required to make a connection with the action client - (send self :publish-status)) + (send self :result :success (not (not res)))))))) (:set-output (name val) (let ((msg (send self :feedback (intern (string-upcase name) *keyword-package*) val))) (send msg :feedback :update_field_name name) From 047cc1ef0c477257e974533113e58dff5980bb0e Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 21 Nov 2022 20:28:32 +0900 Subject: [PATCH 184/195] add rosparam_argument_mapping.h --- roseus_bt/include/roseus_bt/gen_template.h | 12 ++-- .../roseus_bt/rosparam_argument_mapping.h | 72 +++++++++++++++++++ 2 files changed, 79 insertions(+), 5 deletions(-) create mode 100644 roseus_bt/include/roseus_bt/rosparam_argument_mapping.h diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 7f7874a0b..2048b4ce7 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -113,6 +113,7 @@ std::string GenTemplate::headers_template(std::vector headers) { #define DEBUG // rosbridgecpp logging #include #include +#include #include #include #include @@ -451,13 +452,14 @@ std::string GenTemplate::main_function_template(std::string roscpp_node_name, std::string fmt_string = 1 + R"( int main(int argc, char **argv) { - std::map init_variables; - if (!roseus_bt::parse_command_line(argc, argv, "%1%", init_variables)) { - return 1; - } - %2% ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + + std::map init_variables; + // please comment in if you want to use command line argument + // if (!roseus_bt::parse_command_line(argc, argv, "%1%", init_variables)) return 1; + if (!roseus_bt::parse_rosparam(pnh, init_variables)) return 1; BehaviorTreeFactory factory; diff --git a/roseus_bt/include/roseus_bt/rosparam_argument_mapping.h b/roseus_bt/include/roseus_bt/rosparam_argument_mapping.h new file mode 100644 index 000000000..1042ba799 --- /dev/null +++ b/roseus_bt/include/roseus_bt/rosparam_argument_mapping.h @@ -0,0 +1,72 @@ +#ifndef BEHAVIOR_TREE_ROSEUS_BT_ROSPARAM_ARGUMENT_MAPPING_ +#define BEHAVIOR_TREE_ROSEUS_BT_ROSPARAM_ARGUMENT_MAPPING_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace roseus_bt +{ + +bool parse_rosparam(ros::NodeHandle pnh, std::map& argument_map) +{ + // search ~node_name/parameter_name + std::vector param_names; + std::string n_name = pnh.resolveName("") + "/"; + pnh.getParamNames(param_names); + + for (int i = 0; i < param_names.size(); ++i) + { + std::string p_name = param_names[i]; + XmlRpc::XmlRpcValue p_value; + if (!(p_name.size() > n_name.size() && + std::equal(std::begin(n_name), std::end(n_name), std::begin(p_name)))) + { + continue; + } + bool is_get = pnh.getParam(p_name, p_value); + if (!is_get) return false; + p_name = p_name.substr(n_name.size()); + if (p_value.getType() == XmlRpc::XmlRpcValue::TypeString) + { + argument_map.insert({p_name, std::string(p_value)}); + } + else if (p_value.getType() == XmlRpc::XmlRpcValue::TypeInt) + { + argument_map.insert({p_name, std::to_string((int)p_value)}); + } + else if (p_value.getType() == XmlRpc::XmlRpcValue::TypeDouble) + { + argument_map.insert({p_name, std::to_string((double)p_value)}); + } + else if (p_value.getType() == XmlRpc::XmlRpcValue::TypeBoolean) + { + argument_map.insert({p_name, std::to_string((bool)p_value)}); + } + else if (p_value.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + std::cout << "Array param is not supported, so skipped!\n"; + std::cout << p_name << "\n"; + continue; + } + else + { + std::cerr << "Unknown param type!\n"; + std::cerr << p_name << "\n"; + return false; + } + } + return true; +} + +} // namespace roseus_bt + +#endif // BEHAVIOR_TREE_ROSEUS_BT_ROSPARAM_ARGUMENT_MAPPING_ From 5d2c79a46a5b4fab9c28393f47ef5973d8d638b0 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 21 Nov 2022 18:09:04 +0900 Subject: [PATCH 185/195] remove ros args in parse_command_line --- .../include/roseus_bt/command_line_argument_mapping.h | 9 ++++++++- 1 file changed, 8 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 e3f424a58..a81bf6170 100644 --- a/roseus_bt/include/roseus_bt/command_line_argument_mapping.h +++ b/roseus_bt/include/roseus_bt/command_line_argument_mapping.h @@ -8,6 +8,7 @@ #include #include #include +#include #include @@ -20,6 +21,10 @@ bool parse_command_line(int argc, char** argv, const std::string& program_description, std::map& argument_map) { + + std::vector argv_vec; + ros::removeROSArgs(argc, argv, argv_vec); + std::string example_description = fmt::format("example:\n {0} --arg var1 hello --arg var2 world\n", argv[0]); @@ -30,7 +35,9 @@ bool parse_command_line(int argc, char** argv, "Initial blackboard variable-value pairs"); po::positional_options_description pos; - po::parsed_options parsed_options = po::command_line_parser(argc, argv). + // when initializing from std::vector, we need to drop the program name + po::parsed_options parsed_options = po::command_line_parser( + std::vector(argv_vec.begin() + 1, argv_vec.end())). options(desc). positional(pos). style(po::command_line_style::unix_style ^ po::command_line_style::allow_short). From 37ab87d42bde5f4a9cd429e26f3968201765dd3d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Nov 2022 13:27:48 +0900 Subject: [PATCH 186/195] (roseus_bt) Timeout cancel and check result in remote actions --- .../roseus_bt/eus_remote_action_node.h | 1 - .../roseus_bt/eus_remote_condition_node.h | 1 - .../include/roseus_bt/ws_action_client.h | 23 ++++++++++++++++++- .../include/roseus_bt/ws_service_client.h | 12 +++++++++- 4 files changed, 33 insertions(+), 4 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 441e89688..f1edb7b18 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_action_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_action_node.h @@ -64,7 +64,6 @@ class EusRemoteActionNode : public BT::ActionNodeBase action_client_.cancelGoal(); } setStatus(NodeStatus::IDLE); - action_client_.waitForResult(); } protected: 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 ee15b9208..007c6ea01 100644 --- a/roseus_bt/include/roseus_bt/eus_remote_condition_node.h +++ b/roseus_bt/include/roseus_bt/eus_remote_condition_node.h @@ -59,7 +59,6 @@ class EusRemoteConditionNode : public BT::ActionNodeBase service_client_.cancelRequest(); } setStatus(NodeStatus::IDLE); - service_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 9455e5e1e..b02bdca8f 100644 --- a/roseus_bt/include/roseus_bt/ws_action_client.h +++ b/roseus_bt/include/roseus_bt/ws_action_client.h @@ -53,6 +53,10 @@ class RosbridgeActionClient void sendGoal(const rapidjson::Value& goal) { // TODO: add header and goal_id + // reset result + rapidjson::Document(rapidjson::kObjectType).Swap(result_); + result_.SetObject(); + rapidjson::Document action_goal; action_goal.SetObject(); rapidjson::Value g(goal, action_goal.GetAllocator()); @@ -66,6 +70,15 @@ class RosbridgeActionClient rapidjson::Document msg; msg.SetObject(); rbc_.publish(cancel_topic_, msg); + // check if the request has been successfully processed + for (int i=0; i<50; i++) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + if (!is_active_) { + return; + } + } + // timed out. Force is_active_ to false + is_active_ = false; } bool isActive() { @@ -73,12 +86,20 @@ class RosbridgeActionClient } rapidjson::Value getResult() { - // TODO: reset result after getting + if (!(result_.HasMember("msg") && + result_["msg"].IsObject() && + result_["msg"].GetObject().HasMember("result") && + result_["msg"].GetObject()["result"].IsObject())) { + std::string err = "Invalid remote action result at: " + server_name_; + throw BT::RuntimeError(err); + } return result_["msg"].GetObject()["result"].GetObject(); } void waitForResult() { +#ifdef DEBUG std::cout << "RemoteAction: waiting for result: " << result_topic_ << std::endl; +#endif while (is_active_) { 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 6be87babc..a2ea9a494 100644 --- a/roseus_bt/include/roseus_bt/ws_service_client.h +++ b/roseus_bt/include/roseus_bt/ws_service_client.h @@ -26,6 +26,10 @@ class RosbridgeServiceClient ~RosbridgeServiceClient() {} bool call(const rapidjson::Document& request) { + // reset result + rapidjson::Document(rapidjson::kObjectType).Swap(result_); + result_.SetObject(); + auto service_cb = std::bind(&RosbridgeServiceClient::serviceCallback, this, std::placeholders::_1, std::placeholders::_2); @@ -36,6 +40,7 @@ class RosbridgeServiceClient void cancelRequest() { // connection->send_close(1000); + is_active_ = false; } bool isActive() { @@ -43,6 +48,12 @@ class RosbridgeServiceClient } rapidjson::Value getResult() { + if (!(result_.HasMember("result") && + result_["result"].IsBool() && + result_.HasMember("values"))) { + std::string err = "Invalid remote service result at: " + service_name_; + throw BT::RuntimeError(err); + } if (!(result_["result"].GetBool())) { std::string err = "Error calling remote service: " + service_name_; if (result_["values"].IsString()) { @@ -51,7 +62,6 @@ class RosbridgeServiceClient } throw BT::RuntimeError(err); } - // TODO: reset result after getting return result_["values"].GetObject(); } From 6ea6ee9a08661415dd6c0c30fe42edb2e985683c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Nov 2022 23:22:17 +0900 Subject: [PATCH 187/195] (roseus_bt) Remove unused imports and minor style changes in argument mapping --- .../roseus_bt/command_line_argument_mapping.h | 1 - .../roseus_bt/rosparam_argument_mapping.h | 20 ++++++++----------- 2 files changed, 8 insertions(+), 13 deletions(-) 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 a81bf6170..0b33119f4 100644 --- a/roseus_bt/include/roseus_bt/command_line_argument_mapping.h +++ b/roseus_bt/include/roseus_bt/command_line_argument_mapping.h @@ -7,7 +7,6 @@ #include #include #include -#include #include #include diff --git a/roseus_bt/include/roseus_bt/rosparam_argument_mapping.h b/roseus_bt/include/roseus_bt/rosparam_argument_mapping.h index 1042ba799..3b5f44ad3 100644 --- a/roseus_bt/include/roseus_bt/rosparam_argument_mapping.h +++ b/roseus_bt/include/roseus_bt/rosparam_argument_mapping.h @@ -5,9 +5,6 @@ #include #include #include -#include -#include -#include #include #include @@ -23,17 +20,18 @@ bool parse_rosparam(ros::NodeHandle pnh, std::map& arg std::string n_name = pnh.resolveName("") + "/"; pnh.getParamNames(param_names); - for (int i = 0; i < param_names.size(); ++i) + for (std::string p_name : param_names) { - std::string p_name = param_names[i]; - XmlRpc::XmlRpcValue p_value; if (!(p_name.size() > n_name.size() && std::equal(std::begin(n_name), std::end(n_name), std::begin(p_name)))) { continue; } - bool is_get = pnh.getParam(p_name, p_value); - if (!is_get) return false; + XmlRpc::XmlRpcValue p_value; + if (!pnh.getParam(p_name, p_value)) + { + return false; + } p_name = p_name.substr(n_name.size()); if (p_value.getType() == XmlRpc::XmlRpcValue::TypeString) { @@ -53,14 +51,12 @@ bool parse_rosparam(ros::NodeHandle pnh, std::map& arg } else if (p_value.getType() == XmlRpc::XmlRpcValue::TypeArray) { - std::cout << "Array param is not supported, so skipped!\n"; - std::cout << p_name << "\n"; + std::cout << "Array param is not supported! Skipping: " << p_name << "\n"; continue; } else { - std::cerr << "Unknown param type!\n"; - std::cerr << p_name << "\n"; + std::cerr << "Unknown param type: " << p_name << "\n"; return false; } } From 899ac1acfb465e21093d59989c4a1e79dad4353d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Nov 2022 20:01:00 +0900 Subject: [PATCH 188/195] (roseus_bt) Add optional timeout parameter to the action template --- roseus_bt/include/roseus_bt/xml_parser.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index c01228e6c..552c5e490 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -812,6 +812,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_timeout_port = []() { + return fmt::format(" InputPort(\"timeout\", 500, \"{0}\")", + "timeout to connect (milliseconds)"); + }; auto format_input_port = [](const XMLElement* node) { return fmt::format(" InputPort(\"{0}\")", node->Attribute("name")); @@ -838,6 +842,7 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const std:: std::vector set_outputs; provided_input_ports.push_back(format_server_name(node)); + provided_input_ports.push_back(format_timeout_port()); for (auto port_node = node->FirstChildElement(); port_node != nullptr; From cb7c49b5e6d5906bdcd0f74c2f2ab752034a7893 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Nov 2022 22:00:39 +0900 Subject: [PATCH 189/195] (roseus_bt) Use ros::package::getPath when loading model file --- roseus_bt/include/roseus_bt/gen_template.h | 15 +++++++++++---- roseus_bt/include/roseus_bt/package_generator.h | 3 ++- roseus_bt/include/roseus_bt/xml_parser.h | 12 ++++++++---- 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h index 2048b4ce7..1c9cb52e1 100644 --- a/roseus_bt/include/roseus_bt/gen_template.h +++ b/roseus_bt/include/roseus_bt/gen_template.h @@ -40,7 +40,8 @@ class GenTemplate 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 main_function_template(std::string package_name, + std::string roscpp_node_name, std::string program_description, std::string xml_filename, std::vector register_actions, @@ -109,6 +110,7 @@ std::string GenTemplate::launch_file_template(std::vector launch_no std::string GenTemplate::headers_template(std::vector headers) { std::string fmt_string = 1 + R"( #include +#include #define DEBUG // rosbridgecpp logging #include @@ -436,7 +438,8 @@ class %1%: public EusRemoteSubscriberNode<%2%> } -std::string GenTemplate::main_function_template(std::string roscpp_node_name, +std::string GenTemplate::main_function_template(std::string package_name, + std::string roscpp_node_name, std::string program_description, std::string xml_filename, std::vector register_actions, @@ -445,8 +448,12 @@ std::string GenTemplate::main_function_template(std::string roscpp_node_name, 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_ros_filename = [package_name, xml_filename]() { + return fmt::format("fmt::format(\"{{0}}/{1}\", ros::package::getPath(\"{0}\"))", + package_name, xml_filename); + }; + auto format_create_tree = [format_ros_filename]() { + return fmt::format(" auto tree = factory.createTreeFromFile({});", format_ros_filename()); }; std::string fmt_string = 1 + R"( diff --git a/roseus_bt/include/roseus_bt/package_generator.h b/roseus_bt/include/roseus_bt/package_generator.h index 21108232c..c1ea114e7 100644 --- a/roseus_bt/include/roseus_bt/package_generator.h +++ b/roseus_bt/include/roseus_bt/package_generator.h @@ -188,6 +188,7 @@ void PackageGenerator::write_cpp_file(Parser* parser, 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); + std::string xml_file = boost::filesystem::path(xml_filename).lexically_relative(package_name).c_str(); if (boost::filesystem::exists(dest_file) && !overwrite(dest_file)) return; @@ -195,7 +196,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, program_description, - boost::filesystem::absolute(xml_filename).c_str()); + xml_file); output_file.close(); } diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 552c5e490..38281ba82 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -91,7 +91,8 @@ 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, + std::string generate_main_function(const std::string package_name, + const std::string roscpp_node_name, const std::string program_description, const std::string xml_filename); @@ -1055,7 +1056,8 @@ std::string XMLParser::generate_remote_subscriber_class(const XMLElement* node) provided_ports); } -std::string XMLParser::generate_main_function(const std::string roscpp_node_name, +std::string XMLParser::generate_main_function(const std::string package_name, + const std::string roscpp_node_name, const std::string program_description, const std::string xml_filename) { auto format_action_node = [](const XMLElement* node) { @@ -1130,7 +1132,8 @@ 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, + return gen_template.main_function_template(package_name, + roscpp_node_name, program_description, xml_filename, register_actions, @@ -1343,7 +1346,8 @@ std::string XMLParser::generate_cpp_file(const std::string package_name, output.append("\n\n"); } - output.append(generate_main_function(roscpp_node_name, program_description, xml_filename)); + output.append(generate_main_function(package_name, roscpp_node_name, program_description, + xml_filename)); return output; } From fac80bfe95b0d243d0fc2a043b3457c55b9b7753 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 22 Nov 2022 14:04:35 +0900 Subject: [PATCH 190/195] (roseus_bt) Set timeout as an argument in cancelGoal() --- roseus_bt/include/roseus_bt/ws_action_client.h | 11 +++++++++-- 1 file changed, 9 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 b02bdca8f..bc239fc13 100644 --- a/roseus_bt/include/roseus_bt/ws_action_client.h +++ b/roseus_bt/include/roseus_bt/ws_action_client.h @@ -66,18 +66,25 @@ class RosbridgeActionClient rbc_.publish(goal_topic_, action_goal); } - void cancelGoal() { + void cancelGoal(int timeout=-1) { rapidjson::Document msg; msg.SetObject(); rbc_.publish(cancel_topic_, msg); + if (timeout < 0) { + return; + } // check if the request has been successfully processed - for (int i=0; i<50; i++) { + int times = timeout/10; + for (int i=0; i Date: Fri, 25 Nov 2022 17:08:06 +0900 Subject: [PATCH 191/195] (roseus_bt) Use SubTreePlus instead of SubTree --- roseus_bt/include/roseus_bt/xml_parser.h | 1 + roseus_bt/sample/README.md | 2 +- roseus_bt/sample/models/t05_subtrees.xml | 8 ++++---- roseus_bt/sample/models/t06_reactive.xml | 10 +++++----- roseus_bt/sample/models/t07_xacro.xml.xacro | 8 ++++---- 5 files changed, 15 insertions(+), 14 deletions(-) diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h index 38281ba82..0ecb342b4 100644 --- a/roseus_bt/include/roseus_bt/xml_parser.h +++ b/roseus_bt/include/roseus_bt/xml_parser.h @@ -164,6 +164,7 @@ void XMLParser::check_xml_file(std::string filename) { name != "RemoteCondition" && name != "Subscriber" && name != "RemoteSubscriber" && + name != "SubTreePlus" && name != "SubTree") { throw XMLError::UnknownNode(node); } diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index 43e9cad30..36b272a6e 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -148,7 +148,7 @@ rosrun groot Groot 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. +Nested trees can be defined with multiple `` tags and referenced with the `` or `` tag. The `` allows for passing both port mappings and/or static values to the subtree, and should be generally preferred over the `` notation. Each subtree inherits a separate blackboard and accepts remaps in the `inner_name="outer_name"` syntax. diff --git a/roseus_bt/sample/models/t05_subtrees.xml b/roseus_bt/sample/models/t05_subtrees.xml index 3b6787b1b..745a24dbf 100644 --- a/roseus_bt/sample/models/t05_subtrees.xml +++ b/roseus_bt/sample/models/t05_subtrees.xml @@ -6,8 +6,8 @@ - - + + @@ -37,8 +37,8 @@ - - + + diff --git a/roseus_bt/sample/models/t06_reactive.xml b/roseus_bt/sample/models/t06_reactive.xml index 68d056e81..a5c996f42 100644 --- a/roseus_bt/sample/models/t06_reactive.xml +++ b/roseus_bt/sample/models/t06_reactive.xml @@ -9,9 +9,9 @@ - + - + @@ -39,10 +39,10 @@ - + - - + + diff --git a/roseus_bt/sample/models/t07_xacro.xml.xacro b/roseus_bt/sample/models/t07_xacro.xml.xacro index a7c5cd7b5..7111559e5 100644 --- a/roseus_bt/sample/models/t07_xacro.xml.xacro +++ b/roseus_bt/sample/models/t07_xacro.xml.xacro @@ -14,8 +14,8 @@ - - + + @@ -25,8 +25,8 @@ - - + + From cc3048676bc3c6211572ba4118f8a322e76fdf91 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 16 Feb 2023 11:45:33 +0900 Subject: [PATCH 192/195] (roseus_bt) Use ros::ok in roseus_bt:spin --- 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 6e137ae48..02d0154f7 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -110,6 +110,6 @@ (send ac :execute-cb))) (defun spin () - (while t + (while (ros::ok) (spin-once) (ros::sleep))) From bff5b6079208af2ec4cf2b35ca1ce8208c67e53e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 16 Feb 2023 16:23:05 +0900 Subject: [PATCH 193/195] (roseus_bt) Fix tutorial SubTreePlus port remapping --- roseus_bt/sample/models/t05_subtrees.xml | 15 +++++++++------ roseus_bt/sample/models/t06_reactive.xml | 4 ++-- roseus_bt/sample/models/t07_xacro.xml.xacro | 4 ++-- .../sample/models/t07_xacro_sweep_task.xml.xacro | 8 +++++--- 4 files changed, 18 insertions(+), 13 deletions(-) diff --git a/roseus_bt/sample/models/t05_subtrees.xml b/roseus_bt/sample/models/t05_subtrees.xml index 745a24dbf..8dfa43e1c 100644 --- a/roseus_bt/sample/models/t05_subtrees.xml +++ b/roseus_bt/sample/models/t05_subtrees.xml @@ -6,7 +6,7 @@ - + @@ -30,14 +30,18 @@ - - - + + + + + - + + + @@ -60,4 +64,3 @@ - diff --git a/roseus_bt/sample/models/t06_reactive.xml b/roseus_bt/sample/models/t06_reactive.xml index a5c996f42..dd0c01721 100644 --- a/roseus_bt/sample/models/t06_reactive.xml +++ b/roseus_bt/sample/models/t06_reactive.xml @@ -11,7 +11,7 @@ - + @@ -40,7 +40,7 @@ - + diff --git a/roseus_bt/sample/models/t07_xacro.xml.xacro b/roseus_bt/sample/models/t07_xacro.xml.xacro index 7111559e5..4aa1bee9b 100644 --- a/roseus_bt/sample/models/t07_xacro.xml.xacro +++ b/roseus_bt/sample/models/t07_xacro.xml.xacro @@ -6,7 +6,7 @@ - + @@ -14,7 +14,7 @@ - + diff --git a/roseus_bt/sample/models/t07_xacro_sweep_task.xml.xacro b/roseus_bt/sample/models/t07_xacro_sweep_task.xml.xacro index 91d4a940a..d22e74c47 100644 --- a/roseus_bt/sample/models/t07_xacro_sweep_task.xml.xacro +++ b/roseus_bt/sample/models/t07_xacro_sweep_task.xml.xacro @@ -10,9 +10,11 @@ - - - + + + + + From e34aa5e9bbbcb325bd5d61dbb6540163e699441b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 16 Feb 2023 16:23:28 +0900 Subject: [PATCH 194/195] (roseus_bt) Update sample readme --- roseus_bt/sample/README.md | 33 ++++++++++++++------------------- 1 file changed, 14 insertions(+), 19 deletions(-) diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index 36b272a6e..899573a29 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -26,8 +26,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 +roslaunch roseus_bt_tutorials t01_simple_tree_server.launch ``` Run the cpp client @@ -35,11 +34,12 @@ Run the cpp client rosrun roseus_bt_tutorials t01_simple_tree ``` -Optionally run Groot for visualization +Optionally run Groot. Select the Monitor mode for real-time visualization of the ongoing task, or the Interpreter mode for interactive evaluation. In the Interpreter mode, the cpp client is not needed. ```bash rosrun groot Groot ``` + ## t02_conditions ![t02](https://user-images.githubusercontent.com/20625381/125036852-707c2100-e0ce-11eb-99b8-a8d568e6e97c.gif) @@ -51,8 +51,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 +roslaunch roseus_bt_tutorials t02_conditions_server.launch ``` Run the cpp client @@ -86,8 +85,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 +roslaunch roseus_bt_tutorials t03_ports_server.launch ``` Run the cpp client @@ -107,13 +105,18 @@ 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. +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 initialized with an instance of the given message type and updated every time a new message is received. The `received_port` variable is a boolean initialized with false and set to true at every new message. Only proper ROS message types are supported by subscriber nodes (e.g. `std_msgs/Int64` instead of `int64`). #### Run the code +Run the roseus server +```bash +roslaunch roseus_bt_tutorials t04_subscriber_server.launch +``` + Publish the topic: ```bash rostopic pub -r 10 /petbottle/coords geometry_msgs/Pose "position: @@ -127,12 +130,6 @@ orientation: 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 @@ -156,8 +153,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 +roslaunch roseus_bt_tutorials t05_subtrees_server.launch ``` Run the cpp client @@ -177,7 +173,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. -In order to compose a reactive program in single-threded eus, we need to ensure that: +In order to compose a reactive program in single-threaded eus, we need to ensure that: 1. Condition nodes can be evaluated while executing actions 2. Interruption requests are checked while executing actions @@ -192,8 +188,7 @@ 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 +roslaunch roseus_bt_tutorials t06_reactive_server.launch ``` Run the roseus condition server From 98abcd2f95c7da691a03959097d2df18bfd075fb Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 16 Feb 2023 17:32:32 +0900 Subject: [PATCH 195/195] (roseus_bt) Update links to jsk-ros-pkg --- roseus_bt/README.md | 7 ++++--- roseus_bt/roseus_bt.rosinstall | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/roseus_bt/README.md b/roseus_bt/README.md index 237bd7d5e..b7fe3d3de 100644 --- a/roseus_bt/README.md +++ b/roseus_bt/README.md @@ -16,10 +16,11 @@ 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 +wget https://raw.githubusercontent.com/jsk-ros-pkg/jsk_roseus/master/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 @@ -42,8 +43,8 @@ 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). +For more information on how to compose the model file and use the package check the [tutorials](https://github.com/jsk-ros-pkg/jsk_roseus/master/roseus_bt/sample) and the [BehaviorTree.CPP documentation](https://www.behaviortree.dev/docs/3.8/intro). ## Samples -Follow instructions at the [tutorial page](https://github.com/Affonso-Gui/jsk_roseus/tree/roseus_bt/roseus_bt/sample). +Follow instructions at the [tutorial page](https://github.com/jsk-ros-pkg/jsk_roseus/master/roseus_bt/sample). diff --git a/roseus_bt/roseus_bt.rosinstall b/roseus_bt/roseus_bt.rosinstall index 188a54ba1..3f441eed1 100644 --- a/roseus_bt/roseus_bt.rosinstall +++ b/roseus_bt/roseus_bt.rosinstall @@ -10,5 +10,5 @@ - git: local-name: Groot - uri: https://github.com/BehaviorTree/Groot.git - version: master + uri: https://github.com/Affonso-Gui/Groot + version: roseus_bt