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..0c2fad1 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,9 +16,15 @@ 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. +- **BT.CPP** 4.6 or newer. - **ROS2** Humble or newer. Additionally, check **plugins.hpp** to see how to learn how to @@ -26,8 +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. - - diff --git a/behaviortree_ros2/CMakeLists.txt b/behaviortree_ros2/CMakeLists.txt index 3c46da2..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) +install(DIRECTORY include/ DESTINATION 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/bt_executor_parameters.md b/behaviortree_ros2/bt_executor_parameters.md new file mode 100644 index 0000000..877d5ec --- /dev/null +++ b/behaviortree_ros2/bt_executor_parameters.md @@ -0,0 +1,74 @@ +# 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/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index c6b0c97..05c664d 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,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; } @@ -122,7 +124,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,26 +162,68 @@ 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; + 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 + { + 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() + { + 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 = + 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_; - bool action_name_may_change_ = false; + std::weak_ptr node_; + std::shared_ptr client_instance_; + std::string action_name_; + 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_; 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_; @@ -187,21 +232,31 @@ 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 +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, + 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 @@ -212,93 +267,117 @@ template inline 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(isBlackboardPointer(bb_service_name)) { - 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); - } + // unknown value at construction time. Postpone to tick + action_name_should_be_checked_ = true; } - else if(!isBlackboardPointer(bb_action_name)) + else if(!bb_service_name.empty()) { - // 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); - } - else { - 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 { - - 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); - } + // no port value or it is empty. Use the default value + if(!client_instance_ && !params.default_port_value.empty()) + { + 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()); - action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_); + 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; - prev_action_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); + registry.insert({ action_client_key_, client_instance_ }); + } + else + { + client_instance_ = it->second.lock(); + } + + action_name_ = action_name; - bool found = action_client_->wait_for_action_server(wait_for_server_timeout_); + 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; } -template inline - NodeStatus RosActionNode::tick() +template +inline void RosActionNode::setActionName(const std::string& action_name) +{ + action_name_ = action_name; + createClient(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_should_be_checked_)) { std::string action_name; getInput("action_name", action_name); - if(prev_action_name_ != action_name) + if(action_name_ != action_name) { createClient(action_name); } } - //------------------------------------------ - auto CheckStatus = [](NodeStatus status) + if(!client_instance_) { - if( !isStatusCompleted(status) ) + throw BT::RuntimeError("RosActionNode: no client was specified neither as default or " + "in the ports"); + } + + 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; }; // 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 +388,81 @@ 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(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(logger(), "Goal was rejected by server"); + } + else + { + 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 ); - time_goal_sent_ = node_->now(); + future_goal_handle_ = action_client->async_send_goal(goal, goal_options); + time_goal_sent_ = now(); return NodeStatus::RUNNING; } - if (status() == NodeStatus::RUNNING) + 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_ ) + 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 = client_instance_->callback_executor.spin_until_future_complete( + 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) ); + return CheckStatus(onFailure(SEND_GOAL_TIMEOUT)); } - else{ + else + { return NodeStatus::RUNNING; } } @@ -389,39 +472,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,29 +515,53 @@ template inline } } -template inline - void RosActionNode::cancelGoal() +template +inline void RosActionNode::cancelGoal() { - 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; + if(!goal_handle_) + { + 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& 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_); - if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) + constexpr auto SUCCESS = 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()); } } - - - } // 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..77958cd 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,13 +68,13 @@ 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; + using ServiceClientPtr = std::shared_ptr; using Request = typename ServiceT::Request; using Response = typename ServiceT::Response; @@ -78,8 +82,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 +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; } @@ -109,7 +110,7 @@ class RosServiceNode : public BT::ActionNodeBase return providedBasicPorts({}); } - NodeStatus tick() override final; + NodeStatus tick() override; /// The default halt() implementation. void halt() override; @@ -127,30 +128,72 @@ 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: + 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; + } - std::shared_ptr node_; - std::string prev_service_name_; - bool service_name_may_change_ = false; + // method to set the service name programmatically + void setServiceName(const std::string& service_name); + + rclcpp::Logger 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 = + 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::weak_ptr node_; + std::string service_name_; + bool service_name_should_be_checked_ = 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_; @@ -158,21 +201,33 @@ 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::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, + 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"); @@ -180,89 +235,112 @@ template inline { 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 { - - 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); - } + // no port value or it is empty. Use the default port value + if(!srv_instance_ && !params.default_port_value.empty()) + { + 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()) + if(service_name.empty() || service_name == "__default__placeholder__") { - throw RuntimeError("service_name is empty"); + throw RuntimeError("service_name is empty or invalid"); } - 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 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; - bool found = service_client_->wait_for_service(wait_for_service_timeout_); + 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 + { + srv_instance_ = it->second.lock(); + } + service_name_ = 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(), prev_service_name_.c_str()); + RCLCPP_ERROR(logger(), "%s: Service with name '%s' is not reachable.", name().c_str(), + service_name_.c_str()); } return found; } -template inline - NodeStatus RosServiceNode::tick() +template +inline void RosServiceNode::setServiceName(const std::string& service_name) { - // First, check if the service_client_ is valid and that the name of the + service_name_ = service_name; + createClient(service_name); +} + +template +inline NodeStatus RosServiceNode::tick() +{ + 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_should_be_checked_)) { std::string service_name; getInput("service_name", service_name); - if(prev_service_name_ != service_name) + if(service_name_ != service_name) { createClient(service_name); } } - auto CheckStatus = [](NodeStatus status) + if(!srv_instance_) { - if( !isStatusCompleted(status) ) + throw BT::RuntimeError("RosServiceNode: no service client was specified neither as " + "default or in the ports"); + } + + 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; }; // 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,40 +351,45 @@ 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 - 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(); - time_request_sent_ = node_->now(); + future_response_ = srv_instance_->service_client->async_send_request(request).share(); + time_request_sent_ = now(); return NodeStatus::RUNNING; } - if (status() == NodeStatus::RUNNING) + 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_ ) + 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 = srv_instance_->callback_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((now() - time_request_sent_) > timeout) { - return CheckStatus( onFailure(SERVICE_TIMEOUT) ); + return CheckStatus(onFailure(SERVICE_TIMEOUT)); } - else{ + else + { return NodeStatus::RUNNING; } } @@ -316,27 +399,26 @@ template inline response_ = future_response_.get(); future_response_ = {}; - if (!response_) { - throw std::runtime_error("Request was rejected by the service"); + if(!response_) + { + throw RuntimeError("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..8ccdc69 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; @@ -40,12 +39,11 @@ 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. * */ - 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 f68226d..a58ce03 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,41 +41,23 @@ 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, const rclcpp::QoS& qos_profile) - { - // 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) - { - broadcaster(msg); - }; - subscriber = node->create_subscription(topic_name, qos_profile, callback, option); - } + SubscriberInstance(std::shared_ptr node, const std::string& topic_name, const rclcpp::QoS& qos_profile); 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() @@ -85,15 +66,18 @@ 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; } - std::shared_ptr node_; - std::shared_ptr sub_instance_ = nullptr; + std::weak_ptr node_; + std::shared_ptr sub_instance_; std::shared_ptr last_msg_; std::string topic_name_; boost::signals2::connection signal_connection_; @@ -102,41 +86,27 @@ 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: - +public: /** 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. * */ - 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 - // 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() ); - } - } } /** @@ -147,9 +117,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; } @@ -169,11 +138,24 @@ 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 + * 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; + } + private: bool createSubscriber(const std::string& topic_name, const rclcpp::QoS& qos_profile); @@ -182,14 +164,33 @@ class RosTopicSubNode : public BT::ConditionNode //---------------------------------------------------------------- //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- +template +inline RosTopicSubNode::SubscriberInstance::SubscriberInstance( + std::shared_ptr node, const std::string& topic_name, const rclcpp::QoS& qos_profile) +{ + // 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, qos_profile, callback, option); +} -template inline - RosTopicSubNode::RosTopicSubNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams& params) - : BT::ConditionNode(instance_name, conf), - node_(params.nh), qos_profile_(params.qos_profile) -{ +template +inline RosTopicSubNode::RosTopicSubNode(const std::string& instance_name, + const NodeConfig& conf, + const RosNodeParams& params) + : BT::ConditionNode(instance_name, conf), node_(params.nh), qos_profile_(params.qos_profile) +{ // check port remapping auto portIt = config().input_ports.find("topic_name"); if(portIt != config().input_ports.end()) @@ -198,11 +199,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, params.qos_profile); } } @@ -213,24 +216,28 @@ template inline // create the client in the constructor. createSubscriber(bb_topic_name, params.qos_profile); } - 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, params.qos_profile); } } } -template inline - bool RosTopicSubNode::createSubscriber(const std::string& topic_name, const rclcpp::QoS& qos_profile) +template +inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name, const rclcpp::QoS& qos_profile) { if(topic_name.empty()) { @@ -243,41 +250,60 @@ template inline // find SubscriberInstance in the registry std::unique_lock lk(registryMutex()); - subscriber_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name; + + 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(node->get_fully_qualified_name()) + "/" + topic_name; auto& registry = getRegistry(); auto it = registry.find(subscriber_key_); - if(it == registry.end()) + if(it == registry.end() || it->second.expired()) { - it = registry.insert( {subscriber_key_, std::make_shared() }).first; - sub_instance_ = it->second; - sub_instance_->init(node_, topic_name, qos_profile); + sub_instance_ = std::make_shared(node, topic_name, qos_profile); + registry.insert({ subscriber_key_, sub_instance_ }); - 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 { - sub_instance_ = it->second; + else + { + sub_instance_ = it->second.lock(); } + // 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( - [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. // otherwise, create a new subscriber + std::string topic_name; + getInput("topic_name", topic_name); + + if(!topic_name.empty() && topic_name != "__default__placeholder__" && + topic_name != topic_name_) + { + sub_instance_.reset(); + } + if(!sub_instance_) { std::string topic_name; @@ -285,21 +311,21 @@ template inline createSubscriber(topic_name, qos_profile_); } - 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_)); - last_msg_ = nullptr; - + auto status = CheckStatus(onTick(last_msg_)); + if(!latchLastMessage()) + { + last_msg_.reset(); + } return status; } } // namespace BT - 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..e566758 --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp @@ -0,0 +1,88 @@ +// 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 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 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 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 LoadBehaviorTrees(BT::BehaviorTreeFactory& factory, + const std::string& directory_path); + +/** + * @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 node node pointer that is shared with the ROS based Behavior plugins + */ +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 + * + * @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 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 d94f529..e3b9108 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp @@ -20,6 +20,29 @@ #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 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: // @@ -34,14 +57,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 +72,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&); - auto func = (Func)loader.getSymbol("BT_RegisterRosNodeFromPlugin"); + typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&); + auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL); 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 38cbb2b..ecf4200 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: // @@ -41,8 +41,15 @@ struct RosNodeParams // 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) + {} + // parameter only used by the subscriber rclcpp::QoS qos_profile=1; }; -} +} // namespace 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..f472cfd --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -0,0 +1,192 @@ +// 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 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 +{ +public: + using ExecuteTree = btcpp_ros2_interfaces::action::ExecuteTree; + using GoalHandleExecuteTree = rclcpp_action::ServerGoalHandle; + + /** + * @brief Constructor that will create its own instance of rclcpp::Node + */ + 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(); + + /** + * @brief Gets the NodeBaseInterface of node_. + * @details This function exists to allow running TreeExecutionServer as a component in a composable node container. + * The name of this method shall not change to work properly with the component composer. + */ + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); + + /// @brief Gets the rclcpp::Node pointer + rclcpp::Node::SharedPtr node(); + + /// @brief Name of the tree being executed + const std::string& treeName() const; + + /// @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(); + + /// @brief Pointer to the global blackboard + 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. + * + * @param tree The tree that was created + */ + virtual void onTreeCreated(BT::Tree& tree) + {} + + /** + * @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) + {} + + /** + * @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; + } + + /** + * @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 + * + * @return if not std::nullopt, the string will be sent as [return_message] to the Action Client. + */ + 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(). + * 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; + } + +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_; + rclcpp::Node::SharedPtr node_; + + /** + * @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 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/ros_behavior_wrappers.md b/behaviortree_ros2/ros_behavior_wrappers.md new file mode 100644 index 0000000..d93564d --- /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. diff --git a/behaviortree_ros2/src/bt_executor_parameters.yaml b/behaviortree_ros2/src/bt_executor_parameters.yaml new file mode 100644 index 0000000..b6c36c1 --- /dev/null +++ b/behaviortree_ros2/src/bt_executor_parameters.yaml @@ -0,0 +1,49 @@ +bt_server: + action_name: { + type: string, + default_value: "bt_execution", + read_only: true, + description: "The name the Action Server takes requests from", + } + 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_timeout: { + type: int, + default_value: 1000, + description: "Timeout (ms) used in BT::RosNodeParams", + validation: { + bounds<>: [1, 10000] + } + } + 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..94d1b9c --- /dev/null +++ b/behaviortree_ros2/src/bt_utils.cpp @@ -0,0 +1,175 @@ +// 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" +#include "behaviortree_ros2/plugins.hpp" + +namespace +{ +static const auto kLogger = rclcpp::get_logger("bt_action_server"); +} + +namespace BT +{ + +btcpp_ros2_interfaces::msg::NodeStatus ConvertNodeStatus(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; + 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; +} + +std::string GetDirectoryPath(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 LoadBehaviorTrees(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 LoadPlugin(BT::BehaviorTreeFactory& factory, const std::filesystem::path& file_path, + BT::RosNodeParams params) +{ + const auto filename = file_path.filename(); + try + { + BT::SharedLibrary loader(file_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()); + 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 RegisterPlugins(bt_server::Params& params, BT::BehaviorTreeFactory& factory, + rclcpp::Node::SharedPtr node) +{ + 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; + } + 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); + // skip invalid subtree directories + if(tree_directory.empty()) + continue; + LoadBehaviorTrees(factory, tree_directory); + } +} + +} // namespace 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..f712a52 --- /dev/null +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -0,0 +1,312 @@ +// 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. + +#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" + +#include "behaviortree_cpp/loggers/groot2_publisher.h" + +// generated file +#include "bt_executor_parameters.hpp" +namespace +{ +static const auto kLogger = rclcpp::get_logger("bt_action_server"); +} + +namespace BT +{ + +struct TreeExecutionServer::Pimpl +{ + rclcpp_action::Server::SharedPtr action_server; + std::thread action_thread; + + std::shared_ptr param_listener; + bt_server::Params params; + + BT::BehaviorTreeFactory factory; + std::shared_ptr groot_publisher; + + std::string tree_name; + std::string payload; + BT::Tree tree; + BT::Blackboard::Ptr global_blackboard; + bool factory_initialized_ = false; + + rclcpp::WallTimer::SharedPtr single_shot_timer; +}; + +TreeExecutionServer::TreeExecutionServer(const rclcpp::Node::SharedPtr& node) + : p_(new Pimpl), node_(node) +{ + p_->param_listener = std::make_shared(node_); + p_->params = p_->param_listener->get_params(); + p_->global_blackboard = BT::Blackboard::create(); + + // 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( + 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)); + }); + + // 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 = + 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 node_->get_node_base_interface(); +} + +rclcpp::Node::SharedPtr TreeExecutionServer::node() +{ + return node_; +} + +rclcpp_action::GoalResponse +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()); + + if(!onGoalReceived(goal->target_tree, goal->payload)) + { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +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()) + { + RCLCPP_WARN(kLogger, "Rejecting request to cancel goal because the server is not " + "processing one."); + return rclcpp_action::CancelResponse::REJECT; + } + return rclcpp_action::CancelResponse::ACCEPT; +} + +void TreeExecutionServer::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 TreeExecutionServer::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)) + { + executeRegistration(); + } + + // 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 = 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); + 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.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) { + p_->tree.haltTree(); + action_result->node_status = ConvertNodeStatus(status); + // 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(goal_handle->is_canceling()) + { + 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->message = res.value(); + goal_handle->publish_feedback(feedback); + } + + const auto now = std::chrono::steady_clock::now(); + if(now < loop_deadline) + { + p_->tree.sleep(std::chrono::duration_cast( + loop_deadline - now)); + } + loop_deadline += period; + } + } + catch(const std::exception& ex) + { + 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 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); + + // return success or aborted for the action result + if(status == BT::NodeStatus::SUCCESS) + { + RCLCPP_INFO(kLogger, action_result->return_message.c_str()); + goal_handle->succeed(action_result); + } + else + { + RCLCPP_ERROR(kLogger, action_result->return_message.c_str()); + goal_handle->abort(action_result); + } +} + +const std::string& TreeExecutionServer::treeName() const +{ + return p_->tree_name; +} + +const std::string& TreeExecutionServer::goalPayload() const +{ + return p_->payload; +} + +const BT::Tree& TreeExecutionServer::tree() const +{ + return p_->tree; +} + +BT::Blackboard::Ptr TreeExecutionServer::globalBlackboard() +{ + return p_->global_blackboard; +} + +BT::BehaviorTreeFactory& TreeExecutionServer::factory() +{ + return p_->factory; +} + +} // namespace BT diff --git a/behaviortree_ros2/tree_execution_server.md b/behaviortree_ros2/tree_execution_server.md new file mode 100644 index 0000000..e7347cd --- /dev/null +++ b/behaviortree_ros2/tree_execution_server.md @@ -0,0 +1,75 @@ +# 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/t16_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 + +Documentation for the parameters used by the `TreeExecutionServer` can be found [here](bt_executor_parameters.md). + +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_interfaces/CMakeLists.txt b/btcpp_ros2_interfaces/CMakeLists.txt index 49080ee..23ca9aa 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 + "msg/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..5dee87d --- /dev/null +++ b/btcpp_ros2_interfaces/action/ExecuteTree.action @@ -0,0 +1,18 @@ +#### Request #### + +# Name of the tree to execute +string target_tree +# Optional, implementation-dependent, payload. +string payload +--- +#### 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 message diff --git a/btcpp_ros2_interfaces/msg/NodeStatus.msg b/btcpp_ros2_interfaces/msg/NodeStatus.msg new file mode 100644 index 0000000..ba36a72 --- /dev/null +++ b/btcpp_ros2_interfaces/msg/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 f2bb870..0ff08e5 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -7,18 +7,25 @@ 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) +find_package(std_srvs REQUIRED) -set(THIS_PACKAGE_DEPS +set(THIS_PACKAGE_DEPS behaviortree_ros2 std_msgs + std_srvs 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 +add_executable(sleep_client src/sleep_action.cpp src/sleep_client.cpp) ament_target_dependencies(sleep_client ${THIS_PACKAGE_DEPS}) @@ -38,12 +45,20 @@ 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}) + +###################################################### +# 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 @@ -54,9 +69,32 @@ install(TARGETS sleep_server sleep_plugin subscriber_test + sample_bt_executor + bool_client + bool_server 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 + ) + +###################################################### +# 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..27cdcc9 --- /dev/null +++ b/btcpp_ros2_samples/README.md @@ -0,0 +1,35 @@ +# 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). Documentation of the derived class methods can methods for each ROS interface type can be found [here](../behaviortree_ros2/ros_behavior_wrappers.md). + +# TreeExecutionServer Sample + +Documentation on the TreeExecutionServer used in this example can be found [here](../behaviortree_ros2/tree_execution_server.md). + +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 +``` + +> *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: 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 Plugin: libsleep_plugin.so +[bt_action_server]: Loaded BehaviorTree: door_closed.xml +[bt_action_server]: Loaded BehaviorTree: cross_door.xml +``` + +To call the Action Server from the command line: +``` bash +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 /behavior_server btcpp_ros2_interfaces/action/ExecuteTree "{target_tree: SleepActionSample}" +``` 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..397e356 --- /dev/null +++ b/btcpp_ros2_samples/config/sample_bt_executor.yaml @@ -0,0 +1,13 @@ +bt_action_server: + ros__parameters: + 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) + + plugins: + - behaviortree_cpp/bt_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..632fe7d --- /dev/null +++ b/btcpp_ros2_samples/launch/sample_bt_executor.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + 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 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/sample_bt_executor.cpp b/btcpp_ros2_samples/src/sample_bt_executor.cpp new file mode 100644 index 0000000..6b3e3a4 --- /dev/null +++ b/btcpp_ros2_samples/src/sample_bt_executor.cpp @@ -0,0 +1,77 @@ +// 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 + +// Example that shows how to customize TreeExecutionServer. +// +// 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 + 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 + { + logger_cout_ = std::make_shared(tree); + } + + 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: + std::shared_ptr logger_cout_; + rclcpp::Subscription::SharedPtr sub_; +}; + +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->node()); + exec.spin(); + exec.remove_node(action_server->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_; +}; diff --git a/btcpp_ros2_samples/src/sleep_action.cpp b/btcpp_ros2_samples/src/sleep_action.cpp index a838d74..9b8112e 100644 --- a/btcpp_ros2_samples/src/sleep_action.cpp +++ b/btcpp_ros2_samples/src/sleep_action.cpp @@ -1,32 +1,32 @@ #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(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(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. -// The class SleepAction will self register with name "Sleep". -CreateRosNodePlugin(SleepAction, "Sleep"); +// The class SleepAction will self register with name "SleepAction". +CreateRosNodePlugin(SleepAction, "SleepAction"); 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");