From 3c5bb9b485930773911b834943eed6e722c42eed Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Wed, 31 Jan 2024 11:06:39 +0100 Subject: [PATCH 01/44] feat: allows message clearing behaviour to be customised by the implementer --- .../behaviortree_ros2/bt_topic_sub_node.hpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 311594b..7ea9fc6 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -173,6 +173,16 @@ class RosTopicSubNode : public BT::ConditionNode */ virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; + /** Clear the message that has been processed. If returns true and no new message is + * received, before next call there will be no message to process. If returns false, + * the next call will process the same message again, if no new message received. + * + * This can be equated with latched vs non-latched topics in ros 1. + * + * @return true will clear the message after ticking/processing. + */ + virtual bool clear_processed_message() { return true; } + private: bool createSubscriber(const std::string& topic_name); @@ -294,8 +304,10 @@ template inline }; sub_instance_->callback_group_executor.spin_some(); auto status = CheckStatus (onTick(last_msg_)); - last_msg_ = nullptr; - + if (clear_processed_message()) + { + last_msg_ = nullptr; + } return status; } From ac2e0ac6938ad97e23973541b6d32c0396fc571c Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Fri, 2 Feb 2024 13:53:23 +0100 Subject: [PATCH 02/44] style: match camel case function name --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 7ea9fc6..a2ed0d4 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -181,7 +181,7 @@ class RosTopicSubNode : public BT::ConditionNode * * @return true will clear the message after ticking/processing. */ - virtual bool clear_processed_message() { return true; } + virtual bool clearProcessedMessage() { return true; } private: @@ -304,7 +304,7 @@ template inline }; sub_instance_->callback_group_executor.spin_some(); auto status = CheckStatus (onTick(last_msg_)); - if (clear_processed_message()) + if (clearProcessedMessage()) { last_msg_ = nullptr; } From d31eb1fc2833d0bacbdecfb04206e5ff86273b92 Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Tue, 2 Apr 2024 13:43:27 +0200 Subject: [PATCH 03/44] fix: Get last message on late subscription and ensure connection to correct subscriber on topic_name change --- .../behaviortree_ros2/bt_topic_sub_node.hpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index a2ed0d4..8d3667c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -66,6 +66,7 @@ class RosTopicSubNode : public BT::ConditionNode // The callback will broadcast to all the instances of RosTopicSubNode auto callback = [this](const std::shared_ptr msg) { + last_msg = msg; broadcaster(msg); }; subscriber = node->create_subscription(topic_name, 1, callback, option); @@ -75,6 +76,7 @@ class RosTopicSubNode : public BT::ConditionNode rclcpp::CallbackGroup::SharedPtr callback_group; rclcpp::executors::SingleThreadedExecutor callback_group_executor; boost::signals2::signal)> broadcaster; + std::shared_ptr last_msg = nullptr; }; @@ -270,6 +272,11 @@ template inline sub_instance_ = it->second; } + // Check if there was a message received before the creation of this subscriber action + if (sub_instance_.last_msg) + { + last_msg_ = sub_instance_.last_msg; + } // add "this" as received of the broadcaster signal_connection_ = sub_instance_->broadcaster.connect( @@ -286,12 +293,18 @@ template inline // First, check if the subscriber_ is valid and that the name of the // topic_name in the port didn't change. // otherwise, create a new subscriber + std::string topic_name; + getInput("topic_name", topic_name); + if(!sub_instance_) { - std::string topic_name; - getInput("topic_name", topic_name); createSubscriber(topic_name); } + else if(topic_name_ != topic_name) + { + sub_instance_.reset(); + createSubscriber(topic_name_); + } auto CheckStatus = [](NodeStatus status) { From 06a508e16f8c13e211d1d8b0d566c866043c0a8d Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Tue, 2 Apr 2024 14:17:36 +0200 Subject: [PATCH 04/44] fix: pointer references --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 8d3667c..780b27e 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -273,9 +273,9 @@ template inline } // Check if there was a message received before the creation of this subscriber action - if (sub_instance_.last_msg) + if (sub_instance_->last_msg) { - last_msg_ = sub_instance_.last_msg; + last_msg_ = sub_instance_->last_msg; } // add "this" as received of the broadcaster From 421e5c03bb2e0350d5aecc7dd9e113e58ac93a0b Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Tue, 2 Apr 2024 15:55:17 +0200 Subject: [PATCH 05/44] fix: create subscriber with new topic name --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 780b27e..77bde12 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -303,7 +303,7 @@ template inline else if(topic_name_ != topic_name) { sub_instance_.reset(); - createSubscriber(topic_name_); + createSubscriber(topic_name); } auto CheckStatus = [](NodeStatus status) From e85eb03b3663f153af8fcd005bf3abd04a95d737 Mon Sep 17 00:00:00 2001 From: tony-p Date: Fri, 5 Apr 2024 16:18:56 +0200 Subject: [PATCH 06/44] Apply suggestions from code review style: update from review Co-authored-by: Davide Faconti --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 77bde12..f732f1c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -76,7 +76,7 @@ class RosTopicSubNode : public BT::ConditionNode rclcpp::CallbackGroup::SharedPtr callback_group; rclcpp::executors::SingleThreadedExecutor callback_group_executor; boost::signals2::signal)> broadcaster; - std::shared_ptr last_msg = nullptr; + std::shared_ptr last_msg; }; @@ -319,7 +319,7 @@ template inline auto status = CheckStatus (onTick(last_msg_)); if (clearProcessedMessage()) { - last_msg_ = nullptr; + last_msg_.reset(); } return status; } From bd15f2f60b8c3d077219fb0033ce9774b6ef58bd Mon Sep 17 00:00:00 2001 From: vicmassy Date: Wed, 10 Apr 2024 14:49:51 +0200 Subject: [PATCH 07/44] Fix cancel goal with emopty goal handle --- .../include/behaviortree_ros2/bt_action_node.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index c6b0c97..e125afc 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -433,6 +433,12 @@ template inline template inline void RosActionNode::cancelGoal() { + if (!goal_handle_) + { + RCLCPP_WARN( node_->get_logger(), "cancelGoal called on an empty goal_handle"); + return; + } + auto future_result = action_client_->async_get_result(goal_handle_); auto future_cancel = action_client_->async_cancel_goal(goal_handle_); From 47d60b9ed83c4fe7150e9d0a31b5c8aedd35c146 Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Thu, 11 Apr 2024 21:59:59 +0200 Subject: [PATCH 08/44] refactor: change name of retention method to latchLastMessage --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index f732f1c..fa3014d 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -175,15 +175,15 @@ class RosTopicSubNode : public BT::ConditionNode */ virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; - /** Clear the message that has been processed. If returns true and no new message is - * received, before next call there will be no message to process. If returns false, + /** latch the message that has been processed. If returns false and no new message is + * received, before next call there will be no message to process. If returns true, * the next call will process the same message again, if no new message received. * * This can be equated with latched vs non-latched topics in ros 1. * - * @return true will clear the message after ticking/processing. + * @return false will clear the message after ticking/processing. */ - virtual bool clearProcessedMessage() { return true; } + virtual bool latchLastMessage() const { return false; } private: @@ -317,7 +317,7 @@ template inline }; sub_instance_->callback_group_executor.spin_some(); auto status = CheckStatus (onTick(last_msg_)); - if (clearProcessedMessage()) + if (!latchLastMessage()) { last_msg_.reset(); } From 717ff616289f9e96372f7e9b4668801beac36b15 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Fri, 12 Apr 2024 09:42:25 -0600 Subject: [PATCH 09/44] Export plugins to share directory --- btcpp_ros2_samples/CMakeLists.txt | 9 +++++++++ btcpp_ros2_samples/src/sleep_action.cpp | 4 ++-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index f2bb870..1ff82b7 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -57,6 +57,15 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) +###################################################### +# INSTALL plugins for other packages to load + +install(TARGETS + sleep_plugin + LIBRARY DESTINATION share/${PROJECT_NAME}/bt_plugins + ARCHIVE DESTINATION share/${PROJECT_NAME}/bt_plugins + RUNTIME DESTINATION share/${PROJECT_NAME}/bt_plugins + ) ament_export_dependencies(behaviortree_ros2 btcpp_ros2_interfaces) diff --git a/btcpp_ros2_samples/src/sleep_action.cpp b/btcpp_ros2_samples/src/sleep_action.cpp index a838d74..cd28420 100644 --- a/btcpp_ros2_samples/src/sleep_action.cpp +++ b/btcpp_ros2_samples/src/sleep_action.cpp @@ -28,5 +28,5 @@ void SleepAction::onHalt() } // Plugin registration. -// The class SleepAction will self register with name "Sleep". -CreateRosNodePlugin(SleepAction, "Sleep"); +// The class SleepAction will self register with name "SleepAction". +CreateRosNodePlugin(SleepAction, "SleepAction"); From bdb778ac98583c96587fdc0bc22f8c24e1bb1a54 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Fri, 19 Apr 2024 13:18:00 +0200 Subject: [PATCH 10/44] fix TopicSubscriber --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index fa3014d..598859b 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -296,14 +296,14 @@ template inline std::string topic_name; getInput("topic_name", topic_name); - if(!sub_instance_) + if(!topic_name.empty() && topic_name != "__default__placeholder__" && topic_name != topic_name_) { - createSubscriber(topic_name); + sub_instance_.reset(); } - else if(topic_name_ != topic_name) + + if(!sub_instance_) { - sub_instance_.reset(); - createSubscriber(topic_name); + createSubscriber(topic_name); } auto CheckStatus = [](NodeStatus status) From 7ed2a345721d7fa252f87db7ca3e5960d7f8b647 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Fri, 19 Apr 2024 11:10:05 -0600 Subject: [PATCH 11/44] Add pre-commit config and workflow --- .clang-format | 68 +++++ .github/workflows/pre-commit.yaml | 14 ++ .pre-commit-config.yaml | 44 ++++ LICENSE | 1 - README.md | 2 - behaviortree_ros2/CMakeLists.txt | 4 +- .../behaviortree_ros2/bt_action_node.hpp | 236 +++++++++--------- .../behaviortree_ros2/bt_service_node.hpp | 140 ++++++----- .../behaviortree_ros2/bt_topic_pub_node.hpp | 65 +++-- .../behaviortree_ros2/bt_topic_sub_node.hpp | 141 +++++------ .../include/behaviortree_ros2/plugins.hpp | 29 +-- .../behaviortree_ros2/ros_node_params.hpp | 2 +- btcpp_ros2_samples/CMakeLists.txt | 8 +- btcpp_ros2_samples/src/sleep_action.cpp | 13 +- btcpp_ros2_samples/src/sleep_action.hpp | 7 +- btcpp_ros2_samples/src/sleep_client.cpp | 26 +- btcpp_ros2_samples/src/sleep_server.cpp | 34 ++- btcpp_ros2_samples/src/subscriber_test.cpp | 16 +- run_clang_format.sh | 3 + 19 files changed, 497 insertions(+), 356 deletions(-) create mode 100644 .clang-format create mode 100644 .github/workflows/pre-commit.yaml create mode 100644 .pre-commit-config.yaml create mode 100755 run_clang_format.sh diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..80f4f71 --- /dev/null +++ b/.clang-format @@ -0,0 +1,68 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializers: BeforeComma +BinPackParameters: true +ColumnLimit: 90 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: Never +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false +ReflowComments: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true', + AfterControlStatement: 'true', + AfterEnum : 'true', + AfterFunction : 'true', + AfterNamespace : 'true', + AfterStruct : 'true', + AfterUnion : 'true', + BeforeCatch : 'true', + BeforeElse : 'true', + IndentBraces : 'false', + SplitEmptyFunction: 'false' +} +... diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml new file mode 100644 index 0000000..6095e08 --- /dev/null +++ b/.github/workflows/pre-commit.yaml @@ -0,0 +1,14 @@ +name: pre-commit + +on: + pull_request: + push: + branches: [master] + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v3 + - uses: pre-commit/action@v3.0.1 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..e6ace82 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,44 @@ + +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +exclude: ^3rdparty/|3rdparty|^include/behaviortree_cpp/contrib/ +repos: + + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.5.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: end-of-file-fixer + exclude_types: [svg] + - id: mixed-line-ending + - id: trailing-whitespace + exclude_types: [svg] + - id: fix-byte-order-marker + + # CPP hooks + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v17.0.6 + hooks: + - id: clang-format + args: ['-fallback-style=none', '-i'] diff --git a/LICENSE b/LICENSE index 7fc7319..5f09852 100644 --- a/LICENSE +++ b/LICENSE @@ -202,4 +202,3 @@ distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. - diff --git a/README.md b/README.md index 32c343b..ce5b332 100644 --- a/README.md +++ b/README.md @@ -29,5 +29,3 @@ wrap your Nodes into plugins that can be loaded at run-time. A lot of code is either inspired or copied from [Nav2](https://navigation.ros.org/). For this reason, we retain the same license and copyright. - - diff --git a/behaviortree_ros2/CMakeLists.txt b/behaviortree_ros2/CMakeLists.txt index 3c46da2..01378ff 100644 --- a/behaviortree_ros2/CMakeLists.txt +++ b/behaviortree_ros2/CMakeLists.txt @@ -26,8 +26,8 @@ ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS}) ###################################################### # INSTALL -install(DIRECTORY include/ DESTINATION include/) - +install(DIRECTORY include/ DESTINATION include/) + ament_export_include_directories(include) ament_export_dependencies(${THIS_PACKAGE_DEPS}) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index e125afc..7bb2f99 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -40,14 +40,20 @@ enum ActionNodeErrorCode inline const char* toStr(const ActionNodeErrorCode& err) { - switch (err) + switch(err) { - case SERVER_UNREACHABLE: return "SERVER_UNREACHABLE"; - case SEND_GOAL_TIMEOUT: return "SEND_GOAL_TIMEOUT"; - case GOAL_REJECTED_BY_SERVER: return "GOAL_REJECTED_BY_SERVER"; - case ACTION_ABORTED: return "ACTION_ABORTED"; - case ACTION_CANCELLED: return "ACTION_CANCELLED"; - case INVALID_GOAL: return "INVALID_GOAL"; + case SERVER_UNREACHABLE: + return "SERVER_UNREACHABLE"; + case SEND_GOAL_TIMEOUT: + return "SEND_GOAL_TIMEOUT"; + case GOAL_REJECTED_BY_SERVER: + return "GOAL_REJECTED_BY_SERVER"; + case ACTION_ABORTED: + return "ACTION_ABORTED"; + case ACTION_CANCELLED: + return "ACTION_CANCELLED"; + case INVALID_GOAL: + return "INVALID_GOAL"; } return nullptr; } @@ -70,10 +76,9 @@ inline const char* toStr(const ActionNodeErrorCode& err) * 1. If a value is passes in the InputPort "action_name", use that * 2. Otherwise, use the value in RosNodeParams::default_port_value */ -template +template class RosActionNode : public BT::ActionNodeBase { - public: // Type definitions using ActionType = ActionT; @@ -89,8 +94,7 @@ class RosActionNode : public BT::ActionNodeBase * factory.registerNodeType<>(node_name, params); * */ - explicit RosActionNode(const std::string & instance_name, - const BT::NodeConfig& conf, + explicit RosActionNode(const std::string& instance_name, const BT::NodeConfig& conf, const RosNodeParams& params); virtual ~RosActionNode() = default; @@ -104,9 +108,8 @@ class RosActionNode : public BT::ActionNodeBase */ static PortsList providedBasicPorts(PortsList addition) { - PortsList basic = { - InputPort("action_name", "__default__placeholder__", "Action server name") - }; + PortsList basic = { InputPort("action_name", "__default__placeholder__", + "Action server name") }; basic.insert(addition.begin(), addition.end()); return basic; } @@ -122,7 +125,8 @@ class RosActionNode : public BT::ActionNodeBase /// @brief Callback executed when the node is halted. Note that cancelGoal() /// is done automatically. - virtual void onHalt() {} + virtual void onHalt() + {} /** setGoal s a callback that allows the user to set * the goal message (ActionT::Goal). @@ -159,14 +163,12 @@ class RosActionNode : public BT::ActionNodeBase /// Method used to send a request to the Action server to cancel the current goal void cancelGoal(); - /// The default halt() implementation will call cancelGoal if necessary. void halt() override final; NodeStatus tick() override final; protected: - std::shared_ptr node_; std::string prev_action_name_; bool action_name_may_change_ = false; @@ -174,7 +176,6 @@ class RosActionNode : public BT::ActionNodeBase const std::chrono::milliseconds wait_for_server_timeout_; private: - ActionClientPtr action_client_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; @@ -187,21 +188,21 @@ class RosActionNode : public BT::ActionNodeBase bool goal_received_; WrappedResult result_; - bool createClient(const std::string &action_name); + bool createClient(const std::string& action_name); }; //---------------------------------------------------------------- //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- -template inline - RosActionNode::RosActionNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams ¶ms): - BT::ActionNodeBase(instance_name, conf), - node_(params.nh), - server_timeout_(params.server_timeout), - wait_for_server_timeout_(params.wait_for_server_timeout) +template +inline RosActionNode::RosActionNode(const std::string& instance_name, + const NodeConfig& conf, + const RosNodeParams& params) + : BT::ActionNodeBase(instance_name, conf) + , node_(params.nh) + , server_timeout_(params.server_timeout) + , wait_for_server_timeout_(params.wait_for_server_timeout) { // Three cases: // - we use the default action_name in RosNodeParams when port is empty @@ -216,11 +217,13 @@ template inline if(bb_action_name.empty() || bb_action_name == "__default__placeholder__") { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [action_name] in the InputPort and the RosNodeParams are empty."); + if(params.default_port_value.empty()) + { + throw std::logic_error("Both [action_name] in the InputPort and the " + "RosNodeParams are empty."); } - else { + else + { createClient(params.default_port_value); } } @@ -231,33 +234,38 @@ template inline // create the client in the constructor. createClient(bb_action_name); } - else { + else + { action_name_may_change_ = true; // createClient will be invoked in the first tick(). } } - else { - - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [action_name] in the InputPort and the RosNodeParams are empty."); + else + { + if(params.default_port_value.empty()) + { + throw std::logic_error("Both [action_name] in the InputPort and the RosNodeParams " + "are empty."); } - else { + else + { createClient(params.default_port_value); } } } -template inline - bool RosActionNode::createClient(const std::string& action_name) +template +inline bool RosActionNode::createClient(const std::string& action_name) { if(action_name.empty()) { throw RuntimeError("action_name is empty"); } - callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + callback_group_ = + node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_executor_.add_callback_group(callback_group_, + node_->get_node_base_interface()); action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_); prev_action_name_ = action_name; @@ -265,14 +273,15 @@ template inline bool found = action_client_->wait_for_action_server(wait_for_server_timeout_); if(!found) { - RCLCPP_ERROR(node_->get_logger(), "%s: Action server with name '%s' is not reachable.", - name().c_str(), prev_action_name_.c_str()); + RCLCPP_ERROR(node_->get_logger(), + "%s: Action server with name '%s' is not reachable.", name().c_str(), + prev_action_name_.c_str()); } return found; } -template inline - NodeStatus RosActionNode::tick() +template +inline NodeStatus RosActionNode::tick() { // First, check if the action_client_ is valid and that the name of the // action_name in the port didn't change. @@ -288,17 +297,17 @@ template inline } //------------------------------------------ - auto CheckStatus = [](NodeStatus status) - { - if( !isStatusCompleted(status) ) + auto CheckStatus = [](NodeStatus status) { + if(!isStatusCompleted(status)) { - throw std::logic_error("RosActionNode: the callback must return either SUCCESS of FAILURE"); + throw std::logic_error("RosActionNode: the callback must return either SUCCESS of " + "FAILURE"); } return status; }; // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) + if(status() == BT::NodeStatus::IDLE) { setStatus(NodeStatus::RUNNING); @@ -309,77 +318,80 @@ template inline Goal goal; - if( !setGoal(goal) ) + if(!setGoal(goal)) { - return CheckStatus( onFailure(INVALID_GOAL) ); + return CheckStatus(onFailure(INVALID_GOAL)); } typename ActionClient::SendGoalOptions goal_options; //-------------------- goal_options.feedback_callback = - [this](typename GoalHandle::SharedPtr, - const std::shared_ptr feedback) - { - on_feedback_state_change_ = onFeedback(feedback); - if( on_feedback_state_change_ == NodeStatus::IDLE) - { - throw std::logic_error("onFeedback must not return IDLE"); - } - emitWakeUpSignal(); - }; + [this](typename GoalHandle::SharedPtr, + const std::shared_ptr feedback) { + on_feedback_state_change_ = onFeedback(feedback); + if(on_feedback_state_change_ == NodeStatus::IDLE) + { + throw std::logic_error("onFeedback must not return IDLE"); + } + emitWakeUpSignal(); + }; //-------------------- - goal_options.result_callback = - [this](const WrappedResult& result) - { - if (goal_handle_->get_goal_id() == result.goal_id) { - RCLCPP_DEBUG( node_->get_logger(), "result_callback" ); + goal_options.result_callback = [this](const WrappedResult& result) { + if(goal_handle_->get_goal_id() == result.goal_id) + { + RCLCPP_DEBUG(node_->get_logger(), "result_callback"); result_ = result; emitWakeUpSignal(); } }; //-------------------- goal_options.goal_response_callback = - [this](typename GoalHandle::SharedPtr const future_handle) - { - auto goal_handle_ = future_handle.get(); - if (!goal_handle_) - { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - } else { - RCLCPP_DEBUG(node_->get_logger(), "Goal accepted by server, waiting for result"); - } - }; + [this](typename GoalHandle::SharedPtr const future_handle) { + auto goal_handle_ = future_handle.get(); + if(!goal_handle_) + { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + } + else + { + RCLCPP_DEBUG(node_->get_logger(), "Goal accepted by server, waiting for " + "result"); + } + }; //-------------------- // Check if server is ready if(!action_client_->action_server_is_ready()) return onFailure(SERVER_UNREACHABLE); - future_goal_handle_ = action_client_->async_send_goal( goal, goal_options ); + future_goal_handle_ = action_client_->async_send_goal(goal, goal_options); time_goal_sent_ = node_->now(); return NodeStatus::RUNNING; } - if (status() == NodeStatus::RUNNING) + if(status() == NodeStatus::RUNNING) { callback_group_executor_.spin_some(); // FIRST case: check if the goal request has a timeout - if( !goal_received_ ) + if(!goal_received_) { auto nodelay = std::chrono::milliseconds(0); - auto timeout = rclcpp::Duration::from_seconds( double(server_timeout_.count()) / 1000); + auto timeout = + rclcpp::Duration::from_seconds(double(server_timeout_.count()) / 1000); - auto ret = callback_group_executor_.spin_until_future_complete(future_goal_handle_, nodelay); - if (ret != rclcpp::FutureReturnCode::SUCCESS) + auto ret = callback_group_executor_.spin_until_future_complete(future_goal_handle_, + nodelay); + if(ret != rclcpp::FutureReturnCode::SUCCESS) { - if( (node_->now() - time_goal_sent_) > timeout ) + if((node_->now() - time_goal_sent_) > timeout) { - return CheckStatus( onFailure(SEND_GOAL_TIMEOUT) ); + return CheckStatus(onFailure(SEND_GOAL_TIMEOUT)); } - else{ + else + { return NodeStatus::RUNNING; } } @@ -389,39 +401,41 @@ template inline goal_handle_ = future_goal_handle_.get(); future_goal_handle_ = {}; - if (!goal_handle_) { - return CheckStatus( onFailure( GOAL_REJECTED_BY_SERVER ) ); + if(!goal_handle_) + { + return CheckStatus(onFailure(GOAL_REJECTED_BY_SERVER)); } } } // SECOND case: onFeedback requested a stop - if( on_feedback_state_change_ != NodeStatus::RUNNING ) + if(on_feedback_state_change_ != NodeStatus::RUNNING) { cancelGoal(); return on_feedback_state_change_; } // THIRD case: result received, requested a stop - if( result_.code != rclcpp_action::ResultCode::UNKNOWN) + if(result_.code != rclcpp_action::ResultCode::UNKNOWN) { - if( result_.code == rclcpp_action::ResultCode::ABORTED ) + if(result_.code == rclcpp_action::ResultCode::ABORTED) { - return CheckStatus( onFailure( ACTION_ABORTED ) ); + return CheckStatus(onFailure(ACTION_ABORTED)); } - else if( result_.code == rclcpp_action::ResultCode::CANCELED ) + else if(result_.code == rclcpp_action::ResultCode::CANCELED) { - return CheckStatus( onFailure( ACTION_CANCELLED ) ); + return CheckStatus(onFailure(ACTION_CANCELLED)); } - else{ - return CheckStatus( onResultReceived( result_ ) ); + else + { + return CheckStatus(onResultReceived(result_)); } } } return NodeStatus::RUNNING; } -template inline - void RosActionNode::halt() +template +inline void RosActionNode::halt() { if(status() == BT::NodeStatus::RUNNING) { @@ -430,35 +444,31 @@ template inline } } -template inline - void RosActionNode::cancelGoal() +template +inline void RosActionNode::cancelGoal() { - if (!goal_handle_) + if(!goal_handle_) { - RCLCPP_WARN( node_->get_logger(), "cancelGoal called on an empty goal_handle"); + RCLCPP_WARN(node_->get_logger(), "cancelGoal called on an empty goal_handle"); return; } auto future_result = action_client_->async_get_result(goal_handle_); auto future_cancel = action_client_->async_cancel_goal(goal_handle_); - if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) + if(callback_group_executor_.spin_until_future_complete( + future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR( node_->get_logger(), "Failed to cancel action server for [%s]", + RCLCPP_ERROR(node_->get_logger(), "Failed to cancel action server for [%s]", prev_action_name_.c_str()); } - if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) + if(callback_group_executor_.spin_until_future_complete( + future_result, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR( node_->get_logger(), "Failed to get result call failed :( for [%s]", + RCLCPP_ERROR(node_->get_logger(), "Failed to get result call failed :( for [%s]", prev_action_name_.c_str()); } } - - - } // namespace BT - diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 67a6750..3ceae00 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -36,12 +36,16 @@ enum ServiceNodeErrorCode inline const char* toStr(const ServiceNodeErrorCode& err) { - switch (err) + switch(err) { - case SERVICE_UNREACHABLE: return "SERVICE_UNREACHABLE"; - case SERVICE_TIMEOUT: return "SERVICE_TIMEOUT"; - case INVALID_REQUEST: return "INVALID_REQUEST"; - case SERVICE_ABORTED: return "SERVICE_ABORTED"; + case SERVICE_UNREACHABLE: + return "SERVICE_UNREACHABLE"; + case SERVICE_TIMEOUT: + return "SERVICE_TIMEOUT"; + case INVALID_REQUEST: + return "INVALID_REQUEST"; + case SERVICE_ABORTED: + return "SERVICE_ABORTED"; } return nullptr; } @@ -64,10 +68,9 @@ inline const char* toStr(const ServiceNodeErrorCode& err) * 1. If a value is passes in the InputPort "service_name", use that * 2. Otherwise, use the value in RosNodeParams::default_port_value */ -template +template class RosServiceNode : public BT::ActionNodeBase { - public: // Type definitions using ServiceClient = typename rclcpp::Client; @@ -78,8 +81,7 @@ class RosServiceNode : public BT::ActionNodeBase * * factory.registerNodeType<>(node_name, params); */ - explicit RosServiceNode(const std::string & instance_name, - const BT::NodeConfig& conf, + explicit RosServiceNode(const std::string& instance_name, const BT::NodeConfig& conf, const BT::RosNodeParams& params); virtual ~RosServiceNode() = default; @@ -93,9 +95,8 @@ class RosServiceNode : public BT::ActionNodeBase */ static PortsList providedBasicPorts(PortsList addition) { - PortsList basic = { - InputPort("service_name", "__default__placeholder__", "Service name") - }; + PortsList basic = { InputPort("service_name", "__default__placeholder__", + "Service name") }; basic.insert(addition.begin(), addition.end()); return basic; } @@ -127,18 +128,18 @@ class RosServiceNode : public BT::ActionNodeBase /** Callback invoked when the response is received by the server. * It is up to the user to define if this returns SUCCESS or FAILURE. */ - virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) = 0; + virtual BT::NodeStatus + onResponseReceived(const typename Response::SharedPtr& response) = 0; /** Callback invoked when something goes wrong; you can override it. * It must return either SUCCESS or FAILURE. */ virtual BT::NodeStatus onFailure(ServiceNodeErrorCode /*error*/) - { + { return NodeStatus::FAILURE; } protected: - std::shared_ptr node_; std::string prev_service_name_; bool service_name_may_change_ = false; @@ -146,7 +147,6 @@ class RosServiceNode : public BT::ActionNodeBase const std::chrono::milliseconds wait_for_service_timeout_; private: - typename std::shared_ptr service_client_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; @@ -158,21 +158,21 @@ class RosServiceNode : public BT::ActionNodeBase bool response_received_; typename Response::SharedPtr response_; - bool createClient(const std::string &service_name); + bool createClient(const std::string& service_name); }; //---------------------------------------------------------------- //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- -template inline - RosServiceNode::RosServiceNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams& params): - BT::ActionNodeBase(instance_name, conf), - node_(params.nh), - service_timeout_(params.server_timeout), - wait_for_service_timeout_(params.wait_for_server_timeout) +template +inline RosServiceNode::RosServiceNode(const std::string& instance_name, + const NodeConfig& conf, + const RosNodeParams& params) + : BT::ActionNodeBase(instance_name, conf) + , node_(params.nh) + , service_timeout_(params.server_timeout) + , wait_for_service_timeout_(params.wait_for_server_timeout) { // check port remapping auto portIt = config().input_ports.find("service_name"); @@ -182,11 +182,13 @@ template inline if(bb_service_name.empty() || bb_service_name == "__default__placeholder__") { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [service_name] in the InputPort and the RosNodeParams are empty."); + if(params.default_port_value.empty()) + { + throw std::logic_error("Both [service_name] in the InputPort and the " + "RosNodeParams are empty."); } - else { + else + { createClient(params.default_port_value); } } @@ -197,34 +199,40 @@ template inline // create the client in the constructor. createClient(bb_service_name); } - else { + else + { service_name_may_change_ = true; // createClient will be invoked in the first tick(). } } - else { - - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [service_name] in the InputPort and the RosNodeParams are empty."); + else + { + if(params.default_port_value.empty()) + { + throw std::logic_error("Both [service_name] in the InputPort and the RosNodeParams " + "are empty."); } - else { + else + { createClient(params.default_port_value); } } } -template inline - bool RosServiceNode::createClient(const std::string& service_name) +template +inline bool RosServiceNode::createClient(const std::string& service_name) { if(service_name.empty()) { throw RuntimeError("service_name is empty"); } - callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - service_client_ = node_->create_client(service_name, rmw_qos_profile_services_default, callback_group_); + callback_group_ = + node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_executor_.add_callback_group(callback_group_, + node_->get_node_base_interface()); + service_client_ = node_->create_client( + service_name, rmw_qos_profile_services_default, callback_group_); prev_service_name_ = service_name; bool found = service_client_->wait_for_service(wait_for_service_timeout_); @@ -236,8 +244,8 @@ template inline return found; } -template inline - NodeStatus RosServiceNode::tick() +template +inline NodeStatus RosServiceNode::tick() { // First, check if the service_client_ is valid and that the name of the // service_name in the port didn't change. @@ -252,17 +260,17 @@ template inline } } - auto CheckStatus = [](NodeStatus status) - { - if( !isStatusCompleted(status) ) + auto CheckStatus = [](NodeStatus status) { + if(!isStatusCompleted(status)) { - throw std::logic_error("RosServiceNode: the callback must return either SUCCESS or FAILURE"); + throw std::logic_error("RosServiceNode: the callback must return either SUCCESS or " + "FAILURE"); } return status; }; // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) + if(status() == BT::NodeStatus::IDLE) { setStatus(NodeStatus::RUNNING); @@ -273,9 +281,9 @@ template inline typename Request::SharedPtr request = std::make_shared(); - if( !setRequest(request) ) + if(!setRequest(request)) { - return CheckStatus( onFailure(INVALID_REQUEST) ); + return CheckStatus(onFailure(INVALID_REQUEST)); } // Check if server is ready @@ -288,25 +296,28 @@ template inline return NodeStatus::RUNNING; } - if (status() == NodeStatus::RUNNING) + if(status() == NodeStatus::RUNNING) { callback_group_executor_.spin_some(); // FIRST case: check if the goal request has a timeout - if( !response_received_ ) + if(!response_received_) { auto const nodelay = std::chrono::milliseconds(0); - auto const timeout = rclcpp::Duration::from_seconds( double(service_timeout_.count()) / 1000); + auto const timeout = + rclcpp::Duration::from_seconds(double(service_timeout_.count()) / 1000); - auto ret = callback_group_executor_.spin_until_future_complete(future_response_, nodelay); + auto ret = + callback_group_executor_.spin_until_future_complete(future_response_, nodelay); - if (ret != rclcpp::FutureReturnCode::SUCCESS) + if(ret != rclcpp::FutureReturnCode::SUCCESS) { - if( (node_->now() - time_request_sent_) > timeout ) + if((node_->now() - time_request_sent_) > timeout) { - return CheckStatus( onFailure(SERVICE_TIMEOUT) ); + return CheckStatus(onFailure(SERVICE_TIMEOUT)); } - else{ + else + { return NodeStatus::RUNNING; } } @@ -316,27 +327,26 @@ template inline response_ = future_response_.get(); future_response_ = {}; - if (!response_) { + if(!response_) + { throw std::runtime_error("Request was rejected by the service"); } } } // SECOND case: response received - return CheckStatus( onResponseReceived( response_ ) ); + return CheckStatus(onResponseReceived(response_)); } return NodeStatus::RUNNING; } -template inline - void RosServiceNode::halt() +template +inline void RosServiceNode::halt() { - if( status() == NodeStatus::RUNNING ) + if(status() == NodeStatus::RUNNING) { resetStatus(); } } - } // namespace BT - diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp index ebeb1f5..282259e 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp @@ -29,10 +29,9 @@ namespace BT * @brief Abstract class to wrap a ROS publisher * */ -template +template class RosTopicPubNode : public BT::ConditionNode { - public: // Type definitions using Publisher = typename rclcpp::Publisher; @@ -44,8 +43,7 @@ class RosTopicPubNode : public BT::ConditionNode * * Note that if the external_action_client is not set, the constructor will build its own. * */ - explicit RosTopicPubNode(const std::string & instance_name, - const BT::NodeConfig& conf, + explicit RosTopicPubNode(const std::string& instance_name, const BT::NodeConfig& conf, const RosNodeParams& params); virtual ~RosTopicPubNode() = default; @@ -59,9 +57,8 @@ class RosTopicPubNode : public BT::ConditionNode */ static PortsList providedBasicPorts(PortsList addition) { - PortsList basic = { - InputPort("topic_name", "__default__placeholder__", "Topic name") - }; + PortsList basic = { InputPort("topic_name", "__default__placeholder__", + "Topic name") }; basic.insert(addition.begin(), addition.end()); return basic; } @@ -88,13 +85,11 @@ class RosTopicPubNode : public BT::ConditionNode virtual bool setMessage(TopicT& msg) = 0; protected: - std::shared_ptr node_; std::string prev_topic_name_; bool topic_name_may_change_ = false; private: - std::shared_ptr publisher_; bool createPublisher(const std::string& topic_name); @@ -104,13 +99,12 @@ class RosTopicPubNode : public BT::ConditionNode //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- -template inline - RosTopicPubNode::RosTopicPubNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams& params) - : BT::ConditionNode(instance_name, conf), - node_(params.nh) -{ +template +inline RosTopicPubNode::RosTopicPubNode(const std::string& instance_name, + const NodeConfig& conf, + const RosNodeParams& params) + : BT::ConditionNode(instance_name, conf), node_(params.nh) +{ // check port remapping auto portIt = config().input_ports.find("topic_name"); if(portIt != config().input_ports.end()) @@ -119,11 +113,13 @@ template inline if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + if(params.default_port_value.empty()) + { + throw std::logic_error("Both [topic_name] in the InputPort and the RosNodeParams " + "are empty."); } - else { + else + { createPublisher(params.default_port_value); } } @@ -134,37 +130,41 @@ template inline // create the client in the constructor. createPublisher(bb_topic_name); } - else { + else + { topic_name_may_change_ = true; // createPublisher will be invoked in the first tick(). } } - else { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + else + { + if(params.default_port_value.empty()) + { + throw std::logic_error("Both [topic_name] in the InputPort and the RosNodeParams " + "are empty."); } - else { + else + { createPublisher(params.default_port_value); } } } -template inline - bool RosTopicPubNode::createPublisher(const std::string& topic_name) +template +inline bool RosTopicPubNode::createPublisher(const std::string& topic_name) { if(topic_name.empty()) { throw RuntimeError("topic_name is empty"); } - + publisher_ = node_->create_publisher(topic_name, 1); prev_topic_name_ = topic_name; return true; } -template inline - NodeStatus RosTopicPubNode::tick() +template +inline NodeStatus RosTopicPubNode::tick() { // First, check if the subscriber_ is valid and that the name of the // topic_name in the port didn't change. @@ -180,7 +180,7 @@ template inline } T msg; - if (!setMessage(msg)) + if(!setMessage(msg)) { return NodeStatus::FAILURE; } @@ -189,4 +189,3 @@ template inline } } // namespace BT - diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 598859b..42917b1 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -27,7 +27,6 @@ namespace BT { - /** * @brief Abstract class to wrap a Topic subscriber. * Considering the example in the tutorial: @@ -42,43 +41,40 @@ namespace BT * 1. If a value is passes in the InputPort "topic_name", use that * 2. Otherwise, use the value in RosNodeParams::default_port_value */ -template +template class RosTopicSubNode : public BT::ConditionNode { - public: +public: // Type definitions using Subscriber = typename rclcpp::Subscription; - protected: +protected: struct SubscriberInstance { void init(std::shared_ptr node, const std::string& topic_name) { // create a callback group for this particular instance - callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); - callback_group_executor.add_callback_group( - callback_group, node->get_node_base_interface()); + callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + callback_group_executor.add_callback_group(callback_group, + node->get_node_base_interface()); rclcpp::SubscriptionOptions option; option.callback_group = callback_group; - // The callback will broadcast to all the instances of RosTopicSubNode - auto callback = [this](const std::shared_ptr msg) - { + // The callback will broadcast to all the instances of RosTopicSubNode + auto callback = [this](const std::shared_ptr msg) { last_msg = msg; broadcaster(msg); }; - subscriber = node->create_subscription(topic_name, 1, callback, option); + subscriber = node->create_subscription(topic_name, 1, callback, option); } std::shared_ptr subscriber; rclcpp::CallbackGroup::SharedPtr callback_group; rclcpp::executors::SingleThreadedExecutor callback_group_executor; - boost::signals2::signal)> broadcaster; + boost::signals2::signal)> broadcaster; std::shared_ptr last_msg; - - }; static std::mutex& registryMutex() @@ -88,9 +84,11 @@ class RosTopicSubNode : public BT::ConditionNode } // contains the fully-qualified name of the node and the name of the topic - static std::unordered_map>& getRegistry() + static std::unordered_map>& + getRegistry() { - static std::unordered_map> subscribers_registry; + static std::unordered_map> + subscribers_registry; return subscribers_registry; } @@ -106,8 +104,7 @@ class RosTopicSubNode : public BT::ConditionNode return node_->get_logger(); } - public: - +public: /** You are not supposed to instantiate this class directly, the factory will do it. * To register this class into the factory, use: * @@ -115,11 +112,10 @@ class RosTopicSubNode : public BT::ConditionNode * * Note that if the external_action_client is not set, the constructor will build its own. * */ - explicit RosTopicSubNode(const std::string & instance_name, - const BT::NodeConfig& conf, + explicit RosTopicSubNode(const std::string& instance_name, const BT::NodeConfig& conf, const RosNodeParams& params); - virtual ~RosTopicSubNode() + virtual ~RosTopicSubNode() { signal_connection_.disconnect(); // remove the subscribers from the static registry when the ALL the @@ -135,7 +131,7 @@ class RosTopicSubNode : public BT::ConditionNode if(it != registry.end() && it->second.use_count() <= 1) { registry.erase(it); - RCLCPP_INFO(logger(), "Remove subscriber [%s]", topic_name_.c_str() ); + RCLCPP_INFO(logger(), "Remove subscriber [%s]", topic_name_.c_str()); } } } @@ -148,9 +144,8 @@ class RosTopicSubNode : public BT::ConditionNode */ static PortsList providedBasicPorts(PortsList addition) { - PortsList basic = { - InputPort("topic_name", "__default__placeholder__", "Topic name") - }; + PortsList basic = { InputPort("topic_name", "__default__placeholder__", + "Topic name") }; basic.insert(addition.begin(), addition.end()); return basic; } @@ -170,23 +165,25 @@ class RosTopicSubNode : public BT::ConditionNode * * @param last_msg the latest message received, since the last tick. * Will be empty if no new message received. - * + * * @return the new status of the Node, based on last_msg */ virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; - /** latch the message that has been processed. If returns false and no new message is + /** latch the message that has been processed. If returns false and no new message is * received, before next call there will be no message to process. If returns true, * the next call will process the same message again, if no new message received. - * + * * This can be equated with latched vs non-latched topics in ros 1. - * + * * @return false will clear the message after ticking/processing. */ - virtual bool latchLastMessage() const { return false; } + virtual bool latchLastMessage() const + { + return false; + } private: - bool createSubscriber(const std::string& topic_name); }; @@ -194,13 +191,12 @@ class RosTopicSubNode : public BT::ConditionNode //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- -template inline - RosTopicSubNode::RosTopicSubNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams& params) - : BT::ConditionNode(instance_name, conf), - node_(params.nh) -{ +template +inline RosTopicSubNode::RosTopicSubNode(const std::string& instance_name, + const NodeConfig& conf, + const RosNodeParams& params) + : BT::ConditionNode(instance_name, conf), node_(params.nh) +{ // check port remapping auto portIt = config().input_ports.find("topic_name"); if(portIt != config().input_ports.end()) @@ -209,11 +205,13 @@ template inline if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + if(params.default_port_value.empty()) + { + throw std::logic_error("Both [topic_name] in the InputPort and the RosNodeParams " + "are empty."); } - else { + else + { createSubscriber(params.default_port_value); } } @@ -224,24 +222,28 @@ template inline // create the client in the constructor. createSubscriber(bb_topic_name); } - else { + else + { // do nothing // createSubscriber will be invoked in the first tick(). } } - else { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + else + { + if(params.default_port_value.empty()) + { + throw std::logic_error("Both [topic_name] in the InputPort and the RosNodeParams " + "are empty."); } - else { + else + { createSubscriber(params.default_port_value); } } } -template inline - bool RosTopicSubNode::createSubscriber(const std::string& topic_name) +template +inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) { if(topic_name.empty()) { @@ -260,35 +262,35 @@ template inline auto it = registry.find(subscriber_key_); if(it == registry.end()) { - it = registry.insert( {subscriber_key_, std::make_shared() }).first; + it = registry.insert({ subscriber_key_, std::make_shared() }) + .first; sub_instance_ = it->second; sub_instance_->init(node_, topic_name); - RCLCPP_INFO(logger(), - "Node [%s] created Subscriber to topic [%s]", - name().c_str(), topic_name.c_str() ); + RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(), + topic_name.c_str()); } - else { + else + { sub_instance_ = it->second; } // Check if there was a message received before the creation of this subscriber action - if (sub_instance_->last_msg) + if(sub_instance_->last_msg) { last_msg_ = sub_instance_->last_msg; } // add "this" as received of the broadcaster signal_connection_ = sub_instance_->broadcaster.connect( - [this](const std::shared_ptr msg) { last_msg_ = msg; } ); + [this](const std::shared_ptr msg) { last_msg_ = msg; }); topic_name_ = topic_name; return true; } - -template inline - NodeStatus RosTopicSubNode::tick() +template +inline NodeStatus RosTopicSubNode::tick() { // First, check if the subscriber_ is valid and that the name of the // topic_name in the port didn't change. @@ -296,28 +298,28 @@ template inline std::string topic_name; getInput("topic_name", topic_name); - if(!topic_name.empty() && topic_name != "__default__placeholder__" && topic_name != topic_name_) + if(!topic_name.empty() && topic_name != "__default__placeholder__" && + topic_name != topic_name_) { sub_instance_.reset(); } if(!sub_instance_) { - createSubscriber(topic_name); + createSubscriber(topic_name); } - auto CheckStatus = [](NodeStatus status) - { - if( !isStatusCompleted(status) ) + auto CheckStatus = [](NodeStatus status) { + if(!isStatusCompleted(status)) { - throw std::logic_error("RosTopicSubNode: the callback must return" + throw std::logic_error("RosTopicSubNode: the callback must return" "either SUCCESS or FAILURE"); } return status; }; sub_instance_->callback_group_executor.spin_some(); - auto status = CheckStatus (onTick(last_msg_)); - if (!latchLastMessage()) + auto status = CheckStatus(onTick(last_msg_)); + if(!latchLastMessage()) { last_msg_.reset(); } @@ -325,4 +327,3 @@ template inline } } // namespace BT - diff --git a/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp b/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp index d94f529..4d7ed4d 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp @@ -20,7 +20,6 @@ #include "behaviortree_cpp/utils/shared_library.h" #include "behaviortree_ros2/ros_node_params.hpp" - // Use this macro to generate a plugin for: // // - BT::RosActionNode @@ -34,14 +33,12 @@ // Usage example: // CreateRosNodePlugin(MyClassName, "MyClassName"); -#define CreateRosNodePlugin(TYPE, REGISTRATION_NAME) \ -BTCPP_EXPORT void \ -BT_RegisterRosNodeFromPlugin(BT::BehaviorTreeFactory& factory, \ - const BT::RosNodeParams& params) \ -{ \ - factory.registerNodeType(REGISTRATION_NAME, params); \ -} \ - +#define CreateRosNodePlugin(TYPE, REGISTRATION_NAME) \ + BTCPP_EXPORT void BT_RegisterRosNodeFromPlugin(BT::BehaviorTreeFactory& factory, \ + const BT::RosNodeParams& params) \ + { \ + factory.registerNodeType(REGISTRATION_NAME, params); \ + } /** * @brief RegisterRosNode function used to load a plugin and register @@ -51,18 +48,12 @@ BT_RegisterRosNodeFromPlugin(BT::BehaviorTreeFactory& factory, \ * @param filepath path to the plugin. * @param params parameters to pass to the instances of the Node. */ -inline -void RegisterRosNode(BT::BehaviorTreeFactory& factory, - const std::filesystem::path& filepath, - const BT::RosNodeParams& params) +inline void RegisterRosNode(BT::BehaviorTreeFactory& factory, + const std::filesystem::path& filepath, + const BT::RosNodeParams& params) { BT::SharedLibrary loader(filepath.generic_string()); - typedef void (*Func)(BT::BehaviorTreeFactory&, - const BT::RosNodeParams&); + typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&); auto func = (Func)loader.getSymbol("BT_RegisterRosNodeFromPlugin"); func(factory, params); } - - - - diff --git a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp index 148c086..6ac9e63 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp @@ -42,4 +42,4 @@ struct RosNodeParams std::chrono::milliseconds wait_for_server_timeout = std::chrono::milliseconds(500); }; -} +} // namespace BT diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index 1ff82b7..6d4987b 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -10,7 +10,7 @@ find_package(behaviortree_ros2 REQUIRED) find_package(std_msgs REQUIRED) find_package(btcpp_ros2_interfaces REQUIRED) -set(THIS_PACKAGE_DEPS +set(THIS_PACKAGE_DEPS behaviortree_ros2 std_msgs btcpp_ros2_interfaces ) @@ -18,7 +18,7 @@ set(THIS_PACKAGE_DEPS ###################################################### # Build a client that call the sleep action (STATIC version) -add_executable(sleep_client +add_executable(sleep_client src/sleep_action.cpp src/sleep_client.cpp) ament_target_dependencies(sleep_client ${THIS_PACKAGE_DEPS}) @@ -38,12 +38,12 @@ ament_target_dependencies(sleep_client_dyn ${THIS_PACKAGE_DEPS}) ###################################################### # Build Server add_executable(sleep_server src/sleep_server.cpp) -ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS}) +ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS}) ###################################################### # Build subscriber_test add_executable(subscriber_test src/subscriber_test.cpp) -ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS}) +ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS}) ###################################################### # INSTALL diff --git a/btcpp_ros2_samples/src/sleep_action.cpp b/btcpp_ros2_samples/src/sleep_action.cpp index cd28420..1460806 100644 --- a/btcpp_ros2_samples/src/sleep_action.cpp +++ b/btcpp_ros2_samples/src/sleep_action.cpp @@ -1,30 +1,31 @@ #include "sleep_action.hpp" #include "behaviortree_ros2/plugins.hpp" -bool SleepAction::setGoal(RosActionNode::Goal &goal) +bool SleepAction::setGoal(RosActionNode::Goal& goal) { auto timeout = getInput("msec"); goal.msec_timeout = timeout.value(); return true; } -NodeStatus SleepAction::onResultReceived(const RosActionNode::WrappedResult &wr) +NodeStatus SleepAction::onResultReceived(const RosActionNode::WrappedResult& wr) { - RCLCPP_INFO( node_->get_logger(), "%s: onResultReceived. Done = %s", name().c_str(), - wr.result->done ? "true" : "false" ); + RCLCPP_INFO(node_->get_logger(), "%s: onResultReceived. Done = %s", name().c_str(), + wr.result->done ? "true" : "false"); return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE; } NodeStatus SleepAction::onFailure(ActionNodeErrorCode error) { - RCLCPP_ERROR( node_->get_logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error) ); + RCLCPP_ERROR(node_->get_logger(), "%s: onFailure with error: %s", name().c_str(), + toStr(error)); return NodeStatus::FAILURE; } void SleepAction::onHalt() { - RCLCPP_INFO( node_->get_logger(), "%s: onHalt", name().c_str() ); + RCLCPP_INFO(node_->get_logger(), "%s: onHalt", name().c_str()); } // Plugin registration. diff --git a/btcpp_ros2_samples/src/sleep_action.hpp b/btcpp_ros2_samples/src/sleep_action.hpp index 4cda7e3..84ec3a8 100644 --- a/btcpp_ros2_samples/src/sleep_action.hpp +++ b/btcpp_ros2_samples/src/sleep_action.hpp @@ -3,18 +3,17 @@ using namespace BT; -class SleepAction: public RosActionNode +class SleepAction : public RosActionNode { public: - SleepAction(const std::string& name, - const NodeConfig& conf, + SleepAction(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) : RosActionNode(name, conf, params) {} static BT::PortsList providedPorts() { - return providedBasicPorts({InputPort("msec")}); + return providedBasicPorts({ InputPort("msec") }); } bool setGoal(Goal& goal) override; diff --git a/btcpp_ros2_samples/src/sleep_client.cpp b/btcpp_ros2_samples/src/sleep_client.cpp index 2290f7e..59f04d0 100644 --- a/btcpp_ros2_samples/src/sleep_client.cpp +++ b/btcpp_ros2_samples/src/sleep_client.cpp @@ -18,29 +18,34 @@ class PrintValue : public BT::SyncActionNode { public: PrintValue(const std::string& name, const BT::NodeConfig& config) - : BT::SyncActionNode(name, config) {} + : BT::SyncActionNode(name, config) + {} - BT::NodeStatus tick() override { + BT::NodeStatus tick() override + { std::string msg; - if( getInput("message", msg ) ){ + if(getInput("message", msg)) + { std::cout << "PrintValue: " << msg << std::endl; return NodeStatus::SUCCESS; } - else{ - std::cout << "PrintValue FAILED "<< std::endl; + else + { + std::cout << "PrintValue FAILED " << std::endl; return NodeStatus::FAILURE; } } - static BT::PortsList providedPorts() { + static BT::PortsList providedPorts() + { return { BT::InputPort("message") }; } }; //----------------------------------------------------- - // Simple tree, used to execute once each action. - static const char* xml_text = R"( +// Simple tree, used to execute once each action. +static const char* xml_text = R"( @@ -58,7 +63,7 @@ class PrintValue : public BT::SyncActionNode )"; -int main(int argc, char **argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto nh = std::make_shared("sleep_client"); @@ -79,7 +84,8 @@ int main(int argc, char **argv) auto tree = factory.createTreeFromText(xml_text); - for(int i=0; i<5; i++){ + for(int i = 0; i < 5; i++) + { tree.tickWhileRunning(); } diff --git a/btcpp_ros2_samples/src/sleep_server.cpp b/btcpp_ros2_samples/src/sleep_server.cpp index d7c97ef..c13f8f3 100644 --- a/btcpp_ros2_samples/src/sleep_server.cpp +++ b/btcpp_ros2_samples/src/sleep_server.cpp @@ -13,32 +13,30 @@ class SleepActionServer : public rclcpp::Node using Sleep = btcpp_ros2_interfaces::action::Sleep; using GoalHandleSleep = rclcpp_action::ServerGoalHandle; - explicit SleepActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + explicit SleepActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("sleep_action_server", options) { using namespace std::placeholders; this->action_server_ = rclcpp_action::create_server( - this, - "sleep_service", - std::bind(&SleepActionServer::handle_goal, this, _1, _2), - std::bind(&SleepActionServer::handle_cancel, this, _1), - std::bind(&SleepActionServer::handle_accepted, this, _1)); + this, "sleep_service", std::bind(&SleepActionServer::handle_goal, this, _1, _2), + std::bind(&SleepActionServer::handle_cancel, this, _1), + std::bind(&SleepActionServer::handle_accepted, this, _1)); } private: rclcpp_action::Server::SharedPtr action_server_; - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID &, - std::shared_ptr goal) + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&, + std::shared_ptr goal) { - RCLCPP_INFO(this->get_logger(), "Received goal request with sleep time %d", goal->msec_timeout); + RCLCPP_INFO(this->get_logger(), "Received goal request with sleep time %d", + goal->msec_timeout); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr goal_handle) + rclcpp_action::CancelResponse + handle_cancel(const std::shared_ptr goal_handle) { RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); (void)goal_handle; @@ -49,7 +47,7 @@ class SleepActionServer : public rclcpp::Node { using namespace std::placeholders; // this needs to return quickly to avoid blocking the executor, so spin up a new thread - std::thread{std::bind(&SleepActionServer::execute, this, _1), goal_handle}.detach(); + std::thread{ std::bind(&SleepActionServer::execute, this, _1), goal_handle }.detach(); } void execute(const std::shared_ptr goal_handle) @@ -60,12 +58,13 @@ class SleepActionServer : public rclcpp::Node auto feedback = std::make_shared(); auto result = std::make_shared(); - rclcpp::Time deadline = get_clock()->now() + rclcpp::Duration::from_seconds( double(goal->msec_timeout) / 1000 ); + rclcpp::Time deadline = get_clock()->now() + rclcpp::Duration::from_seconds( + double(goal->msec_timeout) / 1000); int cycle = 0; - while( get_clock()->now() < deadline ) + while(get_clock()->now() < deadline) { - if (goal_handle->is_canceling()) + if(goal_handle->is_canceling()) { result->done = false; goal_handle->canceled(result); @@ -81,7 +80,7 @@ class SleepActionServer : public rclcpp::Node } // Check if goal is done - if (rclcpp::ok()) + if(rclcpp::ok()) { result->done = true; goal_handle->succeed(result); @@ -90,7 +89,6 @@ class SleepActionServer : public rclcpp::Node } }; // class SleepActionServer - int main(int argc, char** argv) { rclcpp::init(argc, argv); diff --git a/btcpp_ros2_samples/src/subscriber_test.cpp b/btcpp_ros2_samples/src/subscriber_test.cpp index 195f1b9..3cd9240 100644 --- a/btcpp_ros2_samples/src/subscriber_test.cpp +++ b/btcpp_ros2_samples/src/subscriber_test.cpp @@ -3,11 +3,10 @@ using namespace BT; -class ReceiveString: public RosTopicSubNode +class ReceiveString : public RosTopicSubNode { public: - ReceiveString(const std::string& name, - const NodeConfig& conf, + ReceiveString(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) : RosTopicSubNode(name, conf, params) {} @@ -19,16 +18,17 @@ class ReceiveString: public RosTopicSubNode NodeStatus onTick(const std::shared_ptr& last_msg) override { - if(last_msg) // empty if no new message received, since the last tick + if(last_msg) // empty if no new message received, since the last tick { - RCLCPP_INFO(logger(), "[%s] new message: %s", name().c_str(), last_msg->data.c_str()); + RCLCPP_INFO(logger(), "[%s] new message: %s", name().c_str(), + last_msg->data.c_str()); } return NodeStatus::SUCCESS; } }; - // Simple tree, used to execute once each action. - static const char* xml_text = R"( +// Simple tree, used to execute once each action. +static const char* xml_text = R"( @@ -40,7 +40,7 @@ class ReceiveString: public RosTopicSubNode )"; -int main(int argc, char **argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto nh = std::make_shared("subscriber_test"); diff --git a/run_clang_format.sh b/run_clang_format.sh new file mode 100755 index 0000000..cf3d51a --- /dev/null +++ b/run_clang_format.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +find . -name '*.h' -or -name '*.hpp' -or -name '*.cpp' | xargs clang-format-3.8 -i -style=file $1 From a5d05488615e989e9ca2dbb4ec9b17ce2015c950 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Tue, 23 Apr 2024 09:14:48 -0600 Subject: [PATCH 12/44] Remove run clang script --- run_clang_format.sh | 3 --- 1 file changed, 3 deletions(-) delete mode 100755 run_clang_format.sh diff --git a/run_clang_format.sh b/run_clang_format.sh deleted file mode 100755 index cf3d51a..0000000 --- a/run_clang_format.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -find . -name '*.h' -or -name '*.hpp' -or -name '*.cpp' | xargs clang-format-3.8 -i -style=file $1 From 4d355bb0efc9e9c6470c437dfe204fe98dc92420 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 24 Apr 2024 15:18:36 +0200 Subject: [PATCH 13/44] use weak_ptr to simplify SubscribersRegistry in RosTopicSubNode --- .../behaviortree_ros2/bt_topic_sub_node.hpp | 74 ++++++++----------- 1 file changed, 30 insertions(+), 44 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 42917b1..c1a8939 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -51,24 +51,7 @@ class RosTopicSubNode : public BT::ConditionNode protected: struct SubscriberInstance { - void init(std::shared_ptr node, const std::string& topic_name) - { - // create a callback group for this particular instance - callback_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, false); - callback_group_executor.add_callback_group(callback_group, - node->get_node_base_interface()); - - rclcpp::SubscriptionOptions option; - option.callback_group = callback_group; - - // The callback will broadcast to all the instances of RosTopicSubNode - auto callback = [this](const std::shared_ptr msg) { - last_msg = msg; - broadcaster(msg); - }; - subscriber = node->create_subscription(topic_name, 1, callback, option); - } + SubscriberInstance(std::shared_ptr node, const std::string& topic_name); std::shared_ptr subscriber; rclcpp::CallbackGroup::SharedPtr callback_group; @@ -83,12 +66,13 @@ class RosTopicSubNode : public BT::ConditionNode return sub_mutex; } + using SubscribersRegistry = + std::unordered_map>; + // contains the fully-qualified name of the node and the name of the topic - static std::unordered_map>& - getRegistry() + static SubscribersRegistry& getRegistry() { - static std::unordered_map> - subscribers_registry; + static SubscribersRegistry subscribers_registry; return subscribers_registry; } @@ -108,7 +92,7 @@ class RosTopicSubNode : public BT::ConditionNode /** You are not supposed to instantiate this class directly, the factory will do it. * To register this class into the factory, use: * - * RegisterRosAction(factory, params) + * RegisterRosAction(factory, params) * * Note that if the external_action_client is not set, the constructor will build its own. * */ @@ -118,22 +102,6 @@ class RosTopicSubNode : public BT::ConditionNode virtual ~RosTopicSubNode() { signal_connection_.disconnect(); - // remove the subscribers from the static registry when the ALL the - // instances of RosTopicSubNode are destroyed (i.e., the tree is destroyed) - if(sub_instance_) - { - sub_instance_.reset(); - std::unique_lock lk(registryMutex()); - auto& registry = getRegistry(); - auto it = registry.find(subscriber_key_); - // when the reference count is 1, means that the only instance is owned by the - // registry itself. Delete it - if(it != registry.end() && it->second.use_count() <= 1) - { - registry.erase(it); - RCLCPP_INFO(logger(), "Remove subscriber [%s]", topic_name_.c_str()); - } - } } /** @@ -190,6 +158,26 @@ class RosTopicSubNode : public BT::ConditionNode //---------------------------------------------------------------- //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- +template +inline RosTopicSubNode::SubscriberInstance::SubscriberInstance( + std::shared_ptr node, const std::string& topic_name) +{ + // create a callback group for this particular instance + callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + callback_group_executor.add_callback_group(callback_group, + node->get_node_base_interface()); + + rclcpp::SubscriptionOptions option; + option.callback_group = callback_group; + + // The callback will broadcast to all the instances of RosTopicSubNode + auto callback = [this](const std::shared_ptr msg) { + last_msg = msg; + broadcaster(msg); + }; + subscriber = node->create_subscription(topic_name, 1, callback, option); +} template inline RosTopicSubNode::RosTopicSubNode(const std::string& instance_name, @@ -262,17 +250,15 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) auto it = registry.find(subscriber_key_); if(it == registry.end()) { - it = registry.insert({ subscriber_key_, std::make_shared() }) - .first; - sub_instance_ = it->second; - sub_instance_->init(node_, topic_name); + sub_instance_ = std::make_shared(node_, topic_name); + registry.insert({ subscriber_key_, sub_instance_ }); RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(), topic_name.c_str()); } else { - sub_instance_ = it->second; + sub_instance_ = it->second.lock(); } // Check if there was a message received before the creation of this subscriber action From 3b399da8b29b117b36fd283f8784f6b0ff54a14a Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 24 Apr 2024 16:27:58 +0200 Subject: [PATCH 14/44] cherry picking changes from PR #46: instance registry of Action Clients --- .../behaviortree_ros2/bt_action_node.hpp | 139 +++++++++++++----- .../behaviortree_ros2/bt_service_node.hpp | 6 +- .../behaviortree_ros2/bt_topic_sub_node.hpp | 2 +- 3 files changed, 103 insertions(+), 44 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 7bb2f99..f41f6a8 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -169,17 +169,46 @@ class RosActionNode : public BT::ActionNodeBase NodeStatus tick() override final; protected: + struct ActionClientInstance + { + ActionClientInstance(std::shared_ptr node, + const std::string& action_name); + + ActionClientPtr action_client; + rclcpp::CallbackGroup::SharedPtr callback_group; + rclcpp::executors::SingleThreadedExecutor callback_executor; + typename ActionClient::SendGoalOptions goal_options; + }; + + static std::mutex& getMutex() + { + static std::mutex action_client_mutex; + return action_client_mutex; + } + + rclcpp::Logger logger() + { + return node_->get_logger(); + } + + using ClientsRegistry = + std::unordered_map>; + // contains the fully-qualified name of the node and the name of the client + static ClientsRegistry& getRegistry() + { + static ClientsRegistry action_clients_registry; + return action_clients_registry; + } + std::shared_ptr node_; - std::string prev_action_name_; + std::shared_ptr client_instance_; + std::string action_name_; bool action_name_may_change_ = false; const std::chrono::milliseconds server_timeout_; const std::chrono::milliseconds wait_for_server_timeout_; + std::string action_client_key_; private: - ActionClientPtr action_client_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - std::shared_future future_goal_handle_; typename GoalHandle::SharedPtr goal_handle_; @@ -195,6 +224,16 @@ class RosActionNode : public BT::ActionNodeBase //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- +template +RosActionNode::ActionClientInstance::ActionClientInstance( + std::shared_ptr node, const std::string& action_name) +{ + callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_executor.add_callback_group(callback_group, node->get_node_base_interface()); + action_client = rclcpp_action::create_client(node, action_name, callback_group); +} + template inline RosActionNode::RosActionNode(const std::string& instance_name, const NodeConfig& conf, @@ -262,20 +301,29 @@ inline bool RosActionNode::createClient(const std::string& action_name) throw RuntimeError("action_name is empty"); } - callback_group_ = - node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_executor_.add_callback_group(callback_group_, - node_->get_node_base_interface()); - action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_); + std::unique_lock lk(getMutex()); + action_client_key_ = std::string(node_->get_fully_qualified_name()) + "/" + action_name; - prev_action_name_ = action_name; + auto& registry = getRegistry(); + auto it = registry.find(action_client_key_); + if(it == registry.end()) + { + client_instance_ = std::make_shared(node_, action_name); + registry.insert({ action_client_key_, client_instance_ }); + } + else + { + client_instance_ = it->second.lock(); + } - bool found = action_client_->wait_for_action_server(wait_for_server_timeout_); + action_name_ = action_name; + + bool found = + client_instance_->action_client->wait_for_action_server(wait_for_server_timeout_); if(!found) { - RCLCPP_ERROR(node_->get_logger(), - "%s: Action server with name '%s' is not reachable.", name().c_str(), - prev_action_name_.c_str()); + RCLCPP_ERROR(logger(), "%s: Action server with name '%s' is not reachable.", + name().c_str(), action_name_.c_str()); } return found; } @@ -283,25 +331,32 @@ inline bool RosActionNode::createClient(const std::string& action_name) template inline NodeStatus RosActionNode::tick() { + if(!rclcpp::ok()) + { + halt(); + return NodeStatus::FAILURE; + } + // First, check if the action_client_ is valid and that the name of the // action_name in the port didn't change. // otherwise, create a new client - if(!action_client_ || (status() == NodeStatus::IDLE && action_name_may_change_)) + if(!client_instance_ || (status() == NodeStatus::IDLE && action_name_may_change_)) { std::string action_name; getInput("action_name", action_name); - if(prev_action_name_ != action_name) + if(action_name_ != action_name) { createClient(action_name); } } + auto& action_client = client_instance_->action_client; //------------------------------------------ auto CheckStatus = [](NodeStatus status) { if(!isStatusCompleted(status)) { - throw std::logic_error("RosActionNode: the callback must return either SUCCESS of " - "FAILURE"); + throw LogicError("RosActionNode: the callback must return either SUCCESS nor " + "FAILURE"); } return status; }; @@ -340,7 +395,7 @@ inline NodeStatus RosActionNode::tick() goal_options.result_callback = [this](const WrappedResult& result) { if(goal_handle_->get_goal_id() == result.goal_id) { - RCLCPP_DEBUG(node_->get_logger(), "result_callback"); + RCLCPP_DEBUG(logger(), "result_callback"); result_ = result; emitWakeUpSignal(); } @@ -351,21 +406,21 @@ inline NodeStatus RosActionNode::tick() auto goal_handle_ = future_handle.get(); if(!goal_handle_) { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + RCLCPP_ERROR(logger(), "Goal was rejected by server"); } else { - RCLCPP_DEBUG(node_->get_logger(), "Goal accepted by server, waiting for " - "result"); + RCLCPP_DEBUG(logger(), "Goal accepted by server, waiting for result"); } }; //-------------------- - // Check if server is ready - if(!action_client_->action_server_is_ready()) + if(!action_client->action_server_is_ready()) + { return onFailure(SERVER_UNREACHABLE); + } - future_goal_handle_ = action_client_->async_send_goal(goal, goal_options); + future_goal_handle_ = action_client->async_send_goal(goal, goal_options); time_goal_sent_ = node_->now(); return NodeStatus::RUNNING; @@ -373,7 +428,8 @@ inline NodeStatus RosActionNode::tick() if(status() == NodeStatus::RUNNING) { - callback_group_executor_.spin_some(); + std::unique_lock lock(getMutex()); + client_instance_->callback_executor.spin_some(); // FIRST case: check if the goal request has a timeout if(!goal_received_) @@ -382,8 +438,8 @@ inline NodeStatus RosActionNode::tick() auto timeout = rclcpp::Duration::from_seconds(double(server_timeout_.count()) / 1000); - auto ret = callback_group_executor_.spin_until_future_complete(future_goal_handle_, - nodelay); + auto ret = client_instance_->callback_executor.spin_until_future_complete( + future_goal_handle_, nodelay); if(ret != rclcpp::FutureReturnCode::SUCCESS) { if((node_->now() - time_goal_sent_) > timeout) @@ -449,25 +505,28 @@ inline void RosActionNode::cancelGoal() { if(!goal_handle_) { - RCLCPP_WARN(node_->get_logger(), "cancelGoal called on an empty goal_handle"); + RCLCPP_WARN(logger(), "cancelGoal called on an empty goal_handle"); return; } - auto future_result = action_client_->async_get_result(goal_handle_); - auto future_cancel = action_client_->async_cancel_goal(goal_handle_); + auto& executor = client_instance_->callback_executor; + auto& action_client = client_instance_->action_client; + + auto future_result = action_client->async_get_result(goal_handle_); + auto future_cancel = action_client->async_cancel_goal(goal_handle_); + + constexpr auto SUCCESS = rclcpp::FutureReturnCode::SUCCESS; - if(callback_group_executor_.spin_until_future_complete( - future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) + if(executor.spin_until_future_complete(future_cancel, server_timeout_) != SUCCESS) { - RCLCPP_ERROR(node_->get_logger(), "Failed to cancel action server for [%s]", - prev_action_name_.c_str()); + RCLCPP_ERROR(logger(), "Failed to cancel action server for [%s]", + action_name_.c_str()); } - if(callback_group_executor_.spin_until_future_complete( - future_result, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) + if(executor.spin_until_future_complete(future_result, server_timeout_) != SUCCESS) { - RCLCPP_ERROR(node_->get_logger(), "Failed to get result call failed :( for [%s]", - prev_action_name_.c_str()); + RCLCPP_ERROR(logger(), "Failed to get result call failed :( for [%s]", + action_name_.c_str()); } } diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 3ceae00..ef986c6 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -263,8 +263,8 @@ inline NodeStatus RosServiceNode::tick() auto CheckStatus = [](NodeStatus status) { if(!isStatusCompleted(status)) { - throw std::logic_error("RosServiceNode: the callback must return either SUCCESS or " - "FAILURE"); + throw LogicError("RosServiceNode: the callback must return either SUCCESS nor " + "FAILURE"); } return status; }; @@ -329,7 +329,7 @@ inline NodeStatus RosServiceNode::tick() if(!response_) { - throw std::runtime_error("Request was rejected by the service"); + throw RuntimeError("Request was rejected by the service"); } } } diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index c1a8939..49390d0 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -77,7 +77,7 @@ class RosTopicSubNode : public BT::ConditionNode } std::shared_ptr node_; - std::shared_ptr sub_instance_ = nullptr; + std::shared_ptr sub_instance_; std::shared_ptr last_msg_; std::string topic_name_; boost::signals2::connection signal_connection_; From fa151bc6d8e6907b5a1fcf731f925ee8a283b461 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 24 Apr 2024 17:11:19 +0200 Subject: [PATCH 15/44] merging changes from PR #46: registry for service clients --- .../behaviortree_ros2/bt_action_node.hpp | 2 +- .../behaviortree_ros2/bt_service_node.hpp | 104 ++++++++++++++---- .../behaviortree_ros2/bt_topic_sub_node.hpp | 2 +- 3 files changed, 84 insertions(+), 24 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index f41f6a8..5addc26 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -306,7 +306,7 @@ inline bool RosActionNode::createClient(const std::string& action_name) auto& registry = getRegistry(); auto it = registry.find(action_client_key_); - if(it == registry.end()) + if(it == registry.end() || it->second.expired()) { client_instance_ = std::make_shared(node_, action_name); registry.insert({ action_client_key_, client_instance_ }); diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index ef986c6..3a40203 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -74,6 +74,7 @@ class RosServiceNode : public BT::ActionNodeBase public: // Type definitions using ServiceClient = typename rclcpp::Client; + using ServiceClientPtr = std::shared_ptr; using Request = typename ServiceT::Request; using Response = typename ServiceT::Response; @@ -140,17 +141,44 @@ class RosServiceNode : public BT::ActionNodeBase } protected: + struct ServiceClientInstance + { + ServiceClientInstance(std::shared_ptr node, + const std::string& service_name); + + ServiceClientPtr service_client; + rclcpp::CallbackGroup::SharedPtr callback_group; + rclcpp::executors::SingleThreadedExecutor callback_executor; + }; + + static std::mutex& getMutex() + { + static std::mutex action_client_mutex; + return action_client_mutex; + } + + rclcpp::Logger logger() + { + return node_->get_logger(); + } + + using ClientsRegistry = + std::unordered_map>; + // contains the fully-qualified name of the node and the name of the client + static ClientsRegistry& getRegistry() + { + static ClientsRegistry clients_registry; + return clients_registry; + } + std::shared_ptr node_; - std::string prev_service_name_; + std::string service_name_; bool service_name_may_change_ = false; const std::chrono::milliseconds service_timeout_; const std::chrono::milliseconds wait_for_service_timeout_; private: - typename std::shared_ptr service_client_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - + std::shared_ptr srv_instance_; std::shared_future future_response_; rclcpp::Time time_request_sent_; @@ -165,6 +193,18 @@ class RosServiceNode : public BT::ActionNodeBase //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- +template +inline RosServiceNode::ServiceClientInstance::ServiceClientInstance( + std::shared_ptr node, const std::string& service_name) +{ + callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + callback_executor.add_callback_group(callback_group, node->get_node_base_interface()); + + service_client = node->create_client(service_name, rmw_qos_profile_services_default, + callback_group); +} + template inline RosServiceNode::RosServiceNode(const std::string& instance_name, const NodeConfig& conf, @@ -227,19 +267,31 @@ inline bool RosServiceNode::createClient(const std::string& service_name) throw RuntimeError("service_name is empty"); } - callback_group_ = - node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_executor_.add_callback_group(callback_group_, - node_->get_node_base_interface()); - service_client_ = node_->create_client( - service_name, rmw_qos_profile_services_default, callback_group_); - prev_service_name_ = service_name; + std::unique_lock lk(getMutex()); + auto client_key = std::string(node_->get_fully_qualified_name()) + "/" + service_name; + + auto& registry = getRegistry(); + auto it = registry.find(client_key); + if(it == registry.end() || it->second.expired() + { + srv_instance_ = std::make_shared(node_, service_name); + registry.insert({ client_key, srv_instance_ }); + + RCLCPP_INFO(logger(), "Node [%s] created service client [%s]", name().c_str(), + service_name.c_str()); + } + else + { + auto prev_instance srv_instance_ = it->second.lock(); + } + service_name_ = service_name; - bool found = service_client_->wait_for_service(wait_for_service_timeout_); + bool found = + srv_instance_->service_client->wait_for_service(wait_for_service_timeout_); if(!found) { RCLCPP_ERROR(node_->get_logger(), "%s: Service with name '%s' is not reachable.", - name().c_str(), prev_service_name_.c_str()); + name().c_str(), service_name_.c_str()); } return found; } @@ -247,14 +299,20 @@ inline bool RosServiceNode::createClient(const std::string& service_name) template inline NodeStatus RosServiceNode::tick() { - // First, check if the service_client_ is valid and that the name of the + if(!rclcpp::ok()) + { + halt(); + return NodeStatus::FAILURE; + } + + // First, check if the service_client is valid and that the name of the // service_name in the port didn't change. // otherwise, create a new client - if(!service_client_ || (status() == NodeStatus::IDLE && service_name_may_change_)) + if(!srv_instance_ || (status() == NodeStatus::IDLE && service_name_may_change_)) { std::string service_name; getInput("service_name", service_name); - if(prev_service_name_ != service_name) + if(service_name_ != service_name) { createClient(service_name); } @@ -287,10 +345,12 @@ inline NodeStatus RosServiceNode::tick() } // Check if server is ready - if(!service_client_->service_is_ready()) + if(!srv_instance_->service_client->service_is_ready()) + { return onFailure(SERVICE_UNREACHABLE); + } - future_response_ = service_client_->async_send_request(request).share(); + future_response_ = srv_instance_->service_client->async_send_request(request).share(); time_request_sent_ = node_->now(); return NodeStatus::RUNNING; @@ -298,7 +358,7 @@ inline NodeStatus RosServiceNode::tick() if(status() == NodeStatus::RUNNING) { - callback_group_executor_.spin_some(); + srv_instance_->callback_executor.spin_some(); // FIRST case: check if the goal request has a timeout if(!response_received_) @@ -307,8 +367,8 @@ inline NodeStatus RosServiceNode::tick() auto const timeout = rclcpp::Duration::from_seconds(double(service_timeout_.count()) / 1000); - auto ret = - callback_group_executor_.spin_until_future_complete(future_response_, nodelay); + auto ret = srv_instance_->callback_executor.spin_until_future_complete( + future_response_, nodelay); if(ret != rclcpp::FutureReturnCode::SUCCESS) { diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 49390d0..1684f21 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -248,7 +248,7 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) auto& registry = getRegistry(); auto it = registry.find(subscriber_key_); - if(it == registry.end()) + if(it == registry.end() || it->second.expired()) { sub_instance_ = std::make_shared(node_, topic_name); registry.insert({ subscriber_key_, sub_instance_ }); From 784d4e9e6e90995f8cdfb0e9b185d199ad5049b4 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 24 Apr 2024 17:20:00 +0200 Subject: [PATCH 16/44] fix previous commit --- .../include/behaviortree_ros2/bt_service_node.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 3a40203..d7ba49c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -272,7 +272,7 @@ inline bool RosServiceNode::createClient(const std::string& service_name) auto& registry = getRegistry(); auto it = registry.find(client_key); - if(it == registry.end() || it->second.expired() + if(it == registry.end() || it->second.expired()) { srv_instance_ = std::make_shared(node_, service_name); registry.insert({ client_key, srv_instance_ }); @@ -282,12 +282,11 @@ inline bool RosServiceNode::createClient(const std::string& service_name) } else { - auto prev_instance srv_instance_ = it->second.lock(); + srv_instance_ = it->second.lock(); } service_name_ = service_name; - bool found = - srv_instance_->service_client->wait_for_service(wait_for_service_timeout_); + bool found = srv_instance_->service_client->wait_for_service(wait_for_service_timeout_); if(!found) { RCLCPP_ERROR(node_->get_logger(), "%s: Service with name '%s' is not reachable.", From 374edcf54393e8ac4486c573334ab35b58a95814 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 30 Apr 2024 12:51:46 +0200 Subject: [PATCH 17/44] Fix issue #58: use weak_ptr to avoid circular shared_ptr ownership --- .../behaviortree_ros2/bt_action_node.hpp | 26 ++++++++++++---- .../behaviortree_ros2/bt_service_node.hpp | 30 ++++++++++++++----- .../behaviortree_ros2/bt_topic_pub_node.hpp | 2 +- .../behaviortree_ros2/bt_topic_sub_node.hpp | 15 +++++++--- .../behaviortree_ros2/ros_node_params.hpp | 2 +- btcpp_ros2_samples/src/sleep_action.cpp | 7 ++--- 6 files changed, 58 insertions(+), 24 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 5addc26..be892de 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -188,7 +188,20 @@ class RosActionNode : public BT::ActionNodeBase rclcpp::Logger logger() { - return node_->get_logger(); + if(auto node = node_.lock()) + { + return node->get_logger(); + } + return rclcpp::get_logger("RosActionNode"); + } + + rclcpp::Time now() + { + if(auto node = node_.lock()) + { + return node->now(); + } + return rclcpp::Clock(RCL_ROS_TIME).now(); } using ClientsRegistry = @@ -200,7 +213,7 @@ class RosActionNode : public BT::ActionNodeBase return action_clients_registry; } - std::shared_ptr node_; + std::weak_ptr node_; std::shared_ptr client_instance_; std::string action_name_; bool action_name_may_change_ = false; @@ -302,13 +315,14 @@ inline bool RosActionNode::createClient(const std::string& action_name) } std::unique_lock lk(getMutex()); - action_client_key_ = std::string(node_->get_fully_qualified_name()) + "/" + action_name; + auto node = node_.lock(); + action_client_key_ = std::string(node->get_fully_qualified_name()) + "/" + action_name; auto& registry = getRegistry(); auto it = registry.find(action_client_key_); if(it == registry.end() || it->second.expired()) { - client_instance_ = std::make_shared(node_, action_name); + client_instance_ = std::make_shared(node, action_name); registry.insert({ action_client_key_, client_instance_ }); } else @@ -421,7 +435,7 @@ inline NodeStatus RosActionNode::tick() } future_goal_handle_ = action_client->async_send_goal(goal, goal_options); - time_goal_sent_ = node_->now(); + time_goal_sent_ = now(); return NodeStatus::RUNNING; } @@ -442,7 +456,7 @@ inline NodeStatus RosActionNode::tick() future_goal_handle_, nodelay); if(ret != rclcpp::FutureReturnCode::SUCCESS) { - if((node_->now() - time_goal_sent_) > timeout) + if((now() - time_goal_sent_) > timeout) { return CheckStatus(onFailure(SEND_GOAL_TIMEOUT)); } diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index d7ba49c..93e14ea 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -159,7 +159,20 @@ class RosServiceNode : public BT::ActionNodeBase rclcpp::Logger logger() { - return node_->get_logger(); + if(auto node = node_.lock()) + { + return node->get_logger(); + } + return rclcpp::get_logger("RosServiceNode"); + } + + rclcpp::Time now() + { + if(auto node = node_.lock()) + { + return node->now(); + } + return rclcpp::Clock(RCL_ROS_TIME).now(); } using ClientsRegistry = @@ -171,7 +184,7 @@ class RosServiceNode : public BT::ActionNodeBase return clients_registry; } - std::shared_ptr node_; + std::weak_ptr node_; std::string service_name_; bool service_name_may_change_ = false; const std::chrono::milliseconds service_timeout_; @@ -268,13 +281,14 @@ inline bool RosServiceNode::createClient(const std::string& service_name) } std::unique_lock lk(getMutex()); - auto client_key = std::string(node_->get_fully_qualified_name()) + "/" + service_name; + auto node = node_.lock(); + auto client_key = std::string(node->get_fully_qualified_name()) + "/" + service_name; auto& registry = getRegistry(); auto it = registry.find(client_key); if(it == registry.end() || it->second.expired()) { - srv_instance_ = std::make_shared(node_, service_name); + srv_instance_ = std::make_shared(node, service_name); registry.insert({ client_key, srv_instance_ }); RCLCPP_INFO(logger(), "Node [%s] created service client [%s]", name().c_str(), @@ -289,8 +303,8 @@ inline bool RosServiceNode::createClient(const std::string& service_name) bool found = srv_instance_->service_client->wait_for_service(wait_for_service_timeout_); if(!found) { - RCLCPP_ERROR(node_->get_logger(), "%s: Service with name '%s' is not reachable.", - name().c_str(), service_name_.c_str()); + RCLCPP_ERROR(logger(), "%s: Service with name '%s' is not reachable.", name().c_str(), + service_name_.c_str()); } return found; } @@ -350,7 +364,7 @@ inline NodeStatus RosServiceNode::tick() } future_response_ = srv_instance_->service_client->async_send_request(request).share(); - time_request_sent_ = node_->now(); + time_request_sent_ = now(); return NodeStatus::RUNNING; } @@ -371,7 +385,7 @@ inline NodeStatus RosServiceNode::tick() if(ret != rclcpp::FutureReturnCode::SUCCESS) { - if((node_->now() - time_request_sent_) > timeout) + if((now() - time_request_sent_) > timeout) { return CheckStatus(onFailure(SERVICE_TIMEOUT)); } diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp index 282259e..8ccdc69 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp @@ -39,7 +39,7 @@ class RosTopicPubNode : public BT::ConditionNode /** You are not supposed to instantiate this class directly, the factory will do it. * To register this class into the factory, use: * - * RegisterRosAction(factory, params) + * RegisterRosAction(factory, params) * * Note that if the external_action_client is not set, the constructor will build its own. * */ diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 1684f21..a8a34fc 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -76,7 +76,7 @@ class RosTopicSubNode : public BT::ConditionNode return subscribers_registry; } - std::shared_ptr node_; + std::weak_ptr node_; std::shared_ptr sub_instance_; std::shared_ptr last_msg_; std::string topic_name_; @@ -85,7 +85,11 @@ class RosTopicSubNode : public BT::ConditionNode rclcpp::Logger logger() { - return node_->get_logger(); + if(auto node = node_.lock()) + { + return node->get_logger(); + } + return rclcpp::get_logger("RosTopicSubNode"); } public: @@ -244,13 +248,16 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) // find SubscriberInstance in the registry std::unique_lock lk(registryMutex()); - subscriber_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name; + + auto shared_node = node_.lock(); + subscriber_key_ = + std::string(shared_node->get_fully_qualified_name()) + "/" + topic_name; auto& registry = getRegistry(); auto it = registry.find(subscriber_key_); if(it == registry.end() || it->second.expired()) { - sub_instance_ = std::make_shared(node_, topic_name); + sub_instance_ = std::make_shared(shared_node, topic_name); registry.insert({ subscriber_key_, sub_instance_ }); RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(), diff --git a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp index 6ac9e63..484429b 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp @@ -24,7 +24,7 @@ namespace BT struct RosNodeParams { - std::shared_ptr nh; + std::weak_ptr nh; // This has different meaning based on the context: // diff --git a/btcpp_ros2_samples/src/sleep_action.cpp b/btcpp_ros2_samples/src/sleep_action.cpp index 1460806..9b8112e 100644 --- a/btcpp_ros2_samples/src/sleep_action.cpp +++ b/btcpp_ros2_samples/src/sleep_action.cpp @@ -10,7 +10,7 @@ bool SleepAction::setGoal(RosActionNode::Goal& goal) NodeStatus SleepAction::onResultReceived(const RosActionNode::WrappedResult& wr) { - RCLCPP_INFO(node_->get_logger(), "%s: onResultReceived. Done = %s", name().c_str(), + RCLCPP_INFO(logger(), "%s: onResultReceived. Done = %s", name().c_str(), wr.result->done ? "true" : "false"); return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE; @@ -18,14 +18,13 @@ NodeStatus SleepAction::onResultReceived(const RosActionNode::WrappedResult& wr) NodeStatus SleepAction::onFailure(ActionNodeErrorCode error) { - RCLCPP_ERROR(node_->get_logger(), "%s: onFailure with error: %s", name().c_str(), - toStr(error)); + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); return NodeStatus::FAILURE; } void SleepAction::onHalt() { - RCLCPP_INFO(node_->get_logger(), "%s: onHalt", name().c_str()); + RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); } // Plugin registration. From 3183c9d5b47c5cbf794d73c03ae3b210c8f253b7 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Thu, 2 May 2024 16:11:40 +0200 Subject: [PATCH 18/44] WIP: added an ActionServerBT to execute trees --- behaviortree_ros2/CMakeLists.txt | 49 +++- .../include/behaviortree_ros2/bt_utils.hpp | 82 ++++++ .../tree_execution_server.hpp | 132 +++++++++ behaviortree_ros2/package.xml | 4 + .../src/bt_executor_parameters.yaml | 49 ++++ behaviortree_ros2/src/bt_utils.cpp | 173 ++++++++++++ .../src/tree_execution_server.cpp | 263 ++++++++++++++++++ btcpp_ros2_interfaces/CMakeLists.txt | 2 + .../action/ExecuteTree.action | 9 + btcpp_ros2_interfaces/msgs/NodeStatus.msg | 8 + btcpp_ros2_samples/CMakeLists.txt | 11 +- .../behavior_trees/cross_door.xml | 13 + .../behavior_trees/door_closed.xml | 11 + .../behavior_trees/sleep_action.xml | 15 + .../config/sample_bt_executor.yaml | 16 ++ .../launch/sample_bt_executor.launch.xml | 11 + btcpp_ros2_samples/src/sample_bt_executor.cpp | 57 ++++ 17 files changed, 891 insertions(+), 14 deletions(-) create mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp create mode 100644 behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp create mode 100644 behaviortree_ros2/src/bt_executor_parameters.yaml create mode 100644 behaviortree_ros2/src/bt_utils.cpp create mode 100644 behaviortree_ros2/src/tree_execution_server.cpp create mode 100644 btcpp_ros2_interfaces/action/ExecuteTree.action create mode 100644 btcpp_ros2_interfaces/msgs/NodeStatus.msg create mode 100644 btcpp_ros2_samples/behavior_trees/cross_door.xml create mode 100644 btcpp_ros2_samples/behavior_trees/door_closed.xml create mode 100644 btcpp_ros2_samples/behavior_trees/sleep_action.xml create mode 100644 btcpp_ros2_samples/config/sample_bt_executor.yaml create mode 100644 btcpp_ros2_samples/launch/sample_bt_executor.launch.xml create mode 100644 btcpp_ros2_samples/src/sample_bt_executor.cpp diff --git a/behaviortree_ros2/CMakeLists.txt b/behaviortree_ros2/CMakeLists.txt index 01378ff..edf1440 100644 --- a/behaviortree_ros2/CMakeLists.txt +++ b/behaviortree_ros2/CMakeLists.txt @@ -3,32 +3,57 @@ project(behaviortree_ros2) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(THIS_PACKAGE_DEPS rclcpp rclcpp_action ament_index_cpp - behaviortree_cpp) + behaviortree_cpp + btcpp_ros2_interfaces + generate_parameter_library + ) find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED ) -find_package(rclcpp_action REQUIRED ) -find_package(behaviortree_cpp REQUIRED ) -find_package(ament_index_cpp REQUIRED) - -# This is compiled only to check if there are errors in the header file -# library will not be exported -include_directories(include) -add_library(${PROJECT_NAME} src/bt_ros2.cpp) + +foreach(PACKAGE ${THIS_PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED ) +endforeach() + + +generate_parameter_library( + bt_executor_parameters + src/bt_executor_parameters.yaml) + +add_library(${PROJECT_NAME} + src/bt_ros2.cpp + src/bt_utils.cpp + src/tree_execution_server.cpp ) + ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS}) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) + +target_link_libraries(${PROJECT_NAME} bt_executor_parameters) + + ###################################################### # INSTALL install(DIRECTORY include/ DESTINATION include/) -ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_DEPS}) +install( + TARGETS ${PROJECT_NAME} bt_executor_parameters + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + ament_package() diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp new file mode 100644 index 0000000..90a93f0 --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp @@ -0,0 +1,82 @@ +// Copyright 2024 Marq Rasmussen +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to +// permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright +// notice and this permission notice shall be included in all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +#include +#include +#include + +// auto-generated header, created by generate_parameter_library +#include "bt_executor_parameters.hpp" + +#include "btcpp_ros2_interfaces/msg/node_status.hpp" + +#include "behaviortree_cpp/bt_factory.h" + +#include "behaviortree_ros2/plugins.hpp" +#include "behaviortree_ros2/ros_node_params.hpp" + +#include "rclcpp/rclcpp.hpp" +#include + +namespace action_server_bt +{ +/** + * @brief Convert BT::NodeStatus into Action Server feedback message NodeStatus + * @param status Current status of the executing BehaviorTree + * @return NodeStatus used to publish feedback to the Action Client + */ +btcpp_ros2_interfaces::msg::NodeStatus convert_node_status(BT::NodeStatus& status); + +/** + * @brief Function the uses ament_index_cpp to get the package path of the parameter specified by the user + * @param parameter_value String containing 'package_name/subfolder' for the directory path to look up + * @return Full path to the directory specified by the parameter_value + */ +std::string get_directory_path(const std::string& parameter_value); + +/** + * @brief Function to load BehaviorTree xml files from a specific directory + * @param factory BehaviorTreeFactory to register the BehaviorTrees into + * @param directory_path Full path to the directory to search for BehaviorTree definitions + */ +void load_behavior_trees(BT::BehaviorTreeFactory& factory, + const std::string& directory_path); + +/** + * @brief Function to load BehaviorTree plugins from a specific directory + * @param factory BehaviorTreeFactory to register the plugins into + * @param directory_path Full path to the directory to search for BehaviorTree plugins + */ +void load_plugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path); + +/** + * @brief Function to load BehaviorTree ROS plugins from a specific directory + * @param factory BehaviorTreeFactory to register the plugins into + * @param directory_path Full path to the directory to search for BehaviorTree plugins + * @param node node pointer that is shared with the ROS based BehaviorTree plugins + */ +void load_ros_plugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, + rclcpp::Node::SharedPtr node); + +/** + * @brief Function to register all Behaviors and BehaviorTrees from user specified packages + * @param params ROS parameters that contain lists of packages to load + * plugins, ros_plugins and BehaviorTrees from + * @param factory BehaviorTreeFactory to register into + * @param node node pointer that is shared with the ROS based Behavior plugins + */ +void register_behavior_trees(action_server_bt::Params& params, + BT::BehaviorTreeFactory& factory, + rclcpp::Node::SharedPtr node); + +} // namespace action_server_bt diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp new file mode 100644 index 0000000..be67bf8 --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -0,0 +1,132 @@ +// Copyright 2024 Marq Rasmussen +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to +// permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright +// notice and this permission notice shall be included in all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +#include +#include + +#include "btcpp_ros2_interfaces/action/execute_tree.hpp" + +#include "behaviortree_cpp/bt_factory.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +namespace action_server_bt +{ + +/** + * @brief ActionServerBT class hosts a ROS Action Server that is able + * to load Behavior plugins, BehaviorTree.xml files and execute them. + */ +class ActionServerBT +{ +public: + using ExecuteTree = btcpp_ros2_interfaces::action::ExecuteTree; + using GoalHandleExecuteTree = rclcpp_action::ServerGoalHandle; + + /** + * @brief Constructor for ActionServerBT. + * @details This initializes a ParameterListener to read configurable options from the user and + * starts an Action Server that takes requests to execute BehaviorTrees. + * + * @param options rclcpp::NodeOptions to pass to node_ when initializing it. + * after the tree is created, while its running and after it finishes. + */ + explicit ActionServerBT(const rclcpp::NodeOptions& options); + + virtual ~ActionServerBT(); + + /** + * @brief Gets the NodeBaseInterface of node_. + * @details This function exists to allow running ActionServerBT as a component in a composable node container. + * + * @return A shared_ptr to the NodeBaseInterface of node_. + */ + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr nodeBaseInterface(); + + // name of the tree being executed + const std::string& currentTreeName() const; + + // tree being executed, nullptr if it doesn't exist yet. + BT::Tree* currentTree(); + + // pointer to the global blackboard + BT::Blackboard::Ptr globalBlackboard(); + +protected: + // To be overridden by the user. + // Callback invoked when the tree is created and before it is executed, + // Can be used to update the blackboard or to attach loggers. + virtual void onTreeCreated(BT::Tree& tree) + {} + + // To be overridden by the user. + // In addition to the built in mechanism to register nodes from plugins, + // you can use this method to register custom nodes into the factory. + virtual void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) + {} + + // To be overridden by the user. + // Callback invoked after the tickOnce. + // If it returns something different than std::nullopt, the tree execution will + // be halted and the returned value will be the optional NodeStatus. + virtual std::optional onLoopAfterTick(BT::NodeStatus status) + { + return std::nullopt; + } + + // To be overridden by the user. + // Callback invoked when the tree execution is completed + virtual void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) + {} + + virtual std::optional onLoopFeedback() + { + return std::nullopt; + } + +private: + struct Pimpl; + std::unique_ptr p_; + + /** + * @brief handle the goal requested: accept or reject. This implementation always accepts. + * @param uuid Goal ID + * @param goal A shared pointer to the specific goal + * @return GoalResponse response of the goal processed + */ + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + + /** + * @brief Accepts cancellation requests of action server. + * @param goal A server goal handle to cancel + * @return CancelResponse response of the goal cancelled + */ + rclcpp_action::CancelResponse + handle_cancel(const std::shared_ptr goal_handle); + + /** + * @brief Handles accepted goals and starts a thread to process them + * @param goal_handle Server goal handle to process feedback and set the response when finished + */ + void handle_accepted(const std::shared_ptr goal_handle); + + /** + * @brief Background processes to execute the BehaviorTree and handle stop requests + * @param goal_handle Server goal handle to process feedback and set the response when finished + */ + void execute(const std::shared_ptr goal_handle); +}; + +} // namespace action_server_bt diff --git a/behaviortree_ros2/package.xml b/behaviortree_ros2/package.xml index 59c0de5..fb1dcc8 100644 --- a/behaviortree_ros2/package.xml +++ b/behaviortree_ros2/package.xml @@ -11,10 +11,14 @@ ament_cmake + fmt libboost-dev + rclcpp rclcpp_action behaviortree_cpp + generate_parameter_library + btcpp_ros2_interfaces ament_cmake diff --git a/behaviortree_ros2/src/bt_executor_parameters.yaml b/behaviortree_ros2/src/bt_executor_parameters.yaml new file mode 100644 index 0000000..1364d8f --- /dev/null +++ b/behaviortree_ros2/src/bt_executor_parameters.yaml @@ -0,0 +1,49 @@ +action_server_bt: + action_name: { + type: string, + default_value: "action_server_bt", + read_only: true, + description: "The name the Action Server takes requests from", + } + behavior_tick_frequency: { + type: int, + default_value: 100, + read_only: true, + description: "Frequency in Hz to tick() the Behavior tree at", + validation: { + bounds<>: [1, 1000] + } + } + groot2_port: { + type: int, + default_value: 1667, + read_only: true, + description: "Server port value to publish Groot2 messages on", + validation: { + bounds<>: [1, 49151] + } + } + plugins: { + type: string_array, + default_value: [], + description: "List of 'package_name/subfolder' containing BehaviorTree plugins to load into the factory", + validation: { + unique<>: null, + } + } + ros_plugins: { + type: string_array, + default_value: [], + description: "List of 'package_name/subfolder' containing BehaviorTree ROS plugins to load into the factory", + validation: { + unique<>: null, + } + } + behavior_trees: { + type: string_array, + default_value: [], + description: "List of 'package_name/subfolder' containing SubTrees to load into the BehaviorTree factory", + validation: { + unique<>: null, + } + } diff --git a/behaviortree_ros2/src/bt_utils.cpp b/behaviortree_ros2/src/bt_utils.cpp new file mode 100644 index 0000000..6c72ec9 --- /dev/null +++ b/behaviortree_ros2/src/bt_utils.cpp @@ -0,0 +1,173 @@ +// Copyright 2024 Marq Rasmussen +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to +// permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright +// notice and this permission notice shall be included in all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +#include "behaviortree_ros2/bt_utils.hpp" + +namespace +{ +static const auto kLogger = rclcpp::get_logger("action_server_bt"); +} + +namespace action_server_bt +{ + +btcpp_ros2_interfaces::msg::NodeStatus convert_node_status(BT::NodeStatus& status) +{ + btcpp_ros2_interfaces::msg::NodeStatus action_status; + switch(status) + { + case BT::NodeStatus::RUNNING: + action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::RUNNING; + case BT::NodeStatus::SUCCESS: + action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::SUCCESS; + case BT::NodeStatus::FAILURE: + action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::FAILURE; + case BT::NodeStatus::IDLE: + action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::IDLE; + case BT::NodeStatus::SKIPPED: + action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::SKIPPED; + } + + return action_status; +} + +std::string get_directory_path(const std::string& parameter_value) +{ + std::string package_name, subfolder; + auto pos = parameter_value.find_first_of("/"); + if(pos == parameter_value.size()) + { + RCLCPP_ERROR(kLogger, "Invalid Parameter: %s. Missing subfolder delimiter '/'.", + parameter_value.c_str()); + return ""; + } + + package_name = std::string(parameter_value.begin(), parameter_value.begin() + pos); + subfolder = std::string(parameter_value.begin() + pos + 1, parameter_value.end()); + try + { + std::string search_directory = + ament_index_cpp::get_package_share_directory(package_name) + "/" + subfolder; + RCLCPP_DEBUG(kLogger, "Searching for Plugins/BehaviorTrees in path: %s", + search_directory.c_str()); + return search_directory; + } + catch(const std::exception& e) + { + RCLCPP_ERROR(kLogger, "Failed to find package: %s \n %s", package_name.c_str(), + e.what()); + } + return ""; +} + +void load_behavior_trees(BT::BehaviorTreeFactory& factory, + const std::string& directory_path) +{ + using std::filesystem::directory_iterator; + for(const auto& entry : directory_iterator(directory_path)) + { + if(entry.path().extension() == ".xml") + { + try + { + factory.registerBehaviorTreeFromFile(entry.path().string()); + RCLCPP_INFO(kLogger, "Loaded BehaviorTree: %s", entry.path().filename().c_str()); + } + catch(const std::exception& e) + { + RCLCPP_ERROR(kLogger, "Failed to load BehaviorTree: %s \n %s", + entry.path().filename().c_str(), e.what()); + } + } + } +} + +void load_plugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path) +{ + using std::filesystem::directory_iterator; + for(const auto& entry : directory_iterator(directory_path)) + { + if(entry.path().extension() == ".so") + { + try + { + factory.registerFromPlugin(entry.path().string()); + RCLCPP_INFO(kLogger, "Loaded Plugin: %s", entry.path().filename().c_str()); + } + catch(const std::exception& e) + { + RCLCPP_ERROR(kLogger, "Failed to load Plugin: %s \n %s", + entry.path().filename().c_str(), e.what()); + } + } + } +} + +void load_ros_plugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, + rclcpp::Node::SharedPtr node) +{ + using std::filesystem::directory_iterator; + BT::RosNodeParams params; + params.nh = node; + for(const auto& entry : directory_iterator(directory_path)) + { + if(entry.path().extension() == ".so") + { + try + { + RegisterRosNode(factory, entry.path().string(), params); + RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", entry.path().filename().c_str()); + } + catch(const std::exception& e) + { + RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", + entry.path().filename().c_str(), e.what()); + } + } + } +} + +void register_behavior_trees(action_server_bt::Params& params, + BT::BehaviorTreeFactory& factory, + rclcpp::Node::SharedPtr node) +{ + // clear the factory and load/reload it with the Behaviors and Trees specified by the user in their action_server_bt config yaml + factory.clearRegisteredBehaviorTrees(); + + for(const auto& plugin : params.plugins) + { + const auto plugin_directory = get_directory_path(plugin); + // skip invalid plugins directories + if(plugin_directory.empty()) + continue; + load_plugins(factory, plugin_directory); + } + for(const auto& plugin : params.ros_plugins) + { + const auto plugin_directory = get_directory_path(plugin); + // skip invalid plugins directories + if(plugin_directory.empty()) + continue; + load_ros_plugins(factory, plugin_directory, node); + } + for(const auto& tree_dir : params.behavior_trees) + { + const auto tree_directory = get_directory_path(tree_dir); + // skip invalid subtree directories + if(tree_directory.empty()) + continue; + load_behavior_trees(factory, tree_directory); + } +} + +} // namespace action_server_bt diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp new file mode 100644 index 0000000..46a09f1 --- /dev/null +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -0,0 +1,263 @@ +// Copyright 2024 Marq Rasmussen +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to +// permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright +// notice and this permission notice shall be included in all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +#include + +#include "behaviortree_ros2/tree_execution_server.hpp" +#include "behaviortree_ros2/bt_utils.hpp" + +#include "behaviortree_cpp/loggers/groot2_publisher.h" + +// generated file +#include "bt_executor_parameters.hpp" +namespace +{ +static const auto kLogger = rclcpp::get_logger("action_server_bt"); +} + +namespace action_server_bt +{ + +struct ActionServerBT::Pimpl +{ + Pimpl(const rclcpp::NodeOptions& node_options); + + rclcpp::Node::SharedPtr node; + rclcpp_action::Server::SharedPtr action_server; + std::thread action_thread; + // thread safe bool to keep track of cancel requests + std::atomic cancel_requested{ false }; + + std::shared_ptr param_listener; + action_server_bt::Params params; + + BT::BehaviorTreeFactory factory; + std::shared_ptr groot_publisher; + + std::string current_tree_name; + std::shared_ptr tree; + BT::Blackboard::Ptr global_blackboard; + + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse + handle_cancel(const std::shared_ptr goal_handle); + + void handle_accepted(const std::shared_ptr goal_handle); + + void execute(const std::shared_ptr goal_handle); +}; + +ActionServerBT::Pimpl::Pimpl(const rclcpp::NodeOptions& node_options) + : node(std::make_unique("action_server_bt", node_options)) +{ + param_listener = std::make_shared(node); + params = param_listener->get_params(); + global_blackboard = BT::Blackboard::create(); +} + +ActionServerBT::~ActionServerBT() +{} + +ActionServerBT::ActionServerBT(const rclcpp::NodeOptions& options) + : p_(new Pimpl(options)) +{ + // create the action server + const auto action_name = p_->params.action_name; + RCLCPP_INFO(kLogger, "Starting Action Server: %s", action_name.c_str()); + p_->action_server = rclcpp_action::create_server( + p_->node, action_name, + [this](const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) { + return handle_goal(uuid, std::move(goal)); + }, + [this](const std::shared_ptr goal_handle) { + return handle_cancel(std::move(goal_handle)); + }, + [this](const std::shared_ptr goal_handle) { + handle_accepted(std::move(goal_handle)); + }); + + // register the users Plugins and BehaviorTree.xml files into the factory + register_behavior_trees(p_->params, p_->factory, p_->node); + registerNodesIntoFactory(p_->factory); +} + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ActionServerBT::nodeBaseInterface() +{ + return p_->node->get_node_base_interface(); +} + +rclcpp_action::GoalResponse +ActionServerBT::handle_goal(const rclcpp_action::GoalUUID& /* uuid */, + std::shared_ptr goal) +{ + RCLCPP_INFO(kLogger, "Received goal request to execute Behavior Tree: %s", + goal->target_tree.c_str()); + p_->cancel_requested = false; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse +ActionServerBT::handle_cancel(const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(kLogger, "Received request to cancel goal"); + if(!goal_handle->is_active()) + { + RCLCPP_WARN(kLogger, "Rejecting request to cancel goal because the server is not " + "processing one."); + return rclcpp_action::CancelResponse::REJECT; + } + p_->cancel_requested = true; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void ActionServerBT::handle_accepted( + const std::shared_ptr goal_handle) +{ + // Join the previous execute thread before replacing it with a new one + if(p_->action_thread.joinable()) + { + p_->action_thread.join(); + } + // To avoid blocking the executor start a new thread to process the goal + p_->action_thread = std::thread{ [=]() { execute(goal_handle); } }; +} + +void ActionServerBT::execute(const std::shared_ptr goal_handle) +{ + const auto goal = goal_handle->get_goal(); + BT::NodeStatus status = BT::NodeStatus::RUNNING; + auto action_result = std::make_shared(); + + // Before executing check if we have new Behaviors or Subtrees to reload + if(p_->param_listener->is_old(p_->params)) + { + p_->params = p_->param_listener->get_params(); + register_behavior_trees(p_->params, p_->factory, p_->node); + registerNodesIntoFactory(p_->factory); + } + + // Loop until something happens with ROS or the node completes + try + { + // This blackboard will be owned by "MainTree". It parent is p_->global_blackboard + auto root_blackboard = BT::Blackboard::create(p_->global_blackboard); + + p_->tree = std::make_shared(); + *(p_->tree) = p_->factory.createTree(goal->target_tree, root_blackboard); + p_->current_tree_name = goal->target_tree; + + // call user defined function after the tree has been created + onTreeCreated(*p_->tree); + p_->groot_publisher.reset(); + p_->groot_publisher = + std::make_shared(*(p_->tree), p_->params.groot2_port); + + // Loop until the tree is done or a cancel is requested + const auto period = std::chrono::milliseconds( + static_cast(1000.0 / p_->params.behavior_tick_frequency)); + auto loop_deadline = std::chrono::steady_clock::now() + period; + + // operations to be done if the tree execution is aborted, either by + // cancel_requested_ or by onLoopAfterTick() + auto stop_action = [this, &action_result](BT::NodeStatus status, + const std::string& message) { + action_result->node_status = convert_node_status(status); + action_result->error_message = message; + RCLCPP_WARN(kLogger, action_result->error_message.c_str()); + p_->tree->haltTree(); + onTreeExecutionCompleted(status, true); + }; + + while(rclcpp::ok() && status == BT::NodeStatus::RUNNING) + { + if(p_->cancel_requested) + { + stop_action(status, "Action Server canceling and halting Behavior Tree"); + goal_handle->canceled(action_result); + return; + } + + // tick the tree once and publish the action feedback + status = p_->tree->tickExactlyOnce(); + + if(const auto res = onLoopAfterTick(status); res.has_value()) + { + stop_action(res.value(), "Action Server aborted by onLoopAfterTick()"); + goal_handle->abort(action_result); + return; + } + + if(const auto res = onLoopFeedback(); res.has_value()) + { + auto feedback = std::make_shared(); + feedback->msg = res.value(); + goal_handle->publish_feedback(feedback); + } + + const auto now = std::chrono::steady_clock::now(); + if(now < loop_deadline) + { + p_->tree->sleep(loop_deadline - now); + } + loop_deadline += period; + } + } + catch(const std::exception& ex) + { + action_result->error_message = "Behavior Tree exception: %s", ex.what(); + RCLCPP_ERROR(kLogger, action_result->error_message.c_str()); + goal_handle->abort(action_result); + return; + } + + // call user defined execution complete function + onTreeExecutionCompleted(status, false); + + // set the node_status result to the action + action_result->node_status = convert_node_status(status); + + // return success or aborted for the action result + if(status == BT::NodeStatus::SUCCESS) + { + RCLCPP_INFO(kLogger, "BT finished with status: %s", BT::toStr(status).c_str()); + goal_handle->succeed(action_result); + } + else + { + action_result->error_message = "Behavior Tree failed during execution with status: " + "%s", + BT::toStr(status); + RCLCPP_ERROR(kLogger, action_result->error_message.c_str()); + goal_handle->abort(action_result); + } +} + +const std::string& ActionServerBT::currentTreeName() const +{ + return p_->current_tree_name; +} + +BT::Tree* ActionServerBT::currentTree() +{ + return p_->tree ? p_->tree.get() : nullptr; +} + +BT::Blackboard::Ptr ActionServerBT::globalBlackboard() +{ + return p_->global_blackboard; +} + +} // namespace action_server_bt diff --git a/btcpp_ros2_interfaces/CMakeLists.txt b/btcpp_ros2_interfaces/CMakeLists.txt index 49080ee..72612f3 100644 --- a/btcpp_ros2_interfaces/CMakeLists.txt +++ b/btcpp_ros2_interfaces/CMakeLists.txt @@ -8,6 +8,8 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(btcpp_ros2_interfaces + "msgs/NodeStatus.msg" + "action/ExecuteTree.action" "action/Sleep.action") ament_export_dependencies(rosidl_default_runtime) diff --git a/btcpp_ros2_interfaces/action/ExecuteTree.action b/btcpp_ros2_interfaces/action/ExecuteTree.action new file mode 100644 index 0000000..d12d6f6 --- /dev/null +++ b/btcpp_ros2_interfaces/action/ExecuteTree.action @@ -0,0 +1,9 @@ +# Request +string target_tree +--- +# Result +string error_message +NodeStatus node_status +--- +# Feedback. This can be customized by the user +string msg diff --git a/btcpp_ros2_interfaces/msgs/NodeStatus.msg b/btcpp_ros2_interfaces/msgs/NodeStatus.msg new file mode 100644 index 0000000..ba36a72 --- /dev/null +++ b/btcpp_ros2_interfaces/msgs/NodeStatus.msg @@ -0,0 +1,8 @@ +# NodeStatus Enums +uint8 IDLE = 0 +uint8 RUNNING = 1 +uint8 SUCCESS = 2 +uint8 FAILURE = 3 +uint8 SKIPPED = 4 + +uint8 status diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index 6d4987b..084da70 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -7,8 +7,8 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(ament_cmake REQUIRED) find_package(behaviortree_ros2 REQUIRED) -find_package(std_msgs REQUIRED) find_package(btcpp_ros2_interfaces REQUIRED) +find_package(std_msgs REQUIRED) set(THIS_PACKAGE_DEPS behaviortree_ros2 @@ -16,7 +16,12 @@ set(THIS_PACKAGE_DEPS btcpp_ros2_interfaces ) ###################################################### -# Build a client that call the sleep action (STATIC version) +# Simple example showing how to use and customize the BtExecutionServer +add_executable(sample_bt_executor src/sample_bt_executor.cpp) +ament_target_dependencies(sample_bt_executor ${THIS_PACKAGE_DEPS}) + +###################################################### +# Build an Action Client that calls the sleep action (STATIC version) add_executable(sleep_client src/sleep_action.cpp @@ -54,6 +59,7 @@ install(TARGETS sleep_server sleep_plugin subscriber_test + sample_bt_executor DESTINATION lib/${PROJECT_NAME} ) @@ -67,6 +73,7 @@ install(TARGETS RUNTIME DESTINATION share/${PROJECT_NAME}/bt_plugins ) + ament_export_dependencies(behaviortree_ros2 btcpp_ros2_interfaces) ament_package() diff --git a/btcpp_ros2_samples/behavior_trees/cross_door.xml b/btcpp_ros2_samples/behavior_trees/cross_door.xml new file mode 100644 index 0000000..d2cba24 --- /dev/null +++ b/btcpp_ros2_samples/behavior_trees/cross_door.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/btcpp_ros2_samples/behavior_trees/door_closed.xml b/btcpp_ros2_samples/behavior_trees/door_closed.xml new file mode 100644 index 0000000..b328a34 --- /dev/null +++ b/btcpp_ros2_samples/behavior_trees/door_closed.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/btcpp_ros2_samples/behavior_trees/sleep_action.xml b/btcpp_ros2_samples/behavior_trees/sleep_action.xml new file mode 100644 index 0000000..2e83ece --- /dev/null +++ b/btcpp_ros2_samples/behavior_trees/sleep_action.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/btcpp_ros2_samples/config/sample_bt_executor.yaml b/btcpp_ros2_samples/config/sample_bt_executor.yaml new file mode 100644 index 0000000..f07dd61 --- /dev/null +++ b/btcpp_ros2_samples/config/sample_bt_executor.yaml @@ -0,0 +1,16 @@ +action_server_bt: + ros__parameters: + action_name: "behavior_server" # Optional (defaults to `action_server_bt`) + behavior_tick_frequency: 100 # Optional (defaults to 100 Hz) + groot2_port: 1667 # Optional (defaults to 1667) + + # Below are a list plugins and BehaviorTrees to load + # (you are not required to have all 3 types) + # These are dynamic parameters and can be changed at runtime via rosparam + # see `action_server_bt/parameters.md` for documentation. + plugins: + - behaviortree_cpp/bt_plugins + ros_plugins: + - btcpp_ros2_samples/bt_plugins + behavior_trees: + - btcpp_ros2_samples/behavior_trees diff --git a/btcpp_ros2_samples/launch/sample_bt_executor.launch.xml b/btcpp_ros2_samples/launch/sample_bt_executor.launch.xml new file mode 100644 index 0000000..2052b16 --- /dev/null +++ b/btcpp_ros2_samples/launch/sample_bt_executor.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/btcpp_ros2_samples/src/sample_bt_executor.cpp b/btcpp_ros2_samples/src/sample_bt_executor.cpp new file mode 100644 index 0000000..0e04d43 --- /dev/null +++ b/btcpp_ros2_samples/src/sample_bt_executor.cpp @@ -0,0 +1,57 @@ +// Copyright 2024 Marq Rasmussen +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to +// permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright +// notice and this permission notice shall be included in all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +#include +#include + +// Example that shows how to customize ActionServerBT. +// Here, we simply add an extra logger +class MyActionServer : public action_server_bt::ActionServerBT +{ +public: + MyActionServer(const rclcpp::NodeOptions& options) : ActionServerBT(options) + {} + + void onTreeCreated(BT::Tree& tree) override + { + logger_cout_ = std::make_shared(tree); + } + + void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override + { + // NOT really needed, even if logger_cout_ may contain a dangling pointer of the tree + // at this point + logger_cout_.reset(); + } + +private: + std::shared_ptr logger_cout_; +}; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + auto action_server = std::make_shared(options); + + // TODO: This workaround is for a bug in MultiThreadedExecutor where it can deadlock when spinning without a timeout. + // Deadlock is caused when Publishers or Subscribers are dynamically removed as the node is spinning. + rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 0, false, + std::chrono::milliseconds(250)); + exec.add_node(action_server->nodeBaseInterface()); + exec.spin(); + exec.remove_node(action_server->nodeBaseInterface()); + + rclcpp::shutdown(); +} From 64a7801eb5aab14751cb9a16b1638dbb4d4fb8be Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Thu, 2 May 2024 17:08:42 +0200 Subject: [PATCH 19/44] renaming --- .../include/behaviortree_ros2/bt_utils.hpp | 30 ++++++----- .../tree_execution_server.hpp | 42 ++++++++------- .../src/bt_executor_parameters.yaml | 2 +- behaviortree_ros2/src/bt_utils.cpp | 37 +++++++------ .../src/tree_execution_server.cpp | 54 ++++++++++--------- btcpp_ros2_samples/src/sample_bt_executor.cpp | 4 +- 6 files changed, 91 insertions(+), 78 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp index 90a93f0..b4c1414 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp @@ -28,55 +28,61 @@ #include "rclcpp/rclcpp.hpp" #include -namespace action_server_bt +namespace BT { /** * @brief Convert BT::NodeStatus into Action Server feedback message NodeStatus + * * @param status Current status of the executing BehaviorTree * @return NodeStatus used to publish feedback to the Action Client */ -btcpp_ros2_interfaces::msg::NodeStatus convert_node_status(BT::NodeStatus& status); +btcpp_ros2_interfaces::msg::NodeStatus ConvertNodeStatus(BT::NodeStatus& status); /** * @brief Function the uses ament_index_cpp to get the package path of the parameter specified by the user + * * @param parameter_value String containing 'package_name/subfolder' for the directory path to look up * @return Full path to the directory specified by the parameter_value */ -std::string get_directory_path(const std::string& parameter_value); +std::string GetDirectoryPath(const std::string& parameter_value); /** * @brief Function to load BehaviorTree xml files from a specific directory + * * @param factory BehaviorTreeFactory to register the BehaviorTrees into * @param directory_path Full path to the directory to search for BehaviorTree definitions */ -void load_behavior_trees(BT::BehaviorTreeFactory& factory, - const std::string& directory_path); +void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory, + const std::string& directory_path); /** * @brief Function to load BehaviorTree plugins from a specific directory + * * @param factory BehaviorTreeFactory to register the plugins into * @param directory_path Full path to the directory to search for BehaviorTree plugins */ -void load_plugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path); +void LoadPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path); /** * @brief Function to load BehaviorTree ROS plugins from a specific directory + * * @param factory BehaviorTreeFactory to register the plugins into * @param directory_path Full path to the directory to search for BehaviorTree plugins * @param node node pointer that is shared with the ROS based BehaviorTree plugins */ -void load_ros_plugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, - rclcpp::Node::SharedPtr node); +void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, + rclcpp::Node::SharedPtr node); /** * @brief Function to register all Behaviors and BehaviorTrees from user specified packages + * * @param params ROS parameters that contain lists of packages to load * plugins, ros_plugins and BehaviorTrees from * @param factory BehaviorTreeFactory to register into * @param node node pointer that is shared with the ROS based Behavior plugins */ -void register_behavior_trees(action_server_bt::Params& params, - BT::BehaviorTreeFactory& factory, - rclcpp::Node::SharedPtr node); +void RegisterBehaviorTrees(action_server_bt::Params& params, + BT::BehaviorTreeFactory& factory, + rclcpp::Node::SharedPtr node); -} // namespace action_server_bt +} // namespace BT diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index be67bf8..950eac9 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -21,34 +21,34 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -namespace action_server_bt +namespace BT { /** - * @brief ActionServerBT class hosts a ROS Action Server that is able + * @brief TreeExecutionServer class hosts a ROS Action Server that is able * to load Behavior plugins, BehaviorTree.xml files and execute them. */ -class ActionServerBT +class TreeExecutionServer { public: using ExecuteTree = btcpp_ros2_interfaces::action::ExecuteTree; using GoalHandleExecuteTree = rclcpp_action::ServerGoalHandle; /** - * @brief Constructor for ActionServerBT. + * @brief Constructor for TreeExecutionServer. * @details This initializes a ParameterListener to read configurable options from the user and * starts an Action Server that takes requests to execute BehaviorTrees. * * @param options rclcpp::NodeOptions to pass to node_ when initializing it. * after the tree is created, while its running and after it finishes. */ - explicit ActionServerBT(const rclcpp::NodeOptions& options); + explicit TreeExecutionServer(const rclcpp::NodeOptions& options); - virtual ~ActionServerBT(); + virtual ~TreeExecutionServer(); /** * @brief Gets the NodeBaseInterface of node_. - * @details This function exists to allow running ActionServerBT as a component in a composable node container. + * @details This function exists to allow running TreeExecutionServer as a component in a composable node container. * * @return A shared_ptr to the NodeBaseInterface of node_. */ @@ -64,22 +64,28 @@ class ActionServerBT BT::Blackboard::Ptr globalBlackboard(); protected: - // To be overridden by the user. - // Callback invoked when the tree is created and before it is executed, - // Can be used to update the blackboard or to attach loggers. + /** + * @brief Callback invoked after the tree is created. + * It can be used, for instance, to initialize a logger or the global blackboard. + * + * @param tree The tree that was created + */ virtual void onTreeCreated(BT::Tree& tree) {} - // To be overridden by the user. - // In addition to the built in mechanism to register nodes from plugins, - // you can use this method to register custom nodes into the factory. + /** + * @brief registerNodesIntoFactory is a callback invoked after the + * plugins were registered into the BT::BehaviorTreeFactory. + * It can be used to register additional custom nodes manually. + * + * @param factory The factory to use to register nodes + */ virtual void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {} - // To be overridden by the user. - // Callback invoked after the tickOnce. - // If it returns something different than std::nullopt, the tree execution will - // be halted and the returned value will be the optional NodeStatus. + /** + * @brief onLoopAfterTick invoked after the tree is created and before the tree is executed. + */ virtual std::optional onLoopAfterTick(BT::NodeStatus status) { return std::nullopt; @@ -129,4 +135,4 @@ class ActionServerBT void execute(const std::shared_ptr goal_handle); }; -} // namespace action_server_bt +} // namespace BT diff --git a/behaviortree_ros2/src/bt_executor_parameters.yaml b/behaviortree_ros2/src/bt_executor_parameters.yaml index 1364d8f..f58e112 100644 --- a/behaviortree_ros2/src/bt_executor_parameters.yaml +++ b/behaviortree_ros2/src/bt_executor_parameters.yaml @@ -1,7 +1,7 @@ action_server_bt: action_name: { type: string, - default_value: "action_server_bt", + default_value: "bt_action_server", read_only: true, description: "The name the Action Server takes requests from", } diff --git a/behaviortree_ros2/src/bt_utils.cpp b/behaviortree_ros2/src/bt_utils.cpp index 6c72ec9..2199dfa 100644 --- a/behaviortree_ros2/src/bt_utils.cpp +++ b/behaviortree_ros2/src/bt_utils.cpp @@ -15,13 +15,13 @@ namespace { -static const auto kLogger = rclcpp::get_logger("action_server_bt"); +static const auto kLogger = rclcpp::get_logger("bt_action_server"); } -namespace action_server_bt +namespace BT { -btcpp_ros2_interfaces::msg::NodeStatus convert_node_status(BT::NodeStatus& status) +btcpp_ros2_interfaces::msg::NodeStatus ConvertNodeStatus(BT::NodeStatus& status) { btcpp_ros2_interfaces::msg::NodeStatus action_status; switch(status) @@ -41,7 +41,7 @@ btcpp_ros2_interfaces::msg::NodeStatus convert_node_status(BT::NodeStatus& statu return action_status; } -std::string get_directory_path(const std::string& parameter_value) +std::string GetDirectoryPath(const std::string& parameter_value) { std::string package_name, subfolder; auto pos = parameter_value.find_first_of("/"); @@ -70,8 +70,8 @@ std::string get_directory_path(const std::string& parameter_value) return ""; } -void load_behavior_trees(BT::BehaviorTreeFactory& factory, - const std::string& directory_path) +void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory, + const std::string& directory_path) { using std::filesystem::directory_iterator; for(const auto& entry : directory_iterator(directory_path)) @@ -92,7 +92,7 @@ void load_behavior_trees(BT::BehaviorTreeFactory& factory, } } -void load_plugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path) +void LoadPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path) { using std::filesystem::directory_iterator; for(const auto& entry : directory_iterator(directory_path)) @@ -113,8 +113,8 @@ void load_plugins(BT::BehaviorTreeFactory& factory, const std::string& directory } } -void load_ros_plugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, - rclcpp::Node::SharedPtr node) +void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, + rclcpp::Node::SharedPtr node) { using std::filesystem::directory_iterator; BT::RosNodeParams params; @@ -137,37 +137,36 @@ void load_ros_plugins(BT::BehaviorTreeFactory& factory, const std::string& direc } } -void register_behavior_trees(action_server_bt::Params& params, - BT::BehaviorTreeFactory& factory, - rclcpp::Node::SharedPtr node) +void RegisterBehaviorTrees(action_server_bt::Params& params, + BT::BehaviorTreeFactory& factory, rclcpp::Node::SharedPtr node) { // clear the factory and load/reload it with the Behaviors and Trees specified by the user in their action_server_bt config yaml factory.clearRegisteredBehaviorTrees(); for(const auto& plugin : params.plugins) { - const auto plugin_directory = get_directory_path(plugin); + const auto plugin_directory = GetDirectoryPath(plugin); // skip invalid plugins directories if(plugin_directory.empty()) continue; - load_plugins(factory, plugin_directory); + LoadPlugins(factory, plugin_directory); } for(const auto& plugin : params.ros_plugins) { - const auto plugin_directory = get_directory_path(plugin); + const auto plugin_directory = GetDirectoryPath(plugin); // skip invalid plugins directories if(plugin_directory.empty()) continue; - load_ros_plugins(factory, plugin_directory, node); + LoadRosPlugins(factory, plugin_directory, node); } for(const auto& tree_dir : params.behavior_trees) { - const auto tree_directory = get_directory_path(tree_dir); + const auto tree_directory = GetDirectoryPath(tree_dir); // skip invalid subtree directories if(tree_directory.empty()) continue; - load_behavior_trees(factory, tree_directory); + LoadBehaviorTrees(factory, tree_directory); } } -} // namespace action_server_bt +} // namespace BT diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 46a09f1..d8c73bc 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -22,13 +22,13 @@ #include "bt_executor_parameters.hpp" namespace { -static const auto kLogger = rclcpp::get_logger("action_server_bt"); +static const auto kLogger = rclcpp::get_logger("bt_action_server"); } -namespace action_server_bt +namespace BT { -struct ActionServerBT::Pimpl +struct TreeExecutionServer::Pimpl { Pimpl(const rclcpp::NodeOptions& node_options); @@ -59,18 +59,18 @@ struct ActionServerBT::Pimpl void execute(const std::shared_ptr goal_handle); }; -ActionServerBT::Pimpl::Pimpl(const rclcpp::NodeOptions& node_options) - : node(std::make_unique("action_server_bt", node_options)) +TreeExecutionServer::Pimpl::Pimpl(const rclcpp::NodeOptions& node_options) + : node(std::make_unique("bt_action_server", node_options)) { param_listener = std::make_shared(node); params = param_listener->get_params(); global_blackboard = BT::Blackboard::create(); } -ActionServerBT::~ActionServerBT() +TreeExecutionServer::~TreeExecutionServer() {} -ActionServerBT::ActionServerBT(const rclcpp::NodeOptions& options) +TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options) : p_(new Pimpl(options)) { // create the action server @@ -90,18 +90,19 @@ ActionServerBT::ActionServerBT(const rclcpp::NodeOptions& options) }); // register the users Plugins and BehaviorTree.xml files into the factory - register_behavior_trees(p_->params, p_->factory, p_->node); + RegisterBehaviorTrees(p_->params, p_->factory, p_->node); registerNodesIntoFactory(p_->factory); } -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ActionServerBT::nodeBaseInterface() +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr +TreeExecutionServer::nodeBaseInterface() { return p_->node->get_node_base_interface(); } rclcpp_action::GoalResponse -ActionServerBT::handle_goal(const rclcpp_action::GoalUUID& /* uuid */, - std::shared_ptr goal) +TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */, + std::shared_ptr goal) { RCLCPP_INFO(kLogger, "Received goal request to execute Behavior Tree: %s", goal->target_tree.c_str()); @@ -109,8 +110,8 @@ ActionServerBT::handle_goal(const rclcpp_action::GoalUUID& /* uuid */, return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -rclcpp_action::CancelResponse -ActionServerBT::handle_cancel(const std::shared_ptr goal_handle) +rclcpp_action::CancelResponse TreeExecutionServer::handle_cancel( + const std::shared_ptr goal_handle) { RCLCPP_INFO(kLogger, "Received request to cancel goal"); if(!goal_handle->is_active()) @@ -123,7 +124,7 @@ ActionServerBT::handle_cancel(const std::shared_ptr goal_ return rclcpp_action::CancelResponse::ACCEPT; } -void ActionServerBT::handle_accepted( +void TreeExecutionServer::handle_accepted( const std::shared_ptr goal_handle) { // Join the previous execute thread before replacing it with a new one @@ -135,7 +136,8 @@ void ActionServerBT::handle_accepted( p_->action_thread = std::thread{ [=]() { execute(goal_handle); } }; } -void ActionServerBT::execute(const std::shared_ptr goal_handle) +void TreeExecutionServer::execute( + const std::shared_ptr goal_handle) { const auto goal = goal_handle->get_goal(); BT::NodeStatus status = BT::NodeStatus::RUNNING; @@ -145,7 +147,7 @@ void ActionServerBT::execute(const std::shared_ptr goal_h if(p_->param_listener->is_old(p_->params)) { p_->params = p_->param_listener->get_params(); - register_behavior_trees(p_->params, p_->factory, p_->node); + RegisterBehaviorTrees(p_->params, p_->factory, p_->node); registerNodesIntoFactory(p_->factory); } @@ -174,7 +176,7 @@ void ActionServerBT::execute(const std::shared_ptr goal_h // cancel_requested_ or by onLoopAfterTick() auto stop_action = [this, &action_result](BT::NodeStatus status, const std::string& message) { - action_result->node_status = convert_node_status(status); + action_result->node_status = ConvertNodeStatus(status); action_result->error_message = message; RCLCPP_WARN(kLogger, action_result->error_message.c_str()); p_->tree->haltTree(); @@ -217,7 +219,7 @@ void ActionServerBT::execute(const std::shared_ptr goal_h } catch(const std::exception& ex) { - action_result->error_message = "Behavior Tree exception: %s", ex.what(); + action_result->error_message = std::string("Behavior Tree exception:") + ex.what(); RCLCPP_ERROR(kLogger, action_result->error_message.c_str()); goal_handle->abort(action_result); return; @@ -227,7 +229,7 @@ void ActionServerBT::execute(const std::shared_ptr goal_h onTreeExecutionCompleted(status, false); // set the node_status result to the action - action_result->node_status = convert_node_status(status); + action_result->node_status = ConvertNodeStatus(status); // return success or aborted for the action result if(status == BT::NodeStatus::SUCCESS) @@ -237,27 +239,27 @@ void ActionServerBT::execute(const std::shared_ptr goal_h } else { - action_result->error_message = "Behavior Tree failed during execution with status: " - "%s", - BT::toStr(status); + action_result->error_message = std::string("Behavior Tree failed during execution " + "with status: ") + + BT::toStr(status); RCLCPP_ERROR(kLogger, action_result->error_message.c_str()); goal_handle->abort(action_result); } } -const std::string& ActionServerBT::currentTreeName() const +const std::string& TreeExecutionServer::currentTreeName() const { return p_->current_tree_name; } -BT::Tree* ActionServerBT::currentTree() +BT::Tree* TreeExecutionServer::currentTree() { return p_->tree ? p_->tree.get() : nullptr; } -BT::Blackboard::Ptr ActionServerBT::globalBlackboard() +BT::Blackboard::Ptr TreeExecutionServer::globalBlackboard() { return p_->global_blackboard; } -} // namespace action_server_bt +} // namespace BT diff --git a/btcpp_ros2_samples/src/sample_bt_executor.cpp b/btcpp_ros2_samples/src/sample_bt_executor.cpp index 0e04d43..3ba0bfd 100644 --- a/btcpp_ros2_samples/src/sample_bt_executor.cpp +++ b/btcpp_ros2_samples/src/sample_bt_executor.cpp @@ -16,10 +16,10 @@ // Example that shows how to customize ActionServerBT. // Here, we simply add an extra logger -class MyActionServer : public action_server_bt::ActionServerBT +class MyActionServer : public BT::TreeExecutionServer { public: - MyActionServer(const rclcpp::NodeOptions& options) : ActionServerBT(options) + MyActionServer(const rclcpp::NodeOptions& options) : TreeExecutionServer(options) {} void onTreeCreated(BT::Tree& tree) override From 9497662c2b6b33034ff114809a6477e63475dcb9 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Thu, 2 May 2024 17:12:52 +0200 Subject: [PATCH 20/44] more comments --- .../tree_execution_server.hpp | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index 950eac9..9a8839a 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -27,6 +27,8 @@ namespace BT /** * @brief TreeExecutionServer class hosts a ROS Action Server that is able * to load Behavior plugins, BehaviorTree.xml files and execute them. + * + * It can be customized by overriding its virtual functions. */ class TreeExecutionServer { @@ -84,18 +86,33 @@ class TreeExecutionServer {} /** - * @brief onLoopAfterTick invoked after the tree is created and before the tree is executed. + * @brief onLoopAfterTick invoked at each loop, after tree.tickOnce(). + * If it returns a valid NodeStatus, the tree will stop and return that status. + * Return std::nullopt to continue the execution. + * + * @param status The status of the tree after the last tick */ virtual std::optional onLoopAfterTick(BT::NodeStatus status) { return std::nullopt; } - // To be overridden by the user. - // Callback invoked when the tree execution is completed + /** + * @brief onTreeExecutionCompleted is a callback invoked after the tree execution is completed, + * i.e. if it returned SUCCESS/FAILURE or if the action was cancelled by the Action Client. + * + * @param status The status of the tree after the last tick + * @param was_cancelled True if the action was cancelled by the Action Client + */ virtual void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) {} + /** + * @brief onLoopFeedback is a callback invoked at each loop, after tree.tickOnce(). + * If it returns a valid string, it will be sent as feedback to the Action Client. + * + * If you don't want to return any feedback, return std::nullopt. + */ virtual std::optional onLoopFeedback() { return std::nullopt; From e6777b6239a51040516b6fe1fae67b840d3cd6db Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Fri, 3 May 2024 12:58:15 +0200 Subject: [PATCH 21/44] some refactoring of parameters and plugin loading --- .../include/behaviortree_ros2/bt_utils.hpp | 17 +--- .../include/behaviortree_ros2/plugins.hpp | 7 +- .../src/bt_executor_parameters.yaml | 12 +-- behaviortree_ros2/src/bt_utils.cpp | 77 +++++++-------- .../src/tree_execution_server.cpp | 10 +- behaviortree_ros2/tree_execution_server.md | 99 +++++++++++++++++++ .../config/sample_bt_executor.yaml | 13 +-- 7 files changed, 162 insertions(+), 73 deletions(-) create mode 100644 behaviortree_ros2/tree_execution_server.md diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp index b4c1414..d846996 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp @@ -56,22 +56,14 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory, const std::string& directory_path); /** - * @brief Function to load BehaviorTree plugins from a specific directory + * @brief Function to load BehaviorTree ROS plugins (or standard BT.CPP plugins) from a specific directory * * @param factory BehaviorTreeFactory to register the plugins into * @param directory_path Full path to the directory to search for BehaviorTree plugins - */ -void LoadPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path); - -/** - * @brief Function to load BehaviorTree ROS plugins from a specific directory - * - * @param factory BehaviorTreeFactory to register the plugins into - * @param directory_path Full path to the directory to search for BehaviorTree plugins - * @param node node pointer that is shared with the ROS based BehaviorTree plugins + * @param params parameters passed to the ROS plugins */ void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, - rclcpp::Node::SharedPtr node); + BT::RosNodeParams params); /** * @brief Function to register all Behaviors and BehaviorTrees from user specified packages @@ -81,8 +73,7 @@ void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directo * @param factory BehaviorTreeFactory to register into * @param node node pointer that is shared with the ROS based Behavior plugins */ -void RegisterBehaviorTrees(action_server_bt::Params& params, - BT::BehaviorTreeFactory& factory, +void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory, rclcpp::Node::SharedPtr node); } // namespace BT diff --git a/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp b/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp index 4d7ed4d..116fd2c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp @@ -20,6 +20,11 @@ #include "behaviortree_cpp/utils/shared_library.h" #include "behaviortree_ros2/ros_node_params.hpp" +namespace BT +{ +constexpr const char* ROS_PLUGIN_SYMBOL = "BT_RegisterRosNodeFromPlugin"; +} + // Use this macro to generate a plugin for: // // - BT::RosActionNode @@ -54,6 +59,6 @@ inline void RegisterRosNode(BT::BehaviorTreeFactory& factory, { BT::SharedLibrary loader(filepath.generic_string()); typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&); - auto func = (Func)loader.getSymbol("BT_RegisterRosNodeFromPlugin"); + auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL); func(factory, params); } diff --git a/behaviortree_ros2/src/bt_executor_parameters.yaml b/behaviortree_ros2/src/bt_executor_parameters.yaml index f58e112..54fafd7 100644 --- a/behaviortree_ros2/src/bt_executor_parameters.yaml +++ b/behaviortree_ros2/src/bt_executor_parameters.yaml @@ -1,11 +1,11 @@ -action_server_bt: +bt_server: action_name: { type: string, default_value: "bt_action_server", read_only: true, description: "The name the Action Server takes requests from", } - behavior_tick_frequency: { + tick_frequency: { type: int, default_value: 100, read_only: true, @@ -31,10 +31,10 @@ action_server_bt: unique<>: null, } } - ros_plugins: { - type: string_array, - default_value: [], - description: "List of 'package_name/subfolder' containing BehaviorTree ROS plugins to load into the factory", + ros_plugins_timeout: { + type: int, + default_value: 1000, + description: "Timeout (ms) used in BT::RosNodeParams", validation: { unique<>: null, } diff --git a/behaviortree_ros2/src/bt_utils.cpp b/behaviortree_ros2/src/bt_utils.cpp index 2199dfa..ce5e5cc 100644 --- a/behaviortree_ros2/src/bt_utils.cpp +++ b/behaviortree_ros2/src/bt_utils.cpp @@ -12,6 +12,7 @@ // OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. #include "behaviortree_ros2/bt_utils.hpp" +#include "behaviortree_ros2/plugins.hpp" namespace { @@ -92,73 +93,69 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory, } } -void LoadPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path) -{ - using std::filesystem::directory_iterator; - for(const auto& entry : directory_iterator(directory_path)) - { - if(entry.path().extension() == ".so") - { - try - { - factory.registerFromPlugin(entry.path().string()); - RCLCPP_INFO(kLogger, "Loaded Plugin: %s", entry.path().filename().c_str()); - } - catch(const std::exception& e) - { - RCLCPP_ERROR(kLogger, "Failed to load Plugin: %s \n %s", - entry.path().filename().c_str(), e.what()); - } - } - } -} - void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, - rclcpp::Node::SharedPtr node) + BT::RosNodeParams params) { using std::filesystem::directory_iterator; - BT::RosNodeParams params; - params.nh = node; + for(const auto& entry : directory_iterator(directory_path)) { + const auto filename = entry.path().filename(); if(entry.path().extension() == ".so") { try { - RegisterRosNode(factory, entry.path().string(), params); - RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", entry.path().filename().c_str()); + BT::SharedLibrary loader(entry.path().string()); + if(loader.hasSymbol(BT::PLUGIN_SYMBOL)) + { + typedef void (*Func)(BehaviorTreeFactory&); + auto func = (Func)loader.getSymbol(BT::PLUGIN_SYMBOL); + func(factory); + } + else if(loader.hasSymbol(BT::ROS_PLUGIN_SYMBOL)) + { + typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&); + auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL); + func(factory, params); + } + else + { + RCLCPP_ERROR(kLogger, "Failed to load Plugin from file: %s.", filename.c_str()); + continue; + } + RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", filename.c_str()); } catch(const std::exception& e) { - RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", - entry.path().filename().c_str(), e.what()); + RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", filename.c_str(), + e.what()); } } } } -void RegisterBehaviorTrees(action_server_bt::Params& params, - BT::BehaviorTreeFactory& factory, rclcpp::Node::SharedPtr node) +void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory, + rclcpp::Node::SharedPtr node) { - // clear the factory and load/reload it with the Behaviors and Trees specified by the user in their action_server_bt config yaml + // clear the factory and load/reload it with the Behaviors and Trees specified by the user in their [bt_action_server] config yaml factory.clearRegisteredBehaviorTrees(); + BT::RosNodeParams ros_params; + ros_params.nh = node; + ros_params.server_timeout = std::chrono::milliseconds(params.ros_plugins_timeout); + ros_params.wait_for_server_timeout = ros_params.server_timeout; + for(const auto& plugin : params.plugins) { const auto plugin_directory = GetDirectoryPath(plugin); // skip invalid plugins directories if(plugin_directory.empty()) + { continue; - LoadPlugins(factory, plugin_directory); - } - for(const auto& plugin : params.ros_plugins) - { - const auto plugin_directory = GetDirectoryPath(plugin); - // skip invalid plugins directories - if(plugin_directory.empty()) - continue; - LoadRosPlugins(factory, plugin_directory, node); + } + LoadRosPlugins(factory, plugin_directory, ros_params); } + for(const auto& tree_dir : params.behavior_trees) { const auto tree_directory = GetDirectoryPath(tree_dir); diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index d8c73bc..26b6f11 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -38,8 +38,8 @@ struct TreeExecutionServer::Pimpl // thread safe bool to keep track of cancel requests std::atomic cancel_requested{ false }; - std::shared_ptr param_listener; - action_server_bt::Params params; + std::shared_ptr param_listener; + bt_server::Params params; BT::BehaviorTreeFactory factory; std::shared_ptr groot_publisher; @@ -62,7 +62,7 @@ struct TreeExecutionServer::Pimpl TreeExecutionServer::Pimpl::Pimpl(const rclcpp::NodeOptions& node_options) : node(std::make_unique("bt_action_server", node_options)) { - param_listener = std::make_shared(node); + param_listener = std::make_shared(node); params = param_listener->get_params(); global_blackboard = BT::Blackboard::create(); } @@ -168,8 +168,8 @@ void TreeExecutionServer::execute( std::make_shared(*(p_->tree), p_->params.groot2_port); // Loop until the tree is done or a cancel is requested - const auto period = std::chrono::milliseconds( - static_cast(1000.0 / p_->params.behavior_tick_frequency)); + const auto period = + std::chrono::milliseconds(static_cast(1000.0 / p_->params.tick_frequency)); auto loop_deadline = std::chrono::steady_clock::now() + period; // operations to be done if the tree execution is aborted, either by diff --git a/behaviortree_ros2/tree_execution_server.md b/behaviortree_ros2/tree_execution_server.md new file mode 100644 index 0000000..6254deb --- /dev/null +++ b/behaviortree_ros2/tree_execution_server.md @@ -0,0 +1,99 @@ +# TreeExecutionServer + +This base class is used to implement a Behavior Tree executor wrapped inside a `rclcpp_action::Server`. + +Users are expected to create a derived class to improve its functionalities, but often it can be used +out of the box directly. + +Further, the terms "load" will be equivalent to "register into the `BT::BehaviorTreeFactory`". + +The `TreeExecutionServer`offers the following features: + +- Configurable using ROS parameters (see below). +- Load Behavior Trees definitions (XML files) from a list of folders. +- Load BT plugins from a list of folders. These plugins may depend or not on ROS. +- Invoke the execution of a Tree from an external ROS Node, using `rclcpp_action::Client`. + +Furthermore, the user can customize it to: + +- Register custom BT Nodes directly (static linking). +- Attach additional loggers. The **Groot2** publisher will be attached by default. +- Use the "global blackboard", a new idiom/pattern explained in [this tutorial](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/examples/t19_global_blackboard.cpp). +- Customize the feedback of the `rclcpp_action::Server`. + + +## ROS Parameters + +Default Config + +```yaml +bt_action_server: + ros__parameters: + action_name: bt_action_server + behavior_tick_frequency: 100.0 + behavior_trees: '{}' + groot2_port: 1667.0 + plugins: '{}' + ros_plugins: '{}' + +``` + +## action_name + +The name the Action Server takes requests from + +* Type: `string` +* Default Value: "bt_action_server" +* Read only: True + +## behavior_tick_frequency + +Frequency in Hz to tick() the Behavior tree at + +* Type: `int` +* Default Value: 100 +* Read only: True + +*Constraints:* + - parameter must be within bounds 1 + +## groot2_port + +Server port value to publish Groot2 messages on + +* Type: `int` +* Default Value: 1667 +* Read only: True + +*Constraints:* + - parameter must be within bounds 1 + +## plugins + +List of 'package_name/subfolder' containing BehaviorTree plugins to load into the factory + +* Type: `string_array` +* Default Value: {} + +*Constraints:* + - contains no duplicates + +## ros_plugins + +List of 'package_name/subfolder' containing BehaviorTree ROS plugins to load into the factory + +* Type: `string_array` +* Default Value: {} + +*Constraints:* + - contains no duplicates + +## behavior_trees + +List of 'package_name/subfolder' containing SubTrees to load into the BehaviorTree factory + +* Type: `string_array` +* Default Value: {} + +*Constraints:* + - contains no duplicates diff --git a/btcpp_ros2_samples/config/sample_bt_executor.yaml b/btcpp_ros2_samples/config/sample_bt_executor.yaml index f07dd61..397e356 100644 --- a/btcpp_ros2_samples/config/sample_bt_executor.yaml +++ b/btcpp_ros2_samples/config/sample_bt_executor.yaml @@ -1,16 +1,13 @@ -action_server_bt: +bt_action_server: ros__parameters: - action_name: "behavior_server" # Optional (defaults to `action_server_bt`) - behavior_tick_frequency: 100 # Optional (defaults to 100 Hz) + action_name: "behavior_server" # Optional (defaults to `bt_action_server`) + tick_frequency: 100 # Optional (defaults to 100 Hz) groot2_port: 1667 # Optional (defaults to 1667) + ros_plugins_timeout: 1000 # Optional (defaults 1000 ms) - # Below are a list plugins and BehaviorTrees to load - # (you are not required to have all 3 types) - # These are dynamic parameters and can be changed at runtime via rosparam - # see `action_server_bt/parameters.md` for documentation. plugins: - behaviortree_cpp/bt_plugins - ros_plugins: - btcpp_ros2_samples/bt_plugins + behavior_trees: - btcpp_ros2_samples/behavior_trees From dc1935cd64618e522e2796c0d259adfa3e1aa9a8 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Fri, 3 May 2024 13:17:20 +0200 Subject: [PATCH 22/44] added documentation --- .../src/bt_executor_parameters.yaml | 4 +- behaviortree_ros2/tree_execution_server.md | 74 +++++++++++++++---- 2 files changed, 61 insertions(+), 17 deletions(-) diff --git a/behaviortree_ros2/src/bt_executor_parameters.yaml b/behaviortree_ros2/src/bt_executor_parameters.yaml index 54fafd7..b6c36c1 100644 --- a/behaviortree_ros2/src/bt_executor_parameters.yaml +++ b/behaviortree_ros2/src/bt_executor_parameters.yaml @@ -1,7 +1,7 @@ bt_server: action_name: { type: string, - default_value: "bt_action_server", + default_value: "bt_execution", read_only: true, description: "The name the Action Server takes requests from", } @@ -36,7 +36,7 @@ bt_server: default_value: 1000, description: "Timeout (ms) used in BT::RosNodeParams", validation: { - unique<>: null, + bounds<>: [1, 10000] } } behavior_trees: { diff --git a/behaviortree_ros2/tree_execution_server.md b/behaviortree_ros2/tree_execution_server.md index 6254deb..3186658 100644 --- a/behaviortree_ros2/tree_execution_server.md +++ b/behaviortree_ros2/tree_execution_server.md @@ -7,7 +7,7 @@ out of the box directly. Further, the terms "load" will be equivalent to "register into the `BT::BehaviorTreeFactory`". -The `TreeExecutionServer`offers the following features: +The `TreeExecutionServer` offers the following features: - Configurable using ROS parameters (see below). - Load Behavior Trees definitions (XML files) from a list of folders. @@ -21,6 +21,49 @@ Furthermore, the user can customize it to: - Use the "global blackboard", a new idiom/pattern explained in [this tutorial](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/examples/t19_global_blackboard.cpp). - Customize the feedback of the `rclcpp_action::Server`. +## Customization points + +These are the virtual method of `TreeExecutionServer` that can be overridden by the user. + +### void onTreeCreated(BT::Tree& tree) + +Callback invoked when a tree is created; this happens after `rclcpp_action::Server` receive a command from a client. + +It can be used, for instance, to initialize a logger or the global blackboard. + +### void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) + +Called at the beginning, after all the plugins have been loaded. + +It can be used to register programmatically more BT.CPP Nodes. + +### std::optional onLoopAfterTick(BT::NodeStatus status) + +Used to do something after the tree was ticked, in its execution loop. + +If this method returns something other than `std::nullopt`, the tree +execution is interrupted an the specified `BT::NodeStatus` is returned to the `rclcpp_action::Client`. + +### void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) + +Callback when the tree execution reaches its end. + +This happens if: + +1. Ticking the tree returns SUCCESS/FAILURE +2. The `rclcpp_action::Client` cancels the action. +3. Callback `onLoopAfterTick`cancels the execution. + +Argument `was_cancelled`is true in the 1st case, false otherwise. + +### std::optional onLoopFeedback() + +This callback is invoked after `tree.tickOnce` and `onLoopAfterTick`. + +If it returns something other than `std::nullopt`, the provided string will be +sent as feedback to the `rclcpp_action::Client`. + + ## ROS Parameters @@ -29,24 +72,23 @@ Default Config ```yaml bt_action_server: ros__parameters: - action_name: bt_action_server + action_name: bt_execution behavior_tick_frequency: 100.0 behavior_trees: '{}' groot2_port: 1667.0 + ros_plugins_timeout: 1000, plugins: '{}' - ros_plugins: '{}' - ``` -## action_name +### action_name The name the Action Server takes requests from * Type: `string` -* Default Value: "bt_action_server" +* Default Value: "bt_execution" * Read only: True -## behavior_tick_frequency +### behavior_tick_frequency Frequency in Hz to tick() the Behavior tree at @@ -57,7 +99,7 @@ Frequency in Hz to tick() the Behavior tree at *Constraints:* - parameter must be within bounds 1 -## groot2_port +### groot2_port Server port value to publish Groot2 messages on @@ -68,19 +110,21 @@ Server port value to publish Groot2 messages on *Constraints:* - parameter must be within bounds 1 -## plugins +### ros_plugins_timeout -List of 'package_name/subfolder' containing BehaviorTree plugins to load into the factory +Timeout, in milliseconds, to use with ROS Plugins (see BT::RosNodeParams) -* Type: `string_array` +* Type: `int` * Default Value: {} *Constraints:* - - contains no duplicates + - parameter must be within 1 and 10000 + +### plugins -## ros_plugins +List of 'package_name/subfolder' containing BehaviorTree plugins to load into the factory. -List of 'package_name/subfolder' containing BehaviorTree ROS plugins to load into the factory +These are plugins created using either the macro `BT_RegisterNodesFromPlugin` or `BT_RegisterRosNodeFromPlugin`. * Type: `string_array` * Default Value: {} @@ -88,7 +132,7 @@ List of 'package_name/subfolder' containing BehaviorTree ROS plugins to load int *Constraints:* - contains no duplicates -## behavior_trees +### behavior_trees List of 'package_name/subfolder' containing SubTrees to load into the BehaviorTree factory From 3a45b468e8713c3fee62a5cb4e906ead488386a0 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Fri, 3 May 2024 18:04:00 -0400 Subject: [PATCH 23/44] cleanup and basic docs --- behaviortree_ros2/bt_executor_parameters.md | 75 +++++++++++++++++++ btcpp_ros2_samples/CMakeLists.txt | 10 +++ btcpp_ros2_samples/README.md | 38 ++++++++++ .../launch/sample_bt_executor.launch.xml | 4 +- btcpp_ros2_samples/src/sample_bt_executor.cpp | 2 +- 5 files changed, 126 insertions(+), 3 deletions(-) create mode 100644 behaviortree_ros2/bt_executor_parameters.md create mode 100644 btcpp_ros2_samples/README.md diff --git a/behaviortree_ros2/bt_executor_parameters.md b/behaviortree_ros2/bt_executor_parameters.md new file mode 100644 index 0000000..7500680 --- /dev/null +++ b/behaviortree_ros2/bt_executor_parameters.md @@ -0,0 +1,75 @@ +# Bt Server Parameters + +Default Config +```yaml +bt_server: + ros__parameters: + action_name: bt_execution + behavior_trees: '{}' + groot2_port: 1667.0 + plugins: '{}' + ros_plugins_timeout: 1000.0 + tick_frequency: 100.0 + +``` + +## action_name + +The name the Action Server takes requests from + +* Type: `string` +* Default Value: "bt_execution" +* Read only: True + +## tick_frequency + +Frequency in Hz to tick() the Behavior tree at + +* Type: `int` +* Default Value: 100 +* Read only: True + +*Constraints:* + - parameter must be within bounds 1 + +## groot2_port + +Server port value to publish Groot2 messages on + +* Type: `int` +* Default Value: 1667 +* Read only: True + +*Constraints:* + - parameter must be within bounds 1 + +## plugins + +List of 'package_name/subfolder' containing BehaviorTree plugins to load into the factory + +* Type: `string_array` +* Default Value: {} + +*Constraints:* + - contains no duplicates + +## ros_plugins_timeout + +Timeout (ms) used in BT::RosNodeParams + +* Type: `int` +* Default Value: 1000 + +*Constraints:* + - parameter must be within bounds 1 + +## behavior_trees + +List of 'package_name/subfolder' containing SubTrees to load into the BehaviorTree factory + +* Type: `string_array` +* Default Value: {} + +*Constraints:* + - contains no duplicates + diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index 084da70..785d674 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -73,6 +73,16 @@ install(TARGETS RUNTIME DESTINATION share/${PROJECT_NAME}/bt_plugins ) +###################################################### +# INSTALL Behavior.xml's, ROS config and launch files + +install(DIRECTORY + behavior_trees + config + launch + DESTINATION share/${PROJECT_NAME}/ + ) + ament_export_dependencies(behaviortree_ros2 btcpp_ros2_interfaces) diff --git a/btcpp_ros2_samples/README.md b/btcpp_ros2_samples/README.md new file mode 100644 index 0000000..c63eac1 --- /dev/null +++ b/btcpp_ros2_samples/README.md @@ -0,0 +1,38 @@ +# Sample Behaviors + +For documentation on sample behaviors included in this package please see the BehaviorTree.CPP [ROS 2 Integration documentation](https://www.behaviortree.dev/docs/ros2_integration) . + +# TreeExecutionServer Documentation and Example + +This package also includes an example Behavior Tree Executor that is designed to make building, combining and executing [BehaviorTree.CPP](https://www.behaviortree.dev/docs/intro) based Behaviors easy and reusable. +This Executor includes an Action Server that is able to register plugins or directly linked Behaviors and Trees/Subtrees so that a user can execute any known BehaviorTree by simply sending the name of it to the server. + +The `TreeExecutionServer` class offers several overridable methods that that can be used to meet your specific needs. Please see [tree_execution_server.hpp](../behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp) for descriptions and requirements of each virtual method. You can also refer to [sample_bt_executor.cpp](src/sample_bt_executor.cpp) for a working example of the `TreeExecutionServer`. + +A launch file is included that starts the Execution Server and loads a list of plugins and BehaviorTrees from `yaml` file: +``` bash +ros2 launch btcpp_ros2_samples sample_bt_executor.launch.xml +``` + +> *NOTE:* For documentation on the `yaml` parameters please see [bt_executor_parameters.md](../behaviortree_ros2/bt_executor_parameters.md). + +As the Server starts up it will print out the name of the ROS Action followed by the plugins and BehaviorTrees it was able to load. +``` +[bt_action_server]: Starting Action Server: bt_action_server +[bt_action_server]: Loaded Plugin: libdummy_nodes_dyn.so +[bt_action_server]: Loaded Plugin: libmovebase_node_dyn.so +[bt_action_server]: Loaded Plugin: libcrossdoor_nodes_dyn.so +[bt_action_server]: Loaded ROS Plugin: libsleep_plugin.so +[bt_action_server]: Loaded BehaviorTree: door_closed.xml +[bt_action_server]: Loaded Beha viorTree: cross_door.xml +``` + +To call the Action Server from the command line: +``` bash +ros2 action send_goal /bt_action_server btcpp_ros2_interfaces/action/ExecuteTree "{target_tree: CrossDoor}" +``` + +You can also try a Behavior that is a ROS Action or Service client itself. +```bash +ros2 action send_goal /bt_action_server btcpp_ros2_interfaces/action/ExecuteTree "{target_tree: SleepActionSample}" +``` \ No newline at end of file diff --git a/btcpp_ros2_samples/launch/sample_bt_executor.launch.xml b/btcpp_ros2_samples/launch/sample_bt_executor.launch.xml index 2052b16..632fe7d 100644 --- a/btcpp_ros2_samples/launch/sample_bt_executor.launch.xml +++ b/btcpp_ros2_samples/launch/sample_bt_executor.launch.xml @@ -1,8 +1,8 @@ - - + + diff --git a/btcpp_ros2_samples/src/sample_bt_executor.cpp b/btcpp_ros2_samples/src/sample_bt_executor.cpp index 3ba0bfd..d2d160b 100644 --- a/btcpp_ros2_samples/src/sample_bt_executor.cpp +++ b/btcpp_ros2_samples/src/sample_bt_executor.cpp @@ -14,7 +14,7 @@ #include #include -// Example that shows how to customize ActionServerBT. +// Example that shows how to customize TreeExecutionServer. // Here, we simply add an extra logger class MyActionServer : public BT::TreeExecutionServer { From bb435f4812b5d8c4909f003989a139e4fc8fe236 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Fri, 3 May 2024 18:04:39 -0400 Subject: [PATCH 24/44] pre-commit --- behaviortree_ros2/bt_executor_parameters.md | 1 - btcpp_ros2_samples/README.md | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/behaviortree_ros2/bt_executor_parameters.md b/behaviortree_ros2/bt_executor_parameters.md index 7500680..877d5ec 100644 --- a/behaviortree_ros2/bt_executor_parameters.md +++ b/behaviortree_ros2/bt_executor_parameters.md @@ -72,4 +72,3 @@ List of 'package_name/subfolder' containing SubTrees to load into the BehaviorTr *Constraints:* - contains no duplicates - diff --git a/btcpp_ros2_samples/README.md b/btcpp_ros2_samples/README.md index c63eac1..fe2543e 100644 --- a/btcpp_ros2_samples/README.md +++ b/btcpp_ros2_samples/README.md @@ -35,4 +35,4 @@ ros2 action send_goal /bt_action_server btcpp_ros2_interfaces/action/ExecuteTree You can also try a Behavior that is a ROS Action or Service client itself. ```bash ros2 action send_goal /bt_action_server btcpp_ros2_interfaces/action/ExecuteTree "{target_tree: SleepActionSample}" -``` \ No newline at end of file +``` From dbf6bc0a6851f2804d311e12277e09b83d576fa5 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Sat, 4 May 2024 08:14:49 -0600 Subject: [PATCH 25/44] docs cleanup --- README.md | 7 ++ behaviortree_ros2/ros_behavior_wrappers.md | 66 ++++++++++++++++++ behaviortree_ros2/tree_execution_server.md | 78 ++-------------------- btcpp_ros2_samples/README.md | 21 +++--- 4 files changed, 87 insertions(+), 85 deletions(-) create mode 100644 behaviortree_ros2/ros_behavior_wrappers.md diff --git a/README.md b/README.md index ce5b332..be77ad1 100644 --- a/README.md +++ b/README.md @@ -5,6 +5,7 @@ This repository contains useful wrappers to use ROS2 and BehaviorTree.CPP togeth In particular, it provides a standard way to implement: +- Behavior Tree Executor with ROS Action interface. - Action clients. - Service Clients. - Topic Subscribers. @@ -15,6 +16,12 @@ Our main goals are: - to minimize the amount of boilerplate. - to make asynchronous Actions non-blocking. +# Documentation + +- [ROS Behavior Wrappers](behaviortree_ros2/ros_behavior_wrappers.md) +- [TreeExecutionServer](behaviortree_ros2/tree_execution_server.md) +- [Sample Behaviors](btcpp_ros2_samples/README.md) + Note that this library is compatible **only** with: - **BT.CPP** 4.1 or newer. diff --git a/behaviortree_ros2/ros_behavior_wrappers.md b/behaviortree_ros2/ros_behavior_wrappers.md new file mode 100644 index 0000000..d493639 --- /dev/null +++ b/behaviortree_ros2/ros_behavior_wrappers.md @@ -0,0 +1,66 @@ +# ROS Behavior Wrappers + +A base class is used to implement each Behavior type for a ROS Action, Service or Topic Publisher / Subscriber. + +Users are expected to create a derived class and can implement the following methods. + +# ROS Action Behavior Wrapper + +### bool setGoal(Goal& goal) + +Required callback that allows the user to set the goal message. +Return false if the request should not be sent. In that case, RosActionNode::onFailure(INVALID_GOAL) will be called. + +### BT::NodeStatus onResultReceived(const WrappedResult& result) + +Required callback invoked when the result is received by the server. +It is up to the user to define if the action returns SUCCESS or FAILURE. + +### BT::NodeStatus onFeedback(const std::shared_ptr feedback) + +Optional callback invoked when the feedback is received. +It generally returns RUNNING, but the user can also use this callback to cancel the current action and return SUCCESS or FAILURE. + +### BT::NodeStatus onFailure(ActionNodeErrorCode error) + +Optional callback invoked when something goes wrong. +It must return either SUCCESS or FAILURE. + +### void onHalt() + +Optional callback executed when the node is halted. +Note that cancelGoal() is done automatically. + +# ROS Service Behavior Wrapper + +### setRequest(typename Request::SharedPtr& request) + +Required callback that allows the user to set the request message (ServiceT::Request). + +### BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) + +Required callback invoked when the response is received by the server. +It is up to the user to define if this returns SUCCESS or FAILURE. + +### BT::NodeStatus onFailure(ServiceNodeErrorCode error) + +Optional callback invoked when something goes wrong; you can override it. +It must return either SUCCESS or FAILURE. + +# ROS Topic Publisher Wrapper + +### bool setMessage(TopicT& msg) + +Required callback invoked in tick to allow the user to pass the message to be published. + +# ROS Topic Subscriber Wrapper + +### NodeStatus onTick(const std::shared_ptr& last_msg) + +Required callback invoked in the tick. You must return either SUCCESS of FAILURE. + +### bool latchLastMessage() + +Optional callback to latch the message that has been processed. +If returns false and no new message is received, before next call there will be no message to process. +If returns true, the next call will process the same message again, if no new message received. \ No newline at end of file diff --git a/behaviortree_ros2/tree_execution_server.md b/behaviortree_ros2/tree_execution_server.md index 3186658..34edf44 100644 --- a/behaviortree_ros2/tree_execution_server.md +++ b/behaviortree_ros2/tree_execution_server.md @@ -67,77 +67,9 @@ sent as feedback to the `rclcpp_action::Client`. ## ROS Parameters -Default Config - -```yaml -bt_action_server: - ros__parameters: - action_name: bt_execution - behavior_tick_frequency: 100.0 - behavior_trees: '{}' - groot2_port: 1667.0 - ros_plugins_timeout: 1000, - plugins: '{}' -``` - -### action_name - -The name the Action Server takes requests from - -* Type: `string` -* Default Value: "bt_execution" -* Read only: True - -### behavior_tick_frequency - -Frequency in Hz to tick() the Behavior tree at - -* Type: `int` -* Default Value: 100 -* Read only: True - -*Constraints:* - - parameter must be within bounds 1 - -### groot2_port - -Server port value to publish Groot2 messages on - -* Type: `int` -* Default Value: 1667 -* Read only: True - -*Constraints:* - - parameter must be within bounds 1 - -### ros_plugins_timeout +Documentation for the parameters used by the `TreeExecutionServer` can be found [here](bt_executor_parameters.md). -Timeout, in milliseconds, to use with ROS Plugins (see BT::RosNodeParams) - -* Type: `int` -* Default Value: {} - -*Constraints:* - - parameter must be within 1 and 10000 - -### plugins - -List of 'package_name/subfolder' containing BehaviorTree plugins to load into the factory. - -These are plugins created using either the macro `BT_RegisterNodesFromPlugin` or `BT_RegisterRosNodeFromPlugin`. - -* Type: `string_array` -* Default Value: {} - -*Constraints:* - - contains no duplicates - -### behavior_trees - -List of 'package_name/subfolder' containing SubTrees to load into the BehaviorTree factory - -* Type: `string_array` -* Default Value: {} - -*Constraints:* - - contains no duplicates +If the parameter documentation needs updating you can regenerate it with: +```bash +generate_parameter_library_markdown --input_yaml src/bt_executor_parameters.yaml --output_markdown_file bt_executor_parameters.md +``` diff --git a/btcpp_ros2_samples/README.md b/btcpp_ros2_samples/README.md index fe2543e..27cdcc9 100644 --- a/btcpp_ros2_samples/README.md +++ b/btcpp_ros2_samples/README.md @@ -1,15 +1,12 @@ # Sample Behaviors -For documentation on sample behaviors included in this package please see the BehaviorTree.CPP [ROS 2 Integration documentation](https://www.behaviortree.dev/docs/ros2_integration) . +For documentation on sample behaviors included in this package please see the BehaviorTree.CPP [ROS 2 Integration documentation](https://www.behaviortree.dev/docs/ros2_integration). Documentation of the derived class methods can methods for each ROS interface type can be found [here](../behaviortree_ros2/ros_behavior_wrappers.md). -# TreeExecutionServer Documentation and Example +# TreeExecutionServer Sample -This package also includes an example Behavior Tree Executor that is designed to make building, combining and executing [BehaviorTree.CPP](https://www.behaviortree.dev/docs/intro) based Behaviors easy and reusable. -This Executor includes an Action Server that is able to register plugins or directly linked Behaviors and Trees/Subtrees so that a user can execute any known BehaviorTree by simply sending the name of it to the server. +Documentation on the TreeExecutionServer used in this example can be found [here](../behaviortree_ros2/tree_execution_server.md). -The `TreeExecutionServer` class offers several overridable methods that that can be used to meet your specific needs. Please see [tree_execution_server.hpp](../behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp) for descriptions and requirements of each virtual method. You can also refer to [sample_bt_executor.cpp](src/sample_bt_executor.cpp) for a working example of the `TreeExecutionServer`. - -A launch file is included that starts the Execution Server and loads a list of plugins and BehaviorTrees from `yaml` file: +To start the sample Execution Server that load a list of plugins and BehaviorTrees from `yaml` file: ``` bash ros2 launch btcpp_ros2_samples sample_bt_executor.launch.xml ``` @@ -18,21 +15,21 @@ ros2 launch btcpp_ros2_samples sample_bt_executor.launch.xml As the Server starts up it will print out the name of the ROS Action followed by the plugins and BehaviorTrees it was able to load. ``` -[bt_action_server]: Starting Action Server: bt_action_server +[bt_action_server]: Starting Action Server: behavior_server [bt_action_server]: Loaded Plugin: libdummy_nodes_dyn.so [bt_action_server]: Loaded Plugin: libmovebase_node_dyn.so [bt_action_server]: Loaded Plugin: libcrossdoor_nodes_dyn.so -[bt_action_server]: Loaded ROS Plugin: libsleep_plugin.so +[bt_action_server]: Loaded Plugin: libsleep_plugin.so [bt_action_server]: Loaded BehaviorTree: door_closed.xml -[bt_action_server]: Loaded Beha viorTree: cross_door.xml +[bt_action_server]: Loaded BehaviorTree: cross_door.xml ``` To call the Action Server from the command line: ``` bash -ros2 action send_goal /bt_action_server btcpp_ros2_interfaces/action/ExecuteTree "{target_tree: CrossDoor}" +ros2 action send_goal /behavior_server btcpp_ros2_interfaces/action/ExecuteTree "{target_tree: CrossDoor}" ``` You can also try a Behavior that is a ROS Action or Service client itself. ```bash -ros2 action send_goal /bt_action_server btcpp_ros2_interfaces/action/ExecuteTree "{target_tree: SleepActionSample}" +ros2 action send_goal /behavior_server btcpp_ros2_interfaces/action/ExecuteTree "{target_tree: SleepActionSample}" ``` From 70b5d7451596cf235e2045e59b0e22ec09f40ea6 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 5 May 2024 16:00:35 +0200 Subject: [PATCH 26/44] add exceptions --- .../include/behaviortree_ros2/bt_action_node.hpp | 5 +++++ .../include/behaviortree_ros2/bt_service_node.hpp | 5 +++++ .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 5 +++++ 3 files changed, 15 insertions(+) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index be892de..4ea55c5 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -316,6 +316,11 @@ inline bool RosActionNode::createClient(const std::string& action_name) std::unique_lock lk(getMutex()); auto node = node_.lock(); + if(!node) + { + throw RuntimeError("The ROS node went out of scope. RosNodeParams doesn't take the " + "ownership of the node."); + } action_client_key_ = std::string(node->get_fully_qualified_name()) + "/" + action_name; auto& registry = getRegistry(); diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 93e14ea..f3a6203 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -282,6 +282,11 @@ inline bool RosServiceNode::createClient(const std::string& service_name) std::unique_lock lk(getMutex()); auto node = node_.lock(); + if(!node) + { + throw RuntimeError("The ROS node went out of scope. RosNodeParams doesn't take the " + "ownership of the node."); + } auto client_key = std::string(node->get_fully_qualified_name()) + "/" + service_name; auto& registry = getRegistry(); diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index a8a34fc..f3a6d87 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -250,6 +250,11 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) std::unique_lock lk(registryMutex()); auto shared_node = node_.lock(); + if(!node) + { + throw RuntimeError("The ROS node went out of scope. RosNodeParams doesn't take the " + "ownership of the node."); + } subscriber_key_ = std::string(shared_node->get_fully_qualified_name()) + "/" + topic_name; From 3ccd2e787c844aee527db4c0b16b7f20c997d833 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 5 May 2024 16:05:49 +0200 Subject: [PATCH 27/44] formatting --- behaviortree_ros2/ros_behavior_wrappers.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/behaviortree_ros2/ros_behavior_wrappers.md b/behaviortree_ros2/ros_behavior_wrappers.md index d493639..d93564d 100644 --- a/behaviortree_ros2/ros_behavior_wrappers.md +++ b/behaviortree_ros2/ros_behavior_wrappers.md @@ -13,7 +13,7 @@ Return false if the request should not be sent. In that case, RosActionNode::onF ### BT::NodeStatus onResultReceived(const WrappedResult& result) -Required callback invoked when the result is received by the server. +Required callback invoked when the result is received by the server. It is up to the user to define if the action returns SUCCESS or FAILURE. ### BT::NodeStatus onFeedback(const std::shared_ptr feedback) @@ -61,6 +61,6 @@ Required callback invoked in the tick. You must return either SUCCESS of FAILURE ### bool latchLastMessage() -Optional callback to latch the message that has been processed. -If returns false and no new message is received, before next call there will be no message to process. -If returns true, the next call will process the same message again, if no new message received. \ No newline at end of file +Optional callback to latch the message that has been processed. +If returns false and no new message is received, before next call there will be no message to process. +If returns true, the next call will process the same message again, if no new message received. From 16baeb5f4c3852d5ce8a60895152435a3e5b31c3 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 5 May 2024 16:07:47 +0200 Subject: [PATCH 28/44] fix --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index f3a6d87..211f41c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -249,20 +249,19 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) // find SubscriberInstance in the registry std::unique_lock lk(registryMutex()); - auto shared_node = node_.lock(); + auto node = node_.lock(); if(!node) { throw RuntimeError("The ROS node went out of scope. RosNodeParams doesn't take the " "ownership of the node."); } - subscriber_key_ = - std::string(shared_node->get_fully_qualified_name()) + "/" + topic_name; + subscriber_key_ = std::string(node->get_fully_qualified_name()) + "/" + topic_name; auto& registry = getRegistry(); auto it = registry.find(subscriber_key_); if(it == registry.end() || it->second.expired()) { - sub_instance_ = std::make_shared(shared_node, topic_name); + sub_instance_ = std::make_shared(node, topic_name); registry.insert({ subscriber_key_, sub_instance_ }); RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(), From a0de087a2f3af5ec5a19edd622aa85506105afcd Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 5 May 2024 17:18:14 +0200 Subject: [PATCH 29/44] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index be77ad1..22ff7d1 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ Our main goals are: Note that this library is compatible **only** with: -- **BT.CPP** 4.1 or newer. +- **BT.CPP** 4.6 or newer. - **ROS2** Humble or newer. Additionally, check **plugins.hpp** to see how to learn how to From 8790909163f96a0946ef7504aa3379f4d85ced35 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 6 May 2024 12:44:00 +0200 Subject: [PATCH 30/44] make the sample_bt_executor more interesting --- btcpp_ros2_samples/src/sample_bt_executor.cpp | 23 +++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/btcpp_ros2_samples/src/sample_bt_executor.cpp b/btcpp_ros2_samples/src/sample_bt_executor.cpp index d2d160b..227a9b7 100644 --- a/btcpp_ros2_samples/src/sample_bt_executor.cpp +++ b/btcpp_ros2_samples/src/sample_bt_executor.cpp @@ -14,13 +14,31 @@ #include #include +#include + // Example that shows how to customize TreeExecutionServer. -// Here, we simply add an extra logger +// +// Here, we: +// - add an extra logger, BT::StdCoutLogger +// - add a subscriber that will continuously update a variable in the global blackboard + class MyActionServer : public BT::TreeExecutionServer { public: MyActionServer(const rclcpp::NodeOptions& options) : TreeExecutionServer(options) - {} + { + // here we assume that the battery voltage is published as a std_msgs::msg::Float32 + auto node = std::dynamic_pointer_cast(nodeBaseInterface()); + sub_ = node->create_subscription( + "battery_level", 10, [this](const std_msgs::msg::Float32::SharedPtr msg) { + // Update the global blackboard + globalBlackboard()->set("battery_level", msg->data); + }); + // Note that the callback above and the execution of the tree accessing the + // global blackboard happen in two different threads. + // The former runs in the MultiThreadedExecutor, while the latter in the thread created + // by TreeExecutionServer. But this is OK because the blackboard is thread-safe. + } void onTreeCreated(BT::Tree& tree) override { @@ -36,6 +54,7 @@ class MyActionServer : public BT::TreeExecutionServer private: std::shared_ptr logger_cout_; + rclcpp::Subscription::SharedPtr sub_; }; int main(int argc, char* argv[]) From d5d869e5423c45bda6cac03ad287bcf7c4058ab8 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 7 May 2024 11:43:43 +0200 Subject: [PATCH 31/44] fix issue #63 : expose factory and rclcppNode --- .../behaviortree_ros2/tree_execution_server.hpp | 17 +++++++++++------ behaviortree_ros2/src/tree_execution_server.cpp | 12 +++++++++++- btcpp_ros2_samples/src/sample_bt_executor.cpp | 7 +++---- 3 files changed, 25 insertions(+), 11 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index 9a8839a..0f86498 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -51,20 +51,25 @@ class TreeExecutionServer /** * @brief Gets the NodeBaseInterface of node_. * @details This function exists to allow running TreeExecutionServer as a component in a composable node container. - * - * @return A shared_ptr to the NodeBaseInterface of node_. + * The name of this method shall not change to work properly with the component composer. */ - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr nodeBaseInterface(); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); + + /// @brief Gets the rclcpp::Node pointer + rclcpp::Node::SharedPtr node(); - // name of the tree being executed + /// @brief Name of the tree being executed const std::string& currentTreeName() const; - // tree being executed, nullptr if it doesn't exist yet. + /// @brief Tree being executed, nullptr if it doesn't exist, yet. BT::Tree* currentTree(); - // pointer to the global blackboard + /// @brief Pointer to the global blackboard BT::Blackboard::Ptr globalBlackboard(); + /// @brief Pointer to the global blackboard + BT::BehaviorTreeFactory& factory(); + protected: /** * @brief Callback invoked after the tree is created. diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 26b6f11..212a2d5 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -95,11 +95,16 @@ TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options) } rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -TreeExecutionServer::nodeBaseInterface() +TreeExecutionServer::get_node_base_interface() { return p_->node->get_node_base_interface(); } +rclcpp::Node::SharedPtr TreeExecutionServer::node() +{ + return p_->node; +} + rclcpp_action::GoalResponse TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */, std::shared_ptr goal) @@ -262,4 +267,9 @@ BT::Blackboard::Ptr TreeExecutionServer::globalBlackboard() return p_->global_blackboard; } +BT::BehaviorTreeFactory& TreeExecutionServer::factory() +{ + return p_->factory; +} + } // namespace BT diff --git a/btcpp_ros2_samples/src/sample_bt_executor.cpp b/btcpp_ros2_samples/src/sample_bt_executor.cpp index 227a9b7..aa5b471 100644 --- a/btcpp_ros2_samples/src/sample_bt_executor.cpp +++ b/btcpp_ros2_samples/src/sample_bt_executor.cpp @@ -28,8 +28,7 @@ class MyActionServer : public BT::TreeExecutionServer MyActionServer(const rclcpp::NodeOptions& options) : TreeExecutionServer(options) { // here we assume that the battery voltage is published as a std_msgs::msg::Float32 - auto node = std::dynamic_pointer_cast(nodeBaseInterface()); - sub_ = node->create_subscription( + sub_ = node()->create_subscription( "battery_level", 10, [this](const std_msgs::msg::Float32::SharedPtr msg) { // Update the global blackboard globalBlackboard()->set("battery_level", msg->data); @@ -68,9 +67,9 @@ int main(int argc, char* argv[]) // Deadlock is caused when Publishers or Subscribers are dynamically removed as the node is spinning. rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250)); - exec.add_node(action_server->nodeBaseInterface()); + exec.add_node(action_server->node()); exec.spin(); - exec.remove_node(action_server->nodeBaseInterface()); + exec.remove_node(action_server->node()); rclcpp::shutdown(); } From 7e727ba2fcbb84f25279ca84d16b745ea7b52cc5 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 7 May 2024 12:38:36 +0200 Subject: [PATCH 32/44] fix issue #61: postpone registerNodesIntoFactory --- .../include/behaviortree_ros2/bt_utils.hpp | 19 +++-- .../tree_execution_server.hpp | 12 +++ behaviortree_ros2/src/bt_utils.cpp | 81 ++++++++++--------- .../src/tree_execution_server.cpp | 38 +++++++-- 4 files changed, 99 insertions(+), 51 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp index d846996..e566758 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp @@ -56,14 +56,23 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory, const std::string& directory_path); /** - * @brief Function to load BehaviorTree ROS plugins (or standard BT.CPP plugins) from a specific directory + * @brief Function to load a BehaviorTree ROS plugin (or standard BT.CPP plugins) * + * @param factory BehaviorTreeFactory to register the plugin into + * @param file_path Full path to the directory to search for BehaviorTree plugin + * @param params parameters passed to the ROS plugin + */ +void LoadPlugin(BT::BehaviorTreeFactory& factory, const std::filesystem::path& file_path, + BT::RosNodeParams params); + +/** @brief Function to load all plugins from the specified package + * + * @param params ROS parameters that contain the package name to load plugins from * @param factory BehaviorTreeFactory to register the plugins into - * @param directory_path Full path to the directory to search for BehaviorTree plugins - * @param params parameters passed to the ROS plugins + * @param node node pointer that is shared with the ROS based Behavior plugins */ -void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, - BT::RosNodeParams params); +void RegisterPlugins(bt_server::Params& params, BT::BehaviorTreeFactory& factory, + rclcpp::Node::SharedPtr node); /** * @brief Function to register all Behaviors and BehaviorTrees from user specified packages diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index 0f86498..f48ebd7 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -123,6 +123,18 @@ class TreeExecutionServer return std::nullopt; } +protected: + /** + * @brief Method to register the trees and BT custom nodes. + * It will call registerNodesIntoFactory(). + * + * This callback will invoked asynchronously when this rclcpp Node is attached + * to a rclcpp Executor. + * Alternatively, it can be called synchronously in the constructor of a + * __derived__ class of TreeExecutionServer. + */ + void executeRegistration(); + private: struct Pimpl; std::unique_ptr p_; diff --git a/behaviortree_ros2/src/bt_utils.cpp b/behaviortree_ros2/src/bt_utils.cpp index ce5e5cc..65dc8de 100644 --- a/behaviortree_ros2/src/bt_utils.cpp +++ b/behaviortree_ros2/src/bt_utils.cpp @@ -93,53 +93,42 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory, } } -void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, - BT::RosNodeParams params) +void LoadPlugin(BT::BehaviorTreeFactory& factory, const std::filesystem::path& file_path, + BT::RosNodeParams params) { - using std::filesystem::directory_iterator; - - for(const auto& entry : directory_iterator(directory_path)) + const auto filename = file_path.filename(); + try { - const auto filename = entry.path().filename(); - if(entry.path().extension() == ".so") + BT::SharedLibrary loader(file_path.string()); + if(loader.hasSymbol(BT::PLUGIN_SYMBOL)) { - try - { - BT::SharedLibrary loader(entry.path().string()); - if(loader.hasSymbol(BT::PLUGIN_SYMBOL)) - { - typedef void (*Func)(BehaviorTreeFactory&); - auto func = (Func)loader.getSymbol(BT::PLUGIN_SYMBOL); - func(factory); - } - else if(loader.hasSymbol(BT::ROS_PLUGIN_SYMBOL)) - { - typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&); - auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL); - func(factory, params); - } - else - { - RCLCPP_ERROR(kLogger, "Failed to load Plugin from file: %s.", filename.c_str()); - continue; - } - RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", filename.c_str()); - } - catch(const std::exception& e) - { - RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", filename.c_str(), - e.what()); - } + typedef void (*Func)(BehaviorTreeFactory&); + auto func = (Func)loader.getSymbol(BT::PLUGIN_SYMBOL); + func(factory); + } + else if(loader.hasSymbol(BT::ROS_PLUGIN_SYMBOL)) + { + typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&); + auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL); + func(factory, params); + } + else + { + RCLCPP_ERROR(kLogger, "Failed to load Plugin from file: %s.", filename.c_str()); + return; } + RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", filename.c_str()); + } + catch(const std::exception& ex) + { + RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", filename.c_str(), + ex.what()); } } -void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory, - rclcpp::Node::SharedPtr node) +void RegisterPlugins(bt_server::Params& params, BT::BehaviorTreeFactory& factory, + rclcpp::Node::SharedPtr node) { - // clear the factory and load/reload it with the Behaviors and Trees specified by the user in their [bt_action_server] config yaml - factory.clearRegisteredBehaviorTrees(); - BT::RosNodeParams ros_params; ros_params.nh = node; ros_params.server_timeout = std::chrono::milliseconds(params.ros_plugins_timeout); @@ -153,9 +142,21 @@ void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& f { continue; } - LoadRosPlugins(factory, plugin_directory, ros_params); + using std::filesystem::directory_iterator; + + for(const auto& entry : directory_iterator(plugin_directory)) + { + if(entry.path().extension() == ".so") + { + LoadPlugin(factory, entry.path(), ros_params); + } + } } +} +void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory, + rclcpp::Node::SharedPtr node) +{ for(const auto& tree_dir : params.behavior_trees) { const auto tree_directory = GetDirectoryPath(tree_dir); diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 212a2d5..64b08ae 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -47,6 +47,9 @@ struct TreeExecutionServer::Pimpl std::string current_tree_name; std::shared_ptr tree; BT::Blackboard::Ptr global_blackboard; + bool factory_initialized_ = false; + + rclcpp::WallTimer::SharedPtr single_shot_timer; rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); @@ -70,6 +73,22 @@ TreeExecutionServer::Pimpl::Pimpl(const rclcpp::NodeOptions& node_options) TreeExecutionServer::~TreeExecutionServer() {} +void TreeExecutionServer::executeRegistration() +{ + // Before executing check if we have new Behaviors or Subtrees to reload + p_->factory.clearRegisteredBehaviorTrees(); + + p_->params = p_->param_listener->get_params(); + // user defined method + registerNodesIntoFactory(p_->factory); + // load plugins from multiple directories + RegisterPlugins(p_->params, p_->factory, p_->node); + // load trees (XML) from multiple directories + RegisterBehaviorTrees(p_->params, p_->factory, p_->node); + + p_->factory_initialized_ = true; +} + TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options) : p_(new Pimpl(options)) { @@ -89,9 +108,18 @@ TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options) handle_accepted(std::move(goal_handle)); }); - // register the users Plugins and BehaviorTree.xml files into the factory - RegisterBehaviorTrees(p_->params, p_->factory, p_->node); - registerNodesIntoFactory(p_->factory); + // we use a wall timer to run asynchronously executeRegistration(); + rclcpp::VoidCallbackType callback = [this]() { + if(!p_->factory_initialized_) + { + executeRegistration(); + } + // we must cancel the timer after the first execution + p_->single_shot_timer->cancel(); + }; + + p_->single_shot_timer = + p_->node->create_wall_timer(std::chrono::milliseconds(1), callback); } rclcpp::node_interfaces::NodeBaseInterface::SharedPtr @@ -151,9 +179,7 @@ void TreeExecutionServer::execute( // Before executing check if we have new Behaviors or Subtrees to reload if(p_->param_listener->is_old(p_->params)) { - p_->params = p_->param_listener->get_params(); - RegisterBehaviorTrees(p_->params, p_->factory, p_->node); - registerNodesIntoFactory(p_->factory); + executeRegistration(); } // Loop until something happens with ROS or the node completes From e81f5888ecaf566a968e484be8f7393b4ad9d29c Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 7 May 2024 17:04:17 +0200 Subject: [PATCH 33/44] add constructor and cleanups --- .../tree_execution_server.hpp | 17 +++-- .../src/tree_execution_server.cpp | 71 +++++++------------ 2 files changed, 37 insertions(+), 51 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index f48ebd7..ca423b5 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -37,14 +37,16 @@ class TreeExecutionServer using GoalHandleExecuteTree = rclcpp_action::ServerGoalHandle; /** - * @brief Constructor for TreeExecutionServer. - * @details This initializes a ParameterListener to read configurable options from the user and - * starts an Action Server that takes requests to execute BehaviorTrees. - * - * @param options rclcpp::NodeOptions to pass to node_ when initializing it. - * after the tree is created, while its running and after it finishes. + * @brief Constructor that will create its own instance of rclcpp::Node */ - explicit TreeExecutionServer(const rclcpp::NodeOptions& options); + explicit TreeExecutionServer(const rclcpp::NodeOptions& options) + : TreeExecutionServer(std::make_unique("bt_action_server", options)) + {} + + /** + * @brief Constructor to use when an already existing node should be used. + */ + explicit TreeExecutionServer(const rclcpp::Node::SharedPtr& node); virtual ~TreeExecutionServer(); @@ -138,6 +140,7 @@ class TreeExecutionServer private: struct Pimpl; std::unique_ptr p_; + rclcpp::Node::SharedPtr node_; /** * @brief handle the goal requested: accept or reject. This implementation always accepts. diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 64b08ae..420afa1 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -30,9 +30,6 @@ namespace BT struct TreeExecutionServer::Pimpl { - Pimpl(const rclcpp::NodeOptions& node_options); - - rclcpp::Node::SharedPtr node; rclcpp_action::Server::SharedPtr action_server; std::thread action_thread; // thread safe bool to keep track of cancel requests @@ -50,53 +47,20 @@ struct TreeExecutionServer::Pimpl bool factory_initialized_ = false; rclcpp::WallTimer::SharedPtr single_shot_timer; - - rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, - std::shared_ptr goal); - - rclcpp_action::CancelResponse - handle_cancel(const std::shared_ptr goal_handle); - - void handle_accepted(const std::shared_ptr goal_handle); - - void execute(const std::shared_ptr goal_handle); }; -TreeExecutionServer::Pimpl::Pimpl(const rclcpp::NodeOptions& node_options) - : node(std::make_unique("bt_action_server", node_options)) -{ - param_listener = std::make_shared(node); - params = param_listener->get_params(); - global_blackboard = BT::Blackboard::create(); -} - -TreeExecutionServer::~TreeExecutionServer() -{} - -void TreeExecutionServer::executeRegistration() +TreeExecutionServer::TreeExecutionServer(const rclcpp::Node::SharedPtr& node) + : p_(new Pimpl), node_(node) { - // Before executing check if we have new Behaviors or Subtrees to reload - p_->factory.clearRegisteredBehaviorTrees(); - + p_->param_listener = std::make_shared(node_); p_->params = p_->param_listener->get_params(); - // user defined method - registerNodesIntoFactory(p_->factory); - // load plugins from multiple directories - RegisterPlugins(p_->params, p_->factory, p_->node); - // load trees (XML) from multiple directories - RegisterBehaviorTrees(p_->params, p_->factory, p_->node); - - p_->factory_initialized_ = true; -} + p_->global_blackboard = BT::Blackboard::create(); -TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options) - : p_(new Pimpl(options)) -{ // create the action server const auto action_name = p_->params.action_name; RCLCPP_INFO(kLogger, "Starting Action Server: %s", action_name.c_str()); p_->action_server = rclcpp_action::create_server( - p_->node, action_name, + node_, action_name, [this](const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { return handle_goal(uuid, std::move(goal)); @@ -119,18 +83,37 @@ TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options) }; p_->single_shot_timer = - p_->node->create_wall_timer(std::chrono::milliseconds(1), callback); + node_->create_wall_timer(std::chrono::milliseconds(1), callback); +} + +TreeExecutionServer::~TreeExecutionServer() +{} + +void TreeExecutionServer::executeRegistration() +{ + // Before executing check if we have new Behaviors or Subtrees to reload + p_->factory.clearRegisteredBehaviorTrees(); + + p_->params = p_->param_listener->get_params(); + // user defined method + registerNodesIntoFactory(p_->factory); + // load plugins from multiple directories + RegisterPlugins(p_->params, p_->factory, node_); + // load trees (XML) from multiple directories + RegisterBehaviorTrees(p_->params, p_->factory, node_); + + p_->factory_initialized_ = true; } rclcpp::node_interfaces::NodeBaseInterface::SharedPtr TreeExecutionServer::get_node_base_interface() { - return p_->node->get_node_base_interface(); + return node_->get_node_base_interface(); } rclcpp::Node::SharedPtr TreeExecutionServer::node() { - return p_->node; + return node_; } rclcpp_action::GoalResponse From e7b2926a5f5168eb76d3b5d098a598150ee26f3e Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 7 May 2024 18:25:15 +0200 Subject: [PATCH 34/44] fix issue #57 --- .../behaviortree_ros2/bt_action_node.hpp | 25 ++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 4ea55c5..e2d4f15 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -522,13 +522,32 @@ inline void RosActionNode::halt() template inline void RosActionNode::cancelGoal() { + auto& executor = client_instance_->callback_executor; if(!goal_handle_) { - RCLCPP_WARN(logger(), "cancelGoal called on an empty goal_handle"); - return; + if(future_goal_handle_.valid()) + { + // Here the discussion is if we should block or put a timer for the waiting + auto ret = + executor.spin_until_future_complete(future_goal_handle_, server_timeout_); + if(ret != rclcpp::FutureReturnCode::SUCCESS) + { + // In that case the goal was not accepted or timed out so probably we should do nothing. + return; + } + else + { + goal_handle_ = future_goal_handle_.get(); + future_goal_handle_ = {}; + } + } + else + { + RCLCPP_WARN(logger(), "cancelGoal called on an empty goal_handle"); + return; + } } - auto& executor = client_instance_->callback_executor; auto& action_client = client_instance_->action_client; auto future_result = action_client->async_get_result(goal_handle_); From 69cb36a38fd5dc8eb0620bf0322cd6e347540641 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 8 May 2024 17:53:40 +0200 Subject: [PATCH 35/44] add payloads to Action (#66) --- .../tree_execution_server.hpp | 18 +++-- .../src/tree_execution_server.cpp | 81 +++++++++++-------- .../action/ExecuteTree.action | 19 +++-- btcpp_ros2_samples/src/sample_bt_executor.cpp | 4 +- 4 files changed, 78 insertions(+), 44 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index ca423b5..1760f44 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -61,10 +61,13 @@ class TreeExecutionServer rclcpp::Node::SharedPtr node(); /// @brief Name of the tree being executed - const std::string& currentTreeName() const; + const std::string& treeName() const; - /// @brief Tree being executed, nullptr if it doesn't exist, yet. - BT::Tree* currentTree(); + /// @brief The payload received in the last goal + const std::string& goalPayload() const; + + /// @brief Tree being executed. + const BT::Tree& tree() const; /// @brief Pointer to the global blackboard BT::Blackboard::Ptr globalBlackboard(); @@ -110,9 +113,14 @@ class TreeExecutionServer * * @param status The status of the tree after the last tick * @param was_cancelled True if the action was cancelled by the Action Client + * + * @return if not std::nullopt, the string will be sent as [return_message] to the Action Client. */ - virtual void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) - {} + virtual std::optional onTreeExecutionCompleted(BT::NodeStatus status, + bool was_cancelled) + { + return std::nullopt; + } /** * @brief onLoopFeedback is a callback invoked at each loop, after tree.tickOnce(). diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 420afa1..80c112a 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -32,8 +32,6 @@ struct TreeExecutionServer::Pimpl { rclcpp_action::Server::SharedPtr action_server; std::thread action_thread; - // thread safe bool to keep track of cancel requests - std::atomic cancel_requested{ false }; std::shared_ptr param_listener; bt_server::Params params; @@ -41,8 +39,9 @@ struct TreeExecutionServer::Pimpl BT::BehaviorTreeFactory factory; std::shared_ptr groot_publisher; - std::string current_tree_name; - std::shared_ptr tree; + std::string tree_name; + std::string payload; + BT::Tree tree; BT::Blackboard::Ptr global_blackboard; bool factory_initialized_ = false; @@ -122,7 +121,6 @@ TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */, { RCLCPP_INFO(kLogger, "Received goal request to execute Behavior Tree: %s", goal->target_tree.c_str()); - p_->cancel_requested = false; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -136,7 +134,6 @@ rclcpp_action::CancelResponse TreeExecutionServer::handle_cancel( "processing one."); return rclcpp_action::CancelResponse::REJECT; } - p_->cancel_requested = true; return rclcpp_action::CancelResponse::ACCEPT; } @@ -171,15 +168,15 @@ void TreeExecutionServer::execute( // This blackboard will be owned by "MainTree". It parent is p_->global_blackboard auto root_blackboard = BT::Blackboard::create(p_->global_blackboard); - p_->tree = std::make_shared(); - *(p_->tree) = p_->factory.createTree(goal->target_tree, root_blackboard); - p_->current_tree_name = goal->target_tree; + p_->tree = p_->factory.createTree(goal->target_tree, root_blackboard); + p_->tree_name = goal->target_tree; + p_->payload = goal->payload; // call user defined function after the tree has been created - onTreeCreated(*p_->tree); + onTreeCreated(p_->tree); p_->groot_publisher.reset(); p_->groot_publisher = - std::make_shared(*(p_->tree), p_->params.groot2_port); + std::make_shared(p_->tree, p_->params.groot2_port); // Loop until the tree is done or a cancel is requested const auto period = @@ -187,19 +184,26 @@ void TreeExecutionServer::execute( auto loop_deadline = std::chrono::steady_clock::now() + period; // operations to be done if the tree execution is aborted, either by - // cancel_requested_ or by onLoopAfterTick() + // cancel requested or by onLoopAfterTick() auto stop_action = [this, &action_result](BT::NodeStatus status, const std::string& message) { + p_->tree.haltTree(); action_result->node_status = ConvertNodeStatus(status); - action_result->error_message = message; - RCLCPP_WARN(kLogger, action_result->error_message.c_str()); - p_->tree->haltTree(); - onTreeExecutionCompleted(status, true); + // override the message value if the user defined function returns it + if(auto msg = onTreeExecutionCompleted(status, true)) + { + action_result->return_message = msg.value(); + } + else + { + action_result->return_message = message; + } + RCLCPP_WARN(kLogger, action_result->return_message.c_str()); }; while(rclcpp::ok() && status == BT::NodeStatus::RUNNING) { - if(p_->cancel_requested) + if(goal_handle->is_canceling()) { stop_action(status, "Action Server canceling and halting Behavior Tree"); goal_handle->canceled(action_result); @@ -207,7 +211,7 @@ void TreeExecutionServer::execute( } // tick the tree once and publish the action feedback - status = p_->tree->tickExactlyOnce(); + status = p_->tree.tickExactlyOnce(); if(const auto res = onLoopAfterTick(status); res.has_value()) { @@ -219,28 +223,37 @@ void TreeExecutionServer::execute( if(const auto res = onLoopFeedback(); res.has_value()) { auto feedback = std::make_shared(); - feedback->msg = res.value(); + feedback->message = res.value(); goal_handle->publish_feedback(feedback); } const auto now = std::chrono::steady_clock::now(); if(now < loop_deadline) { - p_->tree->sleep(loop_deadline - now); + p_->tree.sleep(loop_deadline - now); } loop_deadline += period; } } catch(const std::exception& ex) { - action_result->error_message = std::string("Behavior Tree exception:") + ex.what(); - RCLCPP_ERROR(kLogger, action_result->error_message.c_str()); + action_result->return_message = std::string("Behavior Tree exception:") + ex.what(); + RCLCPP_ERROR(kLogger, action_result->return_message.c_str()); goal_handle->abort(action_result); return; } - // call user defined execution complete function - onTreeExecutionCompleted(status, false); + // Call user defined onTreeExecutionCompleted function. + // Override the message value if the user defined function returns it + if(auto msg = onTreeExecutionCompleted(status, false)) + { + action_result->return_message = msg.value(); + } + else + { + action_result->return_message = + std::string("Tree finished with status: ") + BT::toStr(status); + } // set the node_status result to the action action_result->node_status = ConvertNodeStatus(status); @@ -248,27 +261,29 @@ void TreeExecutionServer::execute( // return success or aborted for the action result if(status == BT::NodeStatus::SUCCESS) { - RCLCPP_INFO(kLogger, "BT finished with status: %s", BT::toStr(status).c_str()); + RCLCPP_INFO(kLogger, action_result->return_message.c_str()); goal_handle->succeed(action_result); } else { - action_result->error_message = std::string("Behavior Tree failed during execution " - "with status: ") + - BT::toStr(status); - RCLCPP_ERROR(kLogger, action_result->error_message.c_str()); + RCLCPP_ERROR(kLogger, action_result->return_message.c_str()); goal_handle->abort(action_result); } } -const std::string& TreeExecutionServer::currentTreeName() const +const std::string& TreeExecutionServer::treeName() const +{ + return p_->tree_name; +} + +const std::string& TreeExecutionServer::goalPayload() const { - return p_->current_tree_name; + return p_->payload; } -BT::Tree* TreeExecutionServer::currentTree() +const BT::Tree& TreeExecutionServer::tree() const { - return p_->tree ? p_->tree.get() : nullptr; + return p_->tree; } BT::Blackboard::Ptr TreeExecutionServer::globalBlackboard() diff --git a/btcpp_ros2_interfaces/action/ExecuteTree.action b/btcpp_ros2_interfaces/action/ExecuteTree.action index d12d6f6..5dee87d 100644 --- a/btcpp_ros2_interfaces/action/ExecuteTree.action +++ b/btcpp_ros2_interfaces/action/ExecuteTree.action @@ -1,9 +1,18 @@ -# Request +#### Request #### + +# Name of the tree to execute string target_tree +# Optional, implementation-dependent, payload. +string payload --- -# Result -string error_message +#### Result #### + +# Status of the tree NodeStatus node_status +# result payload or error message +string return_message --- -# Feedback. This can be customized by the user -string msg +#### Feedback #### + +#This can be customized by the user +string message diff --git a/btcpp_ros2_samples/src/sample_bt_executor.cpp b/btcpp_ros2_samples/src/sample_bt_executor.cpp index aa5b471..6b3e3a4 100644 --- a/btcpp_ros2_samples/src/sample_bt_executor.cpp +++ b/btcpp_ros2_samples/src/sample_bt_executor.cpp @@ -44,11 +44,13 @@ class MyActionServer : public BT::TreeExecutionServer logger_cout_ = std::make_shared(tree); } - void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override + std::optional onTreeExecutionCompleted(BT::NodeStatus status, + bool was_cancelled) override { // NOT really needed, even if logger_cout_ may contain a dangling pointer of the tree // at this point logger_cout_.reset(); + return std::nullopt; } private: From 6247e2ed142211ea13ce9a226ad45f94cde69be0 Mon Sep 17 00:00:00 2001 From: Marq Rasmussen Date: Sun, 12 May 2024 12:33:24 -0400 Subject: [PATCH 36/44] Register multiple nodes with single plugin (#68) --- .../include/behaviortree_ros2/plugins.hpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp b/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp index 116fd2c..e3b9108 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp @@ -25,6 +25,25 @@ namespace BT constexpr const char* ROS_PLUGIN_SYMBOL = "BT_RegisterRosNodeFromPlugin"; } +/* Use this macro to automatically register one or more custom Nodes +* into a factory that require access to the BT::RosNodeParams. +* For instance: +* +* BT_REGISTER_ROS_NODES(factory, params) +* { +* factory.registerNodeType("SpecialNode", params); +* } +* +* IMPORTANT: this function MUST be declared in a cpp file, NOT a header file. +* You must add the definition [BT_PLUGIN_EXPORT] in CMakeLists.txt using: +* +* target_compile_definitions(my_plugin_target PRIVATE BT_PLUGIN_EXPORT ) +*/ + +#define BT_REGISTER_ROS_NODES(factory, params) \ + BTCPP_EXPORT void BT_RegisterRosNodeFromPlugin(BT::BehaviorTreeFactory& factory, \ + const BT::RosNodeParams& params) + // Use this macro to generate a plugin for: // // - BT::RosActionNode From 3da7c291f660620adc26ee7b606041c1967cf5cf Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 14 May 2024 14:30:33 -0700 Subject: [PATCH 37/44] Changing new URL (#72) --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 22ff7d1..0c2fad1 100644 --- a/README.md +++ b/README.md @@ -33,6 +33,6 @@ wrap your Nodes into plugins that can be loaded at run-time. ## Acknowledgements -A lot of code is either inspired or copied from [Nav2](https://navigation.ros.org/). +A lot of code is either inspired or copied from [Nav2](https://docs.nav2.org/). For this reason, we retain the same license and copyright. From bb5535e47921a898fd60a7431300e9ca19ec3533 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 15 May 2024 18:07:52 +0200 Subject: [PATCH 38/44] Change the way default port is passed, to allow namespaces --- .../behaviortree_ros2/bt_action_node.hpp | 70 +++++++++---------- .../behaviortree_ros2/bt_service_node.hpp | 66 ++++++++--------- .../behaviortree_ros2/ros_node_params.hpp | 7 ++ .../tree_execution_server.hpp | 9 +++ .../src/tree_execution_server.cpp | 5 ++ btcpp_ros2_samples/CMakeLists.txt | 12 ++++ btcpp_ros2_samples/src/bool_client.cpp | 45 ++++++++++++ btcpp_ros2_samples/src/bool_server.cpp | 37 ++++++++++ btcpp_ros2_samples/src/set_bool_node.cpp | 41 +++++++++++ btcpp_ros2_samples/src/set_bool_node.hpp | 51 ++++++++++++++ 10 files changed, 270 insertions(+), 73 deletions(-) create mode 100644 btcpp_ros2_samples/src/bool_client.cpp create mode 100644 btcpp_ros2_samples/src/bool_server.cpp create mode 100644 btcpp_ros2_samples/src/set_bool_node.cpp create mode 100644 btcpp_ros2_samples/src/set_bool_node.hpp diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index e2d4f15..05c664d 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -108,8 +108,7 @@ class RosActionNode : public BT::ActionNodeBase */ static PortsList providedBasicPorts(PortsList addition) { - PortsList basic = { InputPort("action_name", "__default__placeholder__", - "Action server name") }; + PortsList basic = { InputPort("action_name", "", "Action server name") }; basic.insert(addition.begin(), addition.end()); return basic; } @@ -164,9 +163,12 @@ class RosActionNode : public BT::ActionNodeBase void cancelGoal(); /// The default halt() implementation will call cancelGoal if necessary. - void halt() override final; + void halt() override; - NodeStatus tick() override final; + NodeStatus tick() override; + + /// Can be used to change the name of the action programmatically + void setActionName(const std::string& action_name); protected: struct ActionClientInstance @@ -216,7 +218,7 @@ class RosActionNode : public BT::ActionNodeBase std::weak_ptr node_; std::shared_ptr client_instance_; std::string action_name_; - bool action_name_may_change_ = false; + bool action_name_should_be_checked_ = false; const std::chrono::milliseconds server_timeout_; const std::chrono::milliseconds wait_for_server_timeout_; std::string action_client_key_; @@ -265,44 +267,23 @@ inline RosActionNode::RosActionNode(const std::string& instance_name, auto portIt = config().input_ports.find("action_name"); if(portIt != config().input_ports.end()) { - const std::string& bb_action_name = portIt->second; + const std::string& bb_service_name = portIt->second; - if(bb_action_name.empty() || bb_action_name == "__default__placeholder__") - { - if(params.default_port_value.empty()) - { - throw std::logic_error("Both [action_name] in the InputPort and the " - "RosNodeParams are empty."); - } - else - { - createClient(params.default_port_value); - } - } - else if(!isBlackboardPointer(bb_action_name)) + if(isBlackboardPointer(bb_service_name)) { - // If the content of the port "action_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. - createClient(bb_action_name); + // unknown value at construction time. Postpone to tick + action_name_should_be_checked_ = true; } - else + else if(!bb_service_name.empty()) { - action_name_may_change_ = true; - // createClient will be invoked in the first tick(). + // "hard-coded" name in the bb_service_name. Use it. + createClient(bb_service_name); } } - else + // no port value or it is empty. Use the default value + if(!client_instance_ && !params.default_port_value.empty()) { - if(params.default_port_value.empty()) - { - throw std::logic_error("Both [action_name] in the InputPort and the RosNodeParams " - "are empty."); - } - else - { - createClient(params.default_port_value); - } + createClient(params.default_port_value); } } @@ -347,6 +328,13 @@ inline bool RosActionNode::createClient(const std::string& action_name) return found; } +template +inline void RosActionNode::setActionName(const std::string& action_name) +{ + action_name_ = action_name; + createClient(action_name); +} + template inline NodeStatus RosActionNode::tick() { @@ -359,7 +347,8 @@ inline NodeStatus RosActionNode::tick() // First, check if the action_client_ is valid and that the name of the // action_name in the port didn't change. // otherwise, create a new client - if(!client_instance_ || (status() == NodeStatus::IDLE && action_name_may_change_)) + if(!client_instance_ || + (status() == NodeStatus::IDLE && action_name_should_be_checked_)) { std::string action_name; getInput("action_name", action_name); @@ -368,6 +357,13 @@ inline NodeStatus RosActionNode::tick() createClient(action_name); } } + + if(!client_instance_) + { + throw BT::RuntimeError("RosActionNode: no client was specified neither as default or " + "in the ports"); + } + auto& action_client = client_instance_->action_client; //------------------------------------------ diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index f3a6203..77958cd 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -96,8 +96,7 @@ class RosServiceNode : public BT::ActionNodeBase */ static PortsList providedBasicPorts(PortsList addition) { - PortsList basic = { InputPort("service_name", "__default__placeholder__", - "Service name") }; + PortsList basic = { InputPort("service_name", "", "Service name") }; basic.insert(addition.begin(), addition.end()); return basic; } @@ -111,7 +110,7 @@ class RosServiceNode : public BT::ActionNodeBase return providedBasicPorts({}); } - NodeStatus tick() override final; + NodeStatus tick() override; /// The default halt() implementation. void halt() override; @@ -157,6 +156,9 @@ class RosServiceNode : public BT::ActionNodeBase return action_client_mutex; } + // method to set the service name programmatically + void setServiceName(const std::string& service_name); + rclcpp::Logger logger() { if(auto node = node_.lock()) @@ -186,7 +188,7 @@ class RosServiceNode : public BT::ActionNodeBase std::weak_ptr node_; std::string service_name_; - bool service_name_may_change_ = false; + bool service_name_should_be_checked_ = false; const std::chrono::milliseconds service_timeout_; const std::chrono::milliseconds wait_for_service_timeout_; @@ -233,51 +235,30 @@ inline RosServiceNode::RosServiceNode(const std::string& instance_name, { const std::string& bb_service_name = portIt->second; - if(bb_service_name.empty() || bb_service_name == "__default__placeholder__") + if(isBlackboardPointer(bb_service_name)) { - if(params.default_port_value.empty()) - { - throw std::logic_error("Both [service_name] in the InputPort and the " - "RosNodeParams are empty."); - } - else - { - createClient(params.default_port_value); - } + // unknown value at construction time. postpone to tick + service_name_should_be_checked_ = true; } - else if(!isBlackboardPointer(bb_service_name)) + else if(!bb_service_name.empty()) { - // If the content of the port "service_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. + // "hard-coded" name in the bb_service_name. Use it. createClient(bb_service_name); } - else - { - service_name_may_change_ = true; - // createClient will be invoked in the first tick(). - } } - else + // no port value or it is empty. Use the default port value + if(!srv_instance_ && !params.default_port_value.empty()) { - if(params.default_port_value.empty()) - { - throw std::logic_error("Both [service_name] in the InputPort and the RosNodeParams " - "are empty."); - } - else - { - createClient(params.default_port_value); - } + createClient(params.default_port_value); } } template inline bool RosServiceNode::createClient(const std::string& service_name) { - if(service_name.empty()) + if(service_name.empty() || service_name == "__default__placeholder__") { - throw RuntimeError("service_name is empty"); + throw RuntimeError("service_name is empty or invalid"); } std::unique_lock lk(getMutex()); @@ -314,6 +295,13 @@ inline bool RosServiceNode::createClient(const std::string& service_name) return found; } +template +inline void RosServiceNode::setServiceName(const std::string& service_name) +{ + service_name_ = service_name; + createClient(service_name); +} + template inline NodeStatus RosServiceNode::tick() { @@ -326,7 +314,7 @@ inline NodeStatus RosServiceNode::tick() // First, check if the service_client is valid and that the name of the // service_name in the port didn't change. // otherwise, create a new client - if(!srv_instance_ || (status() == NodeStatus::IDLE && service_name_may_change_)) + if(!srv_instance_ || (status() == NodeStatus::IDLE && service_name_should_be_checked_)) { std::string service_name; getInput("service_name", service_name); @@ -336,6 +324,12 @@ inline NodeStatus RosServiceNode::tick() } } + if(!srv_instance_) + { + throw BT::RuntimeError("RosServiceNode: no service client was specified neither as " + "default or in the ports"); + } + auto CheckStatus = [](NodeStatus status) { if(!isStatusCompleted(status)) { diff --git a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp index 484429b..d9e1de7 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp @@ -40,6 +40,13 @@ struct RosNodeParams std::chrono::milliseconds server_timeout = std::chrono::milliseconds(1000); // timeout used when detecting the server the first time std::chrono::milliseconds wait_for_server_timeout = std::chrono::milliseconds(500); + + RosNodeParams() = default; + RosNodeParams(std::shared_ptr node) : nh(node) + {} + RosNodeParams(std::shared_ptr node, const std::string& port_name) + : nh(node), default_port_value(port_name) + {} }; } // namespace BT diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index 1760f44..f472cfd 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -76,6 +76,15 @@ class TreeExecutionServer BT::BehaviorTreeFactory& factory(); protected: + /** + * @brief Callback invoked when a goal is received and before the tree is created. + * If it returns false, the goal will be rejected. + */ + virtual bool onGoalReceived(const std::string& tree_name, const std::string& payload) + { + return true; + } + /** * @brief Callback invoked after the tree is created. * It can be used, for instance, to initialize a logger or the global blackboard. diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 80c112a..c7dd46b 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -121,6 +121,11 @@ TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */, { RCLCPP_INFO(kLogger, "Received goal request to execute Behavior Tree: %s", goal->target_tree.c_str()); + + if(!onGoalReceived(goal->target_tree, goal->payload)) + { + return rclcpp_action::GoalResponse::REJECT; + } return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index 785d674..0ff08e5 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -9,10 +9,12 @@ find_package(ament_cmake REQUIRED) find_package(behaviortree_ros2 REQUIRED) find_package(btcpp_ros2_interfaces REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) set(THIS_PACKAGE_DEPS behaviortree_ros2 std_msgs + std_srvs btcpp_ros2_interfaces ) ###################################################### @@ -50,6 +52,14 @@ ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS}) add_executable(subscriber_test src/subscriber_test.cpp) ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS}) +###################################################### +# the SetBool test +add_executable(bool_client src/bool_client.cpp src/set_bool_node.cpp) +ament_target_dependencies(bool_client ${THIS_PACKAGE_DEPS}) + +add_executable(bool_server src/bool_server.cpp ) +ament_target_dependencies(bool_server ${THIS_PACKAGE_DEPS}) + ###################################################### # INSTALL @@ -60,6 +70,8 @@ install(TARGETS sleep_plugin subscriber_test sample_bt_executor + bool_client + bool_server DESTINATION lib/${PROJECT_NAME} ) diff --git a/btcpp_ros2_samples/src/bool_client.cpp b/btcpp_ros2_samples/src/bool_client.cpp new file mode 100644 index 0000000..dc00710 --- /dev/null +++ b/btcpp_ros2_samples/src/bool_client.cpp @@ -0,0 +1,45 @@ +#include "behaviortree_ros2/bt_action_node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors.hpp" + +#include "set_bool_node.hpp" + +using namespace BT; + +static const char* xml_text = R"( + + + + + + + + + + + + )"; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto nh = std::make_shared("bool_client"); + + BehaviorTreeFactory factory; + + // version with default port + factory.registerNodeType("SetBoolA", BT::RosNodeParams(nh, "robotA/" + "set_bool")); + + // version without default port + factory.registerNodeType("SetBool", BT::RosNodeParams(nh)); + + // namespace version + factory.registerNodeType("SetRobotBool", nh, "set_bool"); + + auto tree = factory.createTreeFromText(xml_text); + + tree.tickWhileRunning(); + + return 0; +} diff --git a/btcpp_ros2_samples/src/bool_server.cpp b/btcpp_ros2_samples/src/bool_server.cpp new file mode 100644 index 0000000..91c77e8 --- /dev/null +++ b/btcpp_ros2_samples/src/bool_server.cpp @@ -0,0 +1,37 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/set_bool.hpp" + +#include + +void CallbackA(const std::shared_ptr request, + std::shared_ptr response) +{ + response->success = request->data; + auto logger = rclcpp::get_logger("rclcpp"); + RCLCPP_INFO(logger, "Incoming request A: %s", request->data ? "true" : "false"); +} + +void CallbackB(const std::shared_ptr request, + std::shared_ptr response) +{ + response->success = request->data; + auto logger = rclcpp::get_logger("rclcpp"); + RCLCPP_INFO(logger, "Incoming request B: %s", request->data ? "true" : "false"); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("add_two_ints_server"); + + auto serviceA = + node->create_service("robotA/set_bool", &CallbackA); + auto serviceB = + node->create_service("robotB/set_bool", &CallbackB); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready."); + + rclcpp::spin(node); + rclcpp::shutdown(); +} diff --git a/btcpp_ros2_samples/src/set_bool_node.cpp b/btcpp_ros2_samples/src/set_bool_node.cpp new file mode 100644 index 0000000..8d8240d --- /dev/null +++ b/btcpp_ros2_samples/src/set_bool_node.cpp @@ -0,0 +1,41 @@ +#include "set_bool_node.hpp" + +bool SetBoolService::setRequest(Request::SharedPtr& request) +{ + getInput("value", request->data); + std::cout << "setRequest " << std::endl; + return true; +} + +BT::NodeStatus SetBoolService::onResponseReceived(const Response::SharedPtr& response) +{ + std::cout << "onResponseReceived " << std::endl; + if(response->success) + { + RCLCPP_INFO(logger(), "SetBool service succeeded."); + return BT::NodeStatus::SUCCESS; + } + else + { + RCLCPP_INFO(logger(), "SetBool service failed: %s", response->message.c_str()); + return BT::NodeStatus::FAILURE; + } +} + +BT::NodeStatus SetBoolService::onFailure(BT::ServiceNodeErrorCode error) +{ + RCLCPP_ERROR(logger(), "Error: %d", error); + return BT::NodeStatus::FAILURE; +} + +//----------------------------------------------------------- + +BT::NodeStatus SetRobotBoolService::tick() +{ + std::string robot; + if(getInput("robot", robot) && !robot.empty()) + { + setServiceName(robot + "/" + service_suffix_); + } + return SetBoolService::tick(); +} diff --git a/btcpp_ros2_samples/src/set_bool_node.hpp b/btcpp_ros2_samples/src/set_bool_node.hpp new file mode 100644 index 0000000..86a12d3 --- /dev/null +++ b/btcpp_ros2_samples/src/set_bool_node.hpp @@ -0,0 +1,51 @@ +#pragma once + +#include +#include "std_srvs/srv/set_bool.hpp" + +using SetBool = std_srvs::srv::SetBool; + +class SetBoolService : public BT::RosServiceNode +{ +public: + explicit SetBoolService(const std::string& name, const BT::NodeConfig& conf, + const BT::RosNodeParams& params) + : RosServiceNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ BT::InputPort("value") }); + } + + bool setRequest(Request::SharedPtr& request) override; + + BT::NodeStatus onResponseReceived(const Response::SharedPtr& response) override; + + virtual BT::NodeStatus onFailure(BT::ServiceNodeErrorCode error) override; + +private: + std::string service_suffix_; +}; + +//---------------------------------------------- + +class SetRobotBoolService : public SetBoolService +{ +public: + explicit SetRobotBoolService(const std::string& name, const BT::NodeConfig& conf, + const rclcpp::Node::SharedPtr& node, + const std::string& port_name) + : SetBoolService(name, conf, BT::RosNodeParams(node)), service_suffix_(port_name) + {} + + static BT::PortsList providedPorts() + { + return { BT::InputPort("robot"), BT::InputPort("value") }; + } + + BT::NodeStatus tick() override; + +private: + std::string service_suffix_; +}; From 354f1115ccabcb6ba18d2c2b04d9fafaa83f4f26 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 11 Jun 2024 11:03:38 +0200 Subject: [PATCH 39/44] fix CI --- btcpp_ros2_samples/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/btcpp_ros2_samples/package.xml b/btcpp_ros2_samples/package.xml index b8c33b3..d571c8a 100644 --- a/btcpp_ros2_samples/package.xml +++ b/btcpp_ros2_samples/package.xml @@ -13,6 +13,7 @@ behaviortree_ros2 std_msgs + std_srvs btcpp_ros2_interfaces From d9629581c905262d7734dbe96be91b19bd99925b Mon Sep 17 00:00:00 2001 From: Adrian Zwiener Date: Fri, 28 Jun 2024 10:44:58 +0200 Subject: [PATCH 40/44] Rename msgs to msg (#81) Fixes the issue rosidl_adapter.parser.InvalidResourceName when calling ros2 interface show btcpp_ros2_interfaces * --- btcpp_ros2_interfaces/CMakeLists.txt | 2 +- btcpp_ros2_interfaces/{msgs => msg}/NodeStatus.msg | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename btcpp_ros2_interfaces/{msgs => msg}/NodeStatus.msg (100%) diff --git a/btcpp_ros2_interfaces/CMakeLists.txt b/btcpp_ros2_interfaces/CMakeLists.txt index 72612f3..23ca9aa 100644 --- a/btcpp_ros2_interfaces/CMakeLists.txt +++ b/btcpp_ros2_interfaces/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(btcpp_ros2_interfaces - "msgs/NodeStatus.msg" + "msg/NodeStatus.msg" "action/ExecuteTree.action" "action/Sleep.action") diff --git a/btcpp_ros2_interfaces/msgs/NodeStatus.msg b/btcpp_ros2_interfaces/msg/NodeStatus.msg similarity index 100% rename from btcpp_ros2_interfaces/msgs/NodeStatus.msg rename to btcpp_ros2_interfaces/msg/NodeStatus.msg From aa7fd831a5dc3bf6872df8cf2ad31f5d50e42664 Mon Sep 17 00:00:00 2001 From: tony-p Date: Tue, 20 Aug 2024 23:36:00 +0200 Subject: [PATCH 41/44] fix: resolve MSVC build errors and warnings (#91) --- behaviortree_ros2/src/tree_execution_server.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index c7dd46b..f712a52 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -11,7 +11,14 @@ // COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR // OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable : 4244) #include +#pragma warning(pop) +#else +#include +#endif #include "behaviortree_ros2/tree_execution_server.hpp" #include "behaviortree_ros2/bt_utils.hpp" @@ -235,7 +242,8 @@ void TreeExecutionServer::execute( const auto now = std::chrono::steady_clock::now(); if(now < loop_deadline) { - p_->tree.sleep(loop_deadline - now); + p_->tree.sleep(std::chrono::duration_cast( + loop_deadline - now)); } loop_deadline += period; } From adec04ba58531de37f87328632a72a506cc6b0d4 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Fri, 6 Sep 2024 09:53:56 -0400 Subject: [PATCH 42/44] Fix link to global blackboard tutorial (#94) --- behaviortree_ros2/tree_execution_server.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/behaviortree_ros2/tree_execution_server.md b/behaviortree_ros2/tree_execution_server.md index 34edf44..e7347cd 100644 --- a/behaviortree_ros2/tree_execution_server.md +++ b/behaviortree_ros2/tree_execution_server.md @@ -18,7 +18,7 @@ Furthermore, the user can customize it to: - Register custom BT Nodes directly (static linking). - Attach additional loggers. The **Groot2** publisher will be attached by default. -- Use the "global blackboard", a new idiom/pattern explained in [this tutorial](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/examples/t19_global_blackboard.cpp). +- Use the "global blackboard", a new idiom/pattern explained in [this tutorial](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/examples/t16_global_blackboard.cpp). - Customize the feedback of the `rclcpp_action::Server`. ## Customization points From cc31ea7b97947f1aac6e8c37df6cec379c84a7d9 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 29 Sep 2024 17:42:16 +0200 Subject: [PATCH 43/44] Update bt_utils.cpp fix #97 --- behaviortree_ros2/src/bt_utils.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/behaviortree_ros2/src/bt_utils.cpp b/behaviortree_ros2/src/bt_utils.cpp index 65dc8de..94d1b9c 100644 --- a/behaviortree_ros2/src/bt_utils.cpp +++ b/behaviortree_ros2/src/bt_utils.cpp @@ -29,14 +29,19 @@ btcpp_ros2_interfaces::msg::NodeStatus ConvertNodeStatus(BT::NodeStatus& status) { case BT::NodeStatus::RUNNING: action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::RUNNING; + break; case BT::NodeStatus::SUCCESS: action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::SUCCESS; + break; case BT::NodeStatus::FAILURE: action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::FAILURE; + break; case BT::NodeStatus::IDLE: action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::IDLE; + break; case BT::NodeStatus::SKIPPED: action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::SKIPPED; + break; } return action_status; From 8143027179c0aa918a7c30f3c5bb73c3c278ec3f Mon Sep 17 00:00:00 2001 From: Gregory LeMasurier Date: Wed, 20 Nov 2024 16:27:50 -0500 Subject: [PATCH 44/44] fix compile issue --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index df939b7..a58ce03 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -166,7 +166,7 @@ class RosTopicSubNode : public BT::ConditionNode //---------------------------------------------------------------- template inline RosTopicSubNode::SubscriberInstance::SubscriberInstance( - std::shared_ptr node, const std::string& topic_name) + std::shared_ptr node, const std::string& topic_name, const rclcpp::QoS& qos_profile) { // create a callback group for this particular instance callback_group =