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