From ba636be29b474819189e47f0879e5b1dc74cdefe Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 15:40:25 +0900 Subject: [PATCH 001/164] 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 126683fb66f8e45cb10d2fd4d6615a51f7e8b77e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 15:55:32 +0900 Subject: [PATCH 002/164] (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 ece138efb18563a421f551d8492bcd1904e902b8 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 15:56:42 +0900 Subject: [PATCH 003/164] (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 3a74a072ff3ac89fcf8a5b3ea4a5dae060562a92 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 15:57:40 +0900 Subject: [PATCH 004/164] (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 ab76394b4e37ad311cca87c2ea95d1f95f84f105 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 18:02:46 +0900 Subject: [PATCH 005/164] (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 9c9a8e19a2c24499ed48477e0ee110b57e3efcd7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 19:13:31 +0900 Subject: [PATCH 006/164] (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 f1dc08a2ab0ed18ab938a5c320ebae79489c2dfe Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 20:55:11 +0900 Subject: [PATCH 007/164] (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 1157cda867079daacbe404a03e1d2e007c310c79 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 20:57:34 +0900 Subject: [PATCH 008/164] (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 05592d811b2ea77bae3c608de289b53e406abca2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 21:04:55 +0900 Subject: [PATCH 009/164] (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 cc3466c0687df1b6df7f1644c2c1cfc19c5a1efb Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 16 Jun 2021 21:14:51 +0900 Subject: [PATCH 010/164] (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 0450ccb28b2c897b82747960250800a58dded49d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 11:11:07 +0900 Subject: [PATCH 011/164] (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 885b32c073a170010190388009a09d3bc33c35ac Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 12:56:52 +0900 Subject: [PATCH 012/164] (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 f8705a4a1c5ea3d941e43cdca37b7a7805bb49b9 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 16:44:03 +0900 Subject: [PATCH 013/164] (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 94994513b85741efb576c5c3ed8d005324845d92 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 17:02:44 +0900 Subject: [PATCH 014/164] (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 7bc0ac4d8b52bce8738943a0b6b54f2c9f1962b8 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 17:48:30 +0900 Subject: [PATCH 015/164] (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 6476df2c9acd19102c662b267f87406137d7fbec Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 17:49:17 +0900 Subject: [PATCH 016/164] (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 b88633f9482381aa2b33368f784124f1e6c5c810 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 17 Jun 2021 20:09:16 +0900 Subject: [PATCH 017/164] (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 bfc608c78f72f1cb9740ac22abf5143d5a55fe39 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 18 Jun 2021 13:20:23 +0900 Subject: [PATCH 018/164] (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 ce849dc9d1f54d73bf90f4ecd3bb04e80ade22a0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 18 Jun 2021 13:44:37 +0900 Subject: [PATCH 019/164] (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 1a7086a4eba1330e37c7b77d557237cd62d47435 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 18 Jun 2021 15:56:38 +0900 Subject: [PATCH 020/164] (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 cb61d50c5e50202236e63f470386a0e205e1a34b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 11:11:26 +0900 Subject: [PATCH 021/164] (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 e466a7fac69c0fa940544446e1fe138340576efd Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 11:12:37 +0900 Subject: [PATCH 022/164] (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 77b300b51c8a6475b634b4655d63481f8033ece7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 13:16:53 +0900 Subject: [PATCH 023/164] (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 1e33fb5bb74edaf4ed87465fbd87b1c26d71ea20 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 14:37:40 +0900 Subject: [PATCH 024/164] (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 ca34f7a58f6ae7f35bb0e5bf4b99c16faef1c5f4 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 14:44:08 +0900 Subject: [PATCH 025/164] (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 3ba47f192fdbe1d676bcb5324db6e42865266275 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 14:47:52 +0900 Subject: [PATCH 026/164] (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 bf26571064ef4223d0380c157acae0451075d26d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 17:28:15 +0900 Subject: [PATCH 027/164] (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 9be78d7a0b3e4ed3ad1539c7a99c6edb90141bb6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 17:47:39 +0900 Subject: [PATCH 028/164] (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 b4e7fb4ffad72f7b05d680462816ebc656a212a2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 19:05:11 +0900 Subject: [PATCH 029/164] (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 7cd9bf910836cc7c93e12ebd43faed3261a4d328 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 20 Jun 2021 19:11:56 +0900 Subject: [PATCH 030/164] (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 902158f64915fe03738fbc75c23557159d11e33d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Jun 2021 13:49:24 +0900 Subject: [PATCH 031/164] (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 a821fa0303b773b4ae6a0f42bdb2d05b3433787e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Jun 2021 13:50:22 +0900 Subject: [PATCH 032/164] (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 781309ef40214e9f30e26132f5b22051a827e577 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 14:17:03 +0900 Subject: [PATCH 033/164] (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 c7f615516f332d4a107cef0be93918ccc7ff8ed6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 14:29:27 +0900 Subject: [PATCH 034/164] (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 95422cf0480b502cd9e3d4727742cefb6120fc3e Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 14:31:25 +0900 Subject: [PATCH 035/164] (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 bad10a7999f015cb8d85f0dbae612b959e78f789 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 14:46:47 +0900 Subject: [PATCH 036/164] (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 27c0631a237d5771d73bc0cb361fc105671a3950 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 15:43:39 +0900 Subject: [PATCH 037/164] (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 ab0511bfd2b5b7de2f0902ea96df3f6413f25286 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 30 Jun 2021 15:50:07 +0900 Subject: [PATCH 038/164] (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 cbb489ff67a3b0c35e645a3f1525cea0d3ceb736 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 6 Jul 2021 16:29:41 +0900 Subject: [PATCH 039/164] (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 68d377bef9ed2298e28edc251f45fddec64a8084 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 6 Jul 2021 20:03:21 +0900 Subject: [PATCH 040/164] (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 59bddd6b98c5de231f998b61364e3dc1a8caa854 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 6 Jul 2021 20:04:19 +0900 Subject: [PATCH 041/164] (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 fb280c132753187dbd4d2f17df72091a1533cdc3 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 19:44:04 +0900 Subject: [PATCH 042/164] (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 d693401052cac1286099dc992e0a5591d1967817 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 19:44:27 +0900 Subject: [PATCH 043/164] (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 eb52e53154775596a20e11158dc46b56d587f268 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 20:58:17 +0900 Subject: [PATCH 044/164] (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 343c65d44b0d65adaf547085ab52f272e8361a18 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:01:02 +0900 Subject: [PATCH 045/164] (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 f772296d21af6c19e29e4d51fa4a1abce5d3498c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:15:53 +0900 Subject: [PATCH 046/164] (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 c1459a71003e08d1c3ad2060f7c246935d5811e6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:17:47 +0900 Subject: [PATCH 047/164] (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 c2c20a84d31c35d7157e248b45ea1135bf014d5c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:32:52 +0900 Subject: [PATCH 048/164] (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 1eea29f5a821a39e4757827f24b76d6eb029cea9 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:33:34 +0900 Subject: [PATCH 049/164] (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 c3bffa7756c381a0d229ebf38ae83d70c1b50436 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 7 Jul 2021 21:45:37 +0900 Subject: [PATCH 050/164] (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 39445fa32470a150b102e58f8244369a11c0bd3f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 13:55:23 +0900 Subject: [PATCH 051/164] (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 b16da76fc3db2a8aa06b32ec9b38bff3980b376d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 14:03:23 +0900 Subject: [PATCH 052/164] (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 b1bc0fe8325d29041ffc08d09b1c9cb155e9fafd Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 14:03:36 +0900 Subject: [PATCH 053/164] (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 000dd51fe5c20aa016408a97521e9c1d543ed2be Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 14:04:00 +0900 Subject: [PATCH 054/164] (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 36b30db84161393598119b94a52130d17b5661b5 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 19:17:22 +0900 Subject: [PATCH 055/164] (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 d4e791971292d32edaca7baa246fbc236b68b865 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 19:20:57 +0900 Subject: [PATCH 056/164] (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 d79cdc61bdda65984f9af133903cfd1b8b17ae7d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 20:41:08 +0900 Subject: [PATCH 057/164] (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 87a36b6d209b95a94a32969aa76c64742f459d7c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 8 Jul 2021 22:01:38 +0900 Subject: [PATCH 058/164] (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 44df06ccf3ac9de07d234a3ed83925f5e559fda3 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 9 Jul 2021 16:10:04 +0900 Subject: [PATCH 059/164] (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 c370e30eb4fbb2154d6d69f4a6c503abf9348986 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 9 Jul 2021 18:10:26 +0900 Subject: [PATCH 060/164] (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 e5db8994e2545d6f9e5d37dbb3baabb1f4996401 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 9 Jul 2021 23:40:15 +0900 Subject: [PATCH 061/164] (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 3f974d6282a4f647ec9daa2d34772bb2fb5e942c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 01:51:46 +0900 Subject: [PATCH 062/164] (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 95e81651243626dfcec56869c8d1d91c2d4ff057 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 10:42:13 +0900 Subject: [PATCH 063/164] (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 be7ab31e8bf92d05ca14c3a63614024f419d285b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 11:22:08 +0900 Subject: [PATCH 064/164] (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 9f0f9505f200401a50e760f501e3ed81b09a36db Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 11:26:34 +0900 Subject: [PATCH 065/164] (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 2f803d3f0edd872b376bd9f123fb299472fbf95a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 10 Jul 2021 11:56:54 +0900 Subject: [PATCH 066/164] (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 65d3197c898edaee488f4bbb26eeae7f2cc233dc Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 12 Jul 2021 11:52:12 +0900 Subject: [PATCH 067/164] (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 fd92d438eba7c5ae7d6b49926f1a56033ebeb8d3 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 12 Jul 2021 13:40:21 +0900 Subject: [PATCH 068/164] (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 27a78560175c8783b1f0331034ae7d504ecaaf3a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 12 Jul 2021 13:50:03 +0900 Subject: [PATCH 069/164] (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 1597bdf6cfb54968847c99945edd41f18a75bbc1 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 13 Aug 2021 15:51:00 +0900 Subject: [PATCH 070/164] (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 8ccc544086217b52d659bdcea68d898099612826 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 13 Aug 2021 17:03:10 +0900 Subject: [PATCH 071/164] (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 4948e5500c971a0a0d60713c0a274aa0255a6df2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 16 Aug 2021 13:48:49 +0900 Subject: [PATCH 072/164] (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 8838b28c20e5e6a02cb8768d5df7062ceb583783 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 16 Aug 2021 15:13:34 +0900 Subject: [PATCH 073/164] (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 40ce988be7d610b413931d509eb2635655ca8d37 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 16 Aug 2021 16:20:51 +0900 Subject: [PATCH 074/164] (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 e28f234d69dfaa18775d40c37a3eeb3739345fe6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 24 Aug 2021 14:08:12 +0900 Subject: [PATCH 075/164] (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 2409bce2f2efc4e13e946ea6289e54154f264896 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 12 Sep 2022 17:10:39 +0900 Subject: [PATCH 076/164] (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 63f645856c7302a5c68a25a7da33e1e60896cdb5 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 21 Sep 2021 21:59:42 +0900 Subject: [PATCH 077/164] (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 8105c810121d608f781749ec3e421a164f98894d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 22 Sep 2021 16:43:39 +0900 Subject: [PATCH 078/164] (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 8045e9cd5dbdf735dddf4e1fc88e63a4720fae3d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 22 Sep 2021 17:02:37 +0900 Subject: [PATCH 079/164] (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 85830594d8fbe58e21a49a84847464c70b14af53 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 8 Oct 2021 16:32:34 +0900 Subject: [PATCH 080/164] (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 bfa30017ca3a0460a0244635519ca6e27f5db52c Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 8 Oct 2021 16:40:28 +0900 Subject: [PATCH 081/164] (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 d1bc75761bfe3a420b797f275e0742e40080f7a7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 8 Oct 2021 16:59:34 +0900 Subject: [PATCH 082/164] (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 e733a9c61f2f32015b7510e9a58b6fecf36b1404 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 8 Oct 2021 17:04:38 +0900 Subject: [PATCH 083/164] (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 14e4aa249f16bd7489cdb7ec242091a09c0c896a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 22 Sep 2021 15:30:09 +0900 Subject: [PATCH 084/164] (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 32a8b39f22ea6a9d297a9104f16be12de940b1c2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 24 Feb 2022 14:20:46 +0900 Subject: [PATCH 085/164] (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 8676dba99635598042e972ca1558e617507298f2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 13:29:09 +0900 Subject: [PATCH 086/164] (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 f51eac51c026e2ff25ca49e304c6fa0a134081c9 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 17:35:41 +0900 Subject: [PATCH 087/164] (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 a911c0372f7de70a0648171d09aca5e6724cedc2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 17:39:40 +0900 Subject: [PATCH 088/164] (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 d663eb6d880f75957b209d24d52a4c51d37db839 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 17:41:36 +0900 Subject: [PATCH 089/164] (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 737a1ffdca8bab513aff77ac0dfbdf872f56dcb9 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 20:24:57 +0900 Subject: [PATCH 090/164] (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 208884ed7cc12ddf5906e08f015fa223ca0d8939 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 23:36:21 +0900 Subject: [PATCH 091/164] (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 198c8c5783b60ff04cc2604903d4cbad4a7c236d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 18 Apr 2022 23:38:56 +0900 Subject: [PATCH 092/164] (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 18bb4b3605c915f29409929a11b54d2378d75ee2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 19 Apr 2022 07:37:18 +0900 Subject: [PATCH 093/164] (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 6e288616751d635837508820fd7ffa07bd405475 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 19 Apr 2022 08:28:22 +0900 Subject: [PATCH 094/164] (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 1596dfea99a5587daf4683a4fb81c0d4808e9fe9 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 21 Apr 2022 17:39:43 +0900 Subject: [PATCH 095/164] (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 f3b208aba7a4f9923016696c5f59592f1bcaecf0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 21 Apr 2022 21:02:39 +0900 Subject: [PATCH 096/164] (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 c59c4bd5a250b3043f3daa0e53ad7037579fc6fa Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 21 Apr 2022 21:35:14 +0900 Subject: [PATCH 097/164] (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 1d90c683b0da74362faedb284efdec3b243b39c8 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 13:50:24 +0900 Subject: [PATCH 098/164] (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 ff65a78e037cf5f3cc06bb45ac98affbe5b260be Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 13:53:23 +0900 Subject: [PATCH 099/164] (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 f143e74e56e6d7768df5ca9e3763cd8485dd4bf1 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 14:59:14 +0900 Subject: [PATCH 100/164] (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 d6ef47adc6b37786df5ea1d4e4eacd42bdfd0fc8 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 17:24:49 +0900 Subject: [PATCH 101/164] (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 06fab195ea3945d2e0f30850ea914b8bd74c1723 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 17:25:18 +0900 Subject: [PATCH 102/164] (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 5d50b72042a1822a544c70ed70a1b274bb6854e3 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 17:39:46 +0900 Subject: [PATCH 103/164] (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 e80a9e99c3aaba6ffa397a1f15850638645daeaa Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 18:47:59 +0900 Subject: [PATCH 104/164] (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 3db2c3d5bc117d498e2f1cbf0d3165654f6b4146 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 19:21:57 +0900 Subject: [PATCH 105/164] (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 270e90b882a121a2f49d740cd73b17c06a364f23 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 19:30:18 +0900 Subject: [PATCH 106/164] (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 f7669e00c041463c3c77748c97957db13e7ccb73 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 20:04:58 +0900 Subject: [PATCH 107/164] (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 c959c6ccd8e091eb6bbf24ee7253841dcf5bd975 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 21:31:22 +0900 Subject: [PATCH 108/164] (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 481c7e6f90090afbb1366fba0da4dab09e096cc6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 21:48:21 +0900 Subject: [PATCH 109/164] (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 7ca191b7aba076e25d92e4006496a7dd4aa04ef2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 25 Apr 2022 22:26:05 +0900 Subject: [PATCH 110/164] (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 4b9c6d973215140d2173dba0b1065da1c86540fb Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 11:56:21 +0900 Subject: [PATCH 111/164] (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 4bee738c7ed373a18a9f3c3cf8488dc693b74be7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 12:07:03 +0900 Subject: [PATCH 112/164] (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 1744d507647975d1f2e1d1f5c4506d222344b975 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 12:18:02 +0900 Subject: [PATCH 113/164] (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 6a90f15f05712a540d78f258e9851850d059b9a7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 13:36:36 +0900 Subject: [PATCH 114/164] (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 daa8747d6c81d565c46fb195be2900dc2441d53d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 26 Apr 2022 14:06:41 +0900 Subject: [PATCH 115/164] (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 778ee2b9af4347c5393398dea2b3c6292fdc4e30 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 27 Apr 2022 20:39:44 +0900 Subject: [PATCH 116/164] (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 b9c5f075bb96d654f02d569ba05552eb859fefc5 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 28 Apr 2022 11:18:17 +0900 Subject: [PATCH 117/164] (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 613a6ac2eb79c8d688be48a6a7fb9d37f834314d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 28 Apr 2022 11:28:32 +0900 Subject: [PATCH 118/164] (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 96cb683616e94fd71c342156484793229a86a962 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 28 Apr 2022 15:42:54 +0900 Subject: [PATCH 119/164] (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 d1942b31a5eaa4389078cdde6314ef8417aee418 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 28 Apr 2022 23:08:45 +0900 Subject: [PATCH 120/164] (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 18ceee02eaa03aad991218f205cab9a7569e3441 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 29 Apr 2022 00:10:36 +0900 Subject: [PATCH 121/164] (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 27009c9775066e3b2ff3e9ced05a9b0f0db31e10 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 21:30:12 +0900 Subject: [PATCH 122/164] (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 4968ad560..e6c992e28 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 ed427949dc4d69c2f0c56067a3ce2a4472913991 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 21:38:30 +0900 Subject: [PATCH 123/164] (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 2cd501a2f54c1b7b03bfb8e04b937281668b82c3 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 23:17:39 +0900 Subject: [PATCH 124/164] (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 159895510c03e44f05489a225e41cefd918225c0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 23:21:11 +0900 Subject: [PATCH 125/164] (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 337613599..4e42b1d28 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 4da49e2e0096a65c724d55d7e5db81aaeaf1cbda Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 27 May 2022 23:31:50 +0900 Subject: [PATCH 126/164] (roseus_bt) Update the subscriber documentation --- roseus_bt/sample/README.md | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md index 04a1e5b95..040497479 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -107,10 +107,9 @@ The fourth example https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/rose Such port variables are initialized with an empty message instance and updated every time a new topic message arrives. -To do this we add an action with the `topic_name` and `output_port` fields in the `` section, and declare it as a `` and specify `message_type` and optionally `message_field` in the `` section. -Only proper ROS message types are supported by subscriber nodes (e.g. `std_msgs/Int64` instead of `int64`). +To do this we add a `` node, specifying the input ports `topic_name` and `message_type` and the output ports `output_port` and `received_port`. The `output_port` variable is initilized with an instance of the given message type and updated every time a new message is received. The `received_port` variable is a boolean initialized with false and set to true at every new message. Optionally, `message_field` can also be assigned. -Note how we also add a step to verify and wait for messages. +Only proper ROS message types are supported by subscriber nodes (e.g. `std_msgs/Int64` instead of `int64`). #### Run the code @@ -178,7 +177,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. +Because in such scenario the condition nodes must be evaluated alongside the running action, we prepare two distinct roseus servers -- one for actions and the other for conditions. On the action side it is also necessary to check for the presence of interruption requests with the `(roseus_bt:ok)` function. #### Run the code From d1800ffe59bc7500938158dcfd7d071caba70844 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 28 May 2022 00:45:20 +0900 Subject: [PATCH 127/164] (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 040497479..b9f11b22e 100644 --- a/roseus_bt/sample/README.md +++ b/roseus_bt/sample/README.md @@ -235,11 +235,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: @@ -250,8 +254,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 3cdfbb549d2618f2837460f6581cf9275f5f98cc Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 26 Sep 2022 18:21:34 +0900 Subject: [PATCH 128/164] (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 ab0d723b0bfb15fb8865578de2ed56e7843dd9cb Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 10 Oct 2022 12:58:23 +0900 Subject: [PATCH 129/164] (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 1369d4a136089fc1c52911cdc1f5e86d18fb4d8a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 10 Oct 2022 23:36:44 +0900 Subject: [PATCH 130/164] (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 dee8c0590e0101c904a3cd9b59da62fed11330ce Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 20 Oct 2022 16:25:36 +0900 Subject: [PATCH 131/164] (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 71f15f89391ad829cb06f6f21f3cfcb80e30de87 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 20 Oct 2022 18:19:33 +0900 Subject: [PATCH 132/164] (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 b9f11b22e..9eeedb823 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 @@ -185,13 +185,13 @@ On the action side it is also necessary to check for the presence of interruptio 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 @@ -249,7 +249,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: @@ -263,7 +263,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 3b4af6541717b9a96390d0ac15924281c9a4e38a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 10:45:56 +0900 Subject: [PATCH 133/164] (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 d30d92c392600d8699196ae0e53ec70c4b5a4527 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 13:15:08 +0900 Subject: [PATCH 134/164] (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 e9afe58abb408bde5d65508587af66b756e365b4 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 19:48:17 +0900 Subject: [PATCH 135/164] (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 4bf6c951993d4f340d80c73d7bd983fe5463d98d Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 21:31:50 +0900 Subject: [PATCH 136/164] (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 4e42b1d28..60acfc35f 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 d1783d127effe4362f33a30fb6e281d0e99a300a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 22:31:22 +0900 Subject: [PATCH 137/164] (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 b01a68efd7b6a6d36ff2a34ec4d2d71397075709 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Oct 2022 22:32:32 +0900 Subject: [PATCH 138/164] (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 87f04156831d2028786f92904073719cdc996326 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sat, 22 Oct 2022 16:21:33 +0900 Subject: [PATCH 139/164] (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 6fcf2b74986a4f39c994c8e7d0725aa3dfc5e03b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 10:02:57 +0900 Subject: [PATCH 140/164] (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 e060aec61d130c8313bd84205db051603e2d0f60 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 12:27:09 +0900 Subject: [PATCH 141/164] (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 6e9e6f8f840ad54edb294bc775aa22433af9bffa Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 15:26:45 +0900 Subject: [PATCH 142/164] (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 32c1b461ab62eac649244bcf3ba1676998a74501 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 17:13:12 +0900 Subject: [PATCH 143/164] (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 d7d3045211504f24046507bdfcce276457a27ce2 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 20:06:55 +0900 Subject: [PATCH 144/164] (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 9eeedb823..7973138a1 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 16160ab48c122a6d1d3c93fa8f3e0c9d4fb4e6d0 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Sun, 23 Oct 2022 22:24:08 +0900 Subject: [PATCH 145/164] (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 afdd32542e5e42d5f0e577004ebc9a369d688e65 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 25 Oct 2022 19:32:57 +0900 Subject: [PATCH 146/164] (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 b24d3a31d80effd016731db6bb5f159b7a259234 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 25 Oct 2022 23:08:38 +0900 Subject: [PATCH 147/164] (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 a79dd92c4b1d40493c373df20474442fe3f7e290 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 26 Oct 2022 02:04:08 +0900 Subject: [PATCH 148/164] (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 6635d14b9d9a7c6d7ef7d7104c840b16969541c5 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 26 Oct 2022 02:04:32 +0900 Subject: [PATCH 149/164] (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 bf49aa193b489c800ba1cf9e84c34842152f6797 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 7 Nov 2022 11:47:34 +0900 Subject: [PATCH 150/164] (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 90f30747ef677a39b9955f9f29ca8a1e54b1b8e7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 11 Nov 2022 14:31:18 +0900 Subject: [PATCH 151/164] (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 c2da0cc5286602c40a3643039fddd96487e1f3b7 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 14 Nov 2022 10:19:41 +0900 Subject: [PATCH 152/164] (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 a27c275bebb14db51b78699ad281dcd3c8cb3a4a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 15 Nov 2022 14:10:49 +0900 Subject: [PATCH 153/164] (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 3ba0a247446fd35b73817e0657e9adab502076bb Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 15 Nov 2022 21:58:39 +0900 Subject: [PATCH 154/164] (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 60680a65a2ef72b30e3f63125607a1e2336dd1ef Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 11 Nov 2022 19:52:03 +0900 Subject: [PATCH 155/164] (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 e6c992e28..777d13a63 100644 --- a/roseus_bt/euslisp/nodes.l +++ b/roseus_bt/euslisp/nodes.l @@ -62,6 +62,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.")) @@ -82,10 +89,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 f56ce1b015358b2982077bf813507eb250c04bcc Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Nov 2022 13:27:48 +0900 Subject: [PATCH 156/164] (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 1134d32cfe85d91240ee4a373f8c4f068123a736 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 21 Nov 2022 18:09:04 +0900 Subject: [PATCH 157/164] 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 7c4f4e3d0..484275c0c 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 namespace roseus_bt @@ -19,6 +20,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]); @@ -29,7 +34,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 b4b52b8c032d96a69a16a77065db62d1ca2eb7af Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 21 Nov 2022 18:31:33 +0900 Subject: [PATCH 158/164] 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 484275c0c..a81bf6170 100644 --- a/roseus_bt/include/roseus_bt/command_line_argument_mapping.h +++ b/roseus_bt/include/roseus_bt/command_line_argument_mapping.h @@ -10,6 +10,7 @@ #include #include +#include namespace roseus_bt { @@ -67,21 +68,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 4fb75ed36a3bb89667d8859a783a98c6ab83d004 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 21 Nov 2022 20:28:32 +0900 Subject: [PATCH 159/164] 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 914eedfa9278323a173b7bae73551e19c2471afd Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Nov 2022 23:22:17 +0900 Subject: [PATCH 160/164] (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 75eaf4e8e6c5c843faf9f79e878ee5530ca2a650 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Nov 2022 20:01:00 +0900 Subject: [PATCH 161/164] (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 dbb24535838595d340c45db98a7ba1d1f50ef9f6 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Mon, 21 Nov 2022 22:00:39 +0900 Subject: [PATCH 162/164] (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 53b82f0fc80ef981aaa4e7f27258d4959cc0a51a Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Tue, 22 Nov 2022 14:04:35 +0900 Subject: [PATCH 163/164] (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 164/164] (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 7973138a1..1a332c519 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 @@ - - + +