diff --git a/README.md b/README.md index ce5b332..be77ad1 100644 --- a/README.md +++ b/README.md @@ -5,6 +5,7 @@ This repository contains useful wrappers to use ROS2 and BehaviorTree.CPP togeth In particular, it provides a standard way to implement: +- Behavior Tree Executor with ROS Action interface. - Action clients. - Service Clients. - Topic Subscribers. @@ -15,6 +16,12 @@ Our main goals are: - to minimize the amount of boilerplate. - to make asynchronous Actions non-blocking. +# Documentation + +- [ROS Behavior Wrappers](behaviortree_ros2/ros_behavior_wrappers.md) +- [TreeExecutionServer](behaviortree_ros2/tree_execution_server.md) +- [Sample Behaviors](btcpp_ros2_samples/README.md) + Note that this library is compatible **only** with: - **BT.CPP** 4.1 or newer. diff --git a/behaviortree_ros2/CMakeLists.txt b/behaviortree_ros2/CMakeLists.txt index 01378ff..edf1440 100644 --- a/behaviortree_ros2/CMakeLists.txt +++ b/behaviortree_ros2/CMakeLists.txt @@ -3,32 +3,57 @@ project(behaviortree_ros2) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(THIS_PACKAGE_DEPS rclcpp rclcpp_action ament_index_cpp - behaviortree_cpp) + behaviortree_cpp + btcpp_ros2_interfaces + generate_parameter_library + ) find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED ) -find_package(rclcpp_action REQUIRED ) -find_package(behaviortree_cpp REQUIRED ) -find_package(ament_index_cpp REQUIRED) - -# This is compiled only to check if there are errors in the header file -# library will not be exported -include_directories(include) -add_library(${PROJECT_NAME} src/bt_ros2.cpp) + +foreach(PACKAGE ${THIS_PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED ) +endforeach() + + +generate_parameter_library( + bt_executor_parameters + src/bt_executor_parameters.yaml) + +add_library(${PROJECT_NAME} + src/bt_ros2.cpp + src/bt_utils.cpp + src/tree_execution_server.cpp ) + ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS}) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) + +target_link_libraries(${PROJECT_NAME} bt_executor_parameters) + + ###################################################### # INSTALL install(DIRECTORY include/ DESTINATION include/) -ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_DEPS}) +install( + TARGETS ${PROJECT_NAME} bt_executor_parameters + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + ament_package() diff --git a/behaviortree_ros2/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 be892de..4ea55c5 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -316,6 +316,11 @@ inline bool RosActionNode::createClient(const std::string& action_name) std::unique_lock lk(getMutex()); auto node = node_.lock(); + if(!node) + { + throw RuntimeError("The ROS node went out of scope. RosNodeParams doesn't take the " + "ownership of the node."); + } action_client_key_ = std::string(node->get_fully_qualified_name()) + "/" + action_name; auto& registry = getRegistry(); diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 93e14ea..f3a6203 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -282,6 +282,11 @@ inline bool RosServiceNode::createClient(const std::string& service_name) std::unique_lock lk(getMutex()); auto node = node_.lock(); + if(!node) + { + throw RuntimeError("The ROS node went out of scope. RosNodeParams doesn't take the " + "ownership of the node."); + } auto client_key = std::string(node->get_fully_qualified_name()) + "/" + service_name; auto& registry = getRegistry(); diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index a8a34fc..211f41c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -249,15 +249,19 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) // find SubscriberInstance in the registry std::unique_lock lk(registryMutex()); - auto shared_node = node_.lock(); - subscriber_key_ = - std::string(shared_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() || it->second.expired()) { - sub_instance_ = std::make_shared(shared_node, topic_name); + sub_instance_ = std::make_shared(node, topic_name); registry.insert({ subscriber_key_, sub_instance_ }); RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(), 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..d846996 --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp @@ -0,0 +1,79 @@ +// 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 BehaviorTree ROS plugins (or standard BT.CPP plugins) from a specific directory + * + * @param factory BehaviorTreeFactory to register the plugins into + * @param directory_path Full path to the directory to search for BehaviorTree plugins + * @param params parameters passed to the ROS plugins + */ +void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, + BT::RosNodeParams params); + +/** + * @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 4d7ed4d..116fd2c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/plugins.hpp @@ -20,6 +20,11 @@ #include "behaviortree_cpp/utils/shared_library.h" #include "behaviortree_ros2/ros_node_params.hpp" +namespace BT +{ +constexpr const char* ROS_PLUGIN_SYMBOL = "BT_RegisterRosNodeFromPlugin"; +} + // Use this macro to generate a plugin for: // // - BT::RosActionNode @@ -54,6 +59,6 @@ inline void RegisterRosNode(BT::BehaviorTreeFactory& factory, { BT::SharedLibrary loader(filepath.generic_string()); typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&); - auto func = (Func)loader.getSymbol("BT_RegisterRosNodeFromPlugin"); + auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL); func(factory, params); } diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp new file mode 100644 index 0000000..9a8839a --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -0,0 +1,155 @@ +// 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 for TreeExecutionServer. + * @details This initializes a ParameterListener to read configurable options from the user and + * starts an Action Server that takes requests to execute BehaviorTrees. + * + * @param options rclcpp::NodeOptions to pass to node_ when initializing it. + * after the tree is created, while its running and after it finishes. + */ + explicit TreeExecutionServer(const rclcpp::NodeOptions& options); + + virtual ~TreeExecutionServer(); + + /** + * @brief Gets the NodeBaseInterface of node_. + * @details This function exists to allow running TreeExecutionServer as a component in a composable node container. + * + * @return A shared_ptr to the NodeBaseInterface of node_. + */ + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr nodeBaseInterface(); + + // name of the tree being executed + const std::string& currentTreeName() const; + + // tree being executed, nullptr if it doesn't exist yet. + BT::Tree* currentTree(); + + // pointer to the global blackboard + BT::Blackboard::Ptr globalBlackboard(); + +protected: + /** + * @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 + */ + virtual void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) + {} + + /** + * @brief onLoopFeedback is a callback invoked at each loop, after tree.tickOnce(). + * If it returns a valid string, it will be sent as feedback to the Action Client. + * + * If you don't want to return any feedback, return std::nullopt. + */ + virtual std::optional onLoopFeedback() + { + return std::nullopt; + } + +private: + struct Pimpl; + std::unique_ptr p_; + + /** + * @brief handle the goal requested: accept or reject. This implementation always accepts. + * @param uuid Goal ID + * @param goal A shared pointer to the specific goal + * @return GoalResponse response of the goal processed + */ + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + + /** + * @brief Accepts cancellation requests of action server. + * @param goal A server goal handle to cancel + * @return CancelResponse response of the goal cancelled + */ + rclcpp_action::CancelResponse + handle_cancel(const std::shared_ptr goal_handle); + + /** + * @brief Handles accepted goals and starts a thread to process them + * @param goal_handle Server goal handle to process feedback and set the response when finished + */ + void handle_accepted(const std::shared_ptr goal_handle); + + /** + * @brief Background processes to execute the BehaviorTree and handle stop requests + * @param goal_handle Server goal handle to process feedback and set the response when finished + */ + void execute(const std::shared_ptr goal_handle); +}; + +} // namespace 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..ce5e5cc --- /dev/null +++ b/behaviortree_ros2/src/bt_utils.cpp @@ -0,0 +1,169 @@ +// 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; + case BT::NodeStatus::SUCCESS: + action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::SUCCESS; + case BT::NodeStatus::FAILURE: + action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::FAILURE; + case BT::NodeStatus::IDLE: + action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::IDLE; + case BT::NodeStatus::SKIPPED: + action_status.status = btcpp_ros2_interfaces::msg::NodeStatus::SKIPPED; + } + + return action_status; +} + +std::string 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 LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path, + BT::RosNodeParams params) +{ + using std::filesystem::directory_iterator; + + for(const auto& entry : directory_iterator(directory_path)) + { + const auto filename = entry.path().filename(); + if(entry.path().extension() == ".so") + { + try + { + BT::SharedLibrary loader(entry.path().string()); + if(loader.hasSymbol(BT::PLUGIN_SYMBOL)) + { + typedef void (*Func)(BehaviorTreeFactory&); + auto func = (Func)loader.getSymbol(BT::PLUGIN_SYMBOL); + func(factory); + } + else if(loader.hasSymbol(BT::ROS_PLUGIN_SYMBOL)) + { + typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&); + auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL); + func(factory, params); + } + else + { + RCLCPP_ERROR(kLogger, "Failed to load Plugin from file: %s.", filename.c_str()); + continue; + } + RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", filename.c_str()); + } + catch(const std::exception& e) + { + RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", filename.c_str(), + e.what()); + } + } + } +} + +void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory, + rclcpp::Node::SharedPtr node) +{ + // clear the factory and load/reload it with the Behaviors and Trees specified by the user in their [bt_action_server] config yaml + factory.clearRegisteredBehaviorTrees(); + + BT::RosNodeParams ros_params; + ros_params.nh = node; + ros_params.server_timeout = std::chrono::milliseconds(params.ros_plugins_timeout); + ros_params.wait_for_server_timeout = ros_params.server_timeout; + + for(const auto& plugin : params.plugins) + { + const auto plugin_directory = GetDirectoryPath(plugin); + // skip invalid plugins directories + if(plugin_directory.empty()) + { + continue; + } + LoadRosPlugins(factory, plugin_directory, ros_params); + } + + 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..26b6f11 --- /dev/null +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -0,0 +1,265 @@ +// Copyright 2024 Marq Rasmussen +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to +// permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright +// notice and this permission notice shall be included in all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +#include + +#include "behaviortree_ros2/tree_execution_server.hpp" +#include "behaviortree_ros2/bt_utils.hpp" + +#include "behaviortree_cpp/loggers/groot2_publisher.h" + +// generated file +#include "bt_executor_parameters.hpp" +namespace +{ +static const auto kLogger = rclcpp::get_logger("bt_action_server"); +} + +namespace BT +{ + +struct TreeExecutionServer::Pimpl +{ + Pimpl(const rclcpp::NodeOptions& node_options); + + rclcpp::Node::SharedPtr node; + rclcpp_action::Server::SharedPtr action_server; + std::thread action_thread; + // thread safe bool to keep track of cancel requests + std::atomic cancel_requested{ false }; + + std::shared_ptr param_listener; + bt_server::Params params; + + BT::BehaviorTreeFactory factory; + std::shared_ptr groot_publisher; + + std::string current_tree_name; + std::shared_ptr tree; + BT::Blackboard::Ptr global_blackboard; + + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse + handle_cancel(const std::shared_ptr goal_handle); + + void handle_accepted(const std::shared_ptr goal_handle); + + void execute(const std::shared_ptr goal_handle); +}; + +TreeExecutionServer::Pimpl::Pimpl(const rclcpp::NodeOptions& node_options) + : node(std::make_unique("bt_action_server", node_options)) +{ + param_listener = std::make_shared(node); + params = param_listener->get_params(); + global_blackboard = BT::Blackboard::create(); +} + +TreeExecutionServer::~TreeExecutionServer() +{} + +TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options) + : p_(new Pimpl(options)) +{ + // create the action server + const auto action_name = p_->params.action_name; + RCLCPP_INFO(kLogger, "Starting Action Server: %s", action_name.c_str()); + p_->action_server = rclcpp_action::create_server( + p_->node, action_name, + [this](const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) { + return handle_goal(uuid, std::move(goal)); + }, + [this](const std::shared_ptr goal_handle) { + return handle_cancel(std::move(goal_handle)); + }, + [this](const std::shared_ptr goal_handle) { + handle_accepted(std::move(goal_handle)); + }); + + // register the users Plugins and BehaviorTree.xml files into the factory + RegisterBehaviorTrees(p_->params, p_->factory, p_->node); + registerNodesIntoFactory(p_->factory); +} + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr +TreeExecutionServer::nodeBaseInterface() +{ + return p_->node->get_node_base_interface(); +} + +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()); + p_->cancel_requested = false; + 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; + } + p_->cancel_requested = true; + 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)) + { + p_->params = p_->param_listener->get_params(); + RegisterBehaviorTrees(p_->params, p_->factory, p_->node); + registerNodesIntoFactory(p_->factory); + } + + // Loop until something happens with ROS or the node completes + try + { + // This blackboard will be owned by "MainTree". It parent is p_->global_blackboard + auto root_blackboard = BT::Blackboard::create(p_->global_blackboard); + + p_->tree = std::make_shared(); + *(p_->tree) = p_->factory.createTree(goal->target_tree, root_blackboard); + p_->current_tree_name = goal->target_tree; + + // call user defined function after the tree has been created + onTreeCreated(*p_->tree); + p_->groot_publisher.reset(); + p_->groot_publisher = + std::make_shared(*(p_->tree), p_->params.groot2_port); + + // Loop until the tree is done or a cancel is requested + const auto period = + std::chrono::milliseconds(static_cast(1000.0 / p_->params.tick_frequency)); + auto loop_deadline = std::chrono::steady_clock::now() + period; + + // operations to be done if the tree execution is aborted, either by + // cancel_requested_ or by onLoopAfterTick() + auto stop_action = [this, &action_result](BT::NodeStatus status, + const std::string& message) { + action_result->node_status = ConvertNodeStatus(status); + action_result->error_message = message; + RCLCPP_WARN(kLogger, action_result->error_message.c_str()); + p_->tree->haltTree(); + onTreeExecutionCompleted(status, true); + }; + + while(rclcpp::ok() && status == BT::NodeStatus::RUNNING) + { + if(p_->cancel_requested) + { + stop_action(status, "Action Server canceling and halting Behavior Tree"); + goal_handle->canceled(action_result); + return; + } + + // tick the tree once and publish the action feedback + status = p_->tree->tickExactlyOnce(); + + if(const auto res = onLoopAfterTick(status); res.has_value()) + { + stop_action(res.value(), "Action Server aborted by onLoopAfterTick()"); + goal_handle->abort(action_result); + return; + } + + if(const auto res = onLoopFeedback(); res.has_value()) + { + auto feedback = std::make_shared(); + feedback->msg = res.value(); + goal_handle->publish_feedback(feedback); + } + + const auto now = std::chrono::steady_clock::now(); + if(now < loop_deadline) + { + p_->tree->sleep(loop_deadline - now); + } + loop_deadline += period; + } + } + catch(const std::exception& ex) + { + action_result->error_message = std::string("Behavior Tree exception:") + ex.what(); + RCLCPP_ERROR(kLogger, action_result->error_message.c_str()); + goal_handle->abort(action_result); + return; + } + + // call user defined execution complete function + onTreeExecutionCompleted(status, false); + + // set the node_status result to the action + action_result->node_status = ConvertNodeStatus(status); + + // return success or aborted for the action result + if(status == BT::NodeStatus::SUCCESS) + { + RCLCPP_INFO(kLogger, "BT finished with status: %s", BT::toStr(status).c_str()); + goal_handle->succeed(action_result); + } + else + { + action_result->error_message = std::string("Behavior Tree failed during execution " + "with status: ") + + BT::toStr(status); + RCLCPP_ERROR(kLogger, action_result->error_message.c_str()); + goal_handle->abort(action_result); + } +} + +const std::string& TreeExecutionServer::currentTreeName() const +{ + return p_->current_tree_name; +} + +BT::Tree* TreeExecutionServer::currentTree() +{ + return p_->tree ? p_->tree.get() : nullptr; +} + +BT::Blackboard::Ptr TreeExecutionServer::globalBlackboard() +{ + return p_->global_blackboard; +} + +} // namespace BT diff --git a/behaviortree_ros2/tree_execution_server.md b/behaviortree_ros2/tree_execution_server.md new file mode 100644 index 0000000..34edf44 --- /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/t19_global_blackboard.cpp). +- Customize the feedback of the `rclcpp_action::Server`. + +## Customization points + +These are the virtual method of `TreeExecutionServer` that can be overridden by the user. + +### void onTreeCreated(BT::Tree& tree) + +Callback invoked when a tree is created; this happens after `rclcpp_action::Server` receive a command from a client. + +It can be used, for instance, to initialize a logger or the global blackboard. + +### void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) + +Called at the beginning, after all the plugins have been loaded. + +It can be used to register programmatically more BT.CPP Nodes. + +### std::optional onLoopAfterTick(BT::NodeStatus status) + +Used to do something after the tree was ticked, in its execution loop. + +If this method returns something other than `std::nullopt`, the tree +execution is interrupted an the specified `BT::NodeStatus` is returned to the `rclcpp_action::Client`. + +### void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) + +Callback when the tree execution reaches its end. + +This happens if: + +1. Ticking the tree returns SUCCESS/FAILURE +2. The `rclcpp_action::Client` cancels the action. +3. Callback `onLoopAfterTick`cancels the execution. + +Argument `was_cancelled`is true in the 1st case, false otherwise. + +### std::optional onLoopFeedback() + +This callback is invoked after `tree.tickOnce` and `onLoopAfterTick`. + +If it returns something other than `std::nullopt`, the provided string will be +sent as feedback to the `rclcpp_action::Client`. + + + +## ROS Parameters + +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..72612f3 100644 --- a/btcpp_ros2_interfaces/CMakeLists.txt +++ b/btcpp_ros2_interfaces/CMakeLists.txt @@ -8,6 +8,8 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(btcpp_ros2_interfaces + "msgs/NodeStatus.msg" + "action/ExecuteTree.action" "action/Sleep.action") ament_export_dependencies(rosidl_default_runtime) diff --git a/btcpp_ros2_interfaces/action/ExecuteTree.action b/btcpp_ros2_interfaces/action/ExecuteTree.action new file mode 100644 index 0000000..d12d6f6 --- /dev/null +++ b/btcpp_ros2_interfaces/action/ExecuteTree.action @@ -0,0 +1,9 @@ +# Request +string target_tree +--- +# Result +string error_message +NodeStatus node_status +--- +# Feedback. This can be customized by the user +string msg diff --git a/btcpp_ros2_interfaces/msgs/NodeStatus.msg b/btcpp_ros2_interfaces/msgs/NodeStatus.msg new file mode 100644 index 0000000..ba36a72 --- /dev/null +++ b/btcpp_ros2_interfaces/msgs/NodeStatus.msg @@ -0,0 +1,8 @@ +# NodeStatus Enums +uint8 IDLE = 0 +uint8 RUNNING = 1 +uint8 SUCCESS = 2 +uint8 FAILURE = 3 +uint8 SKIPPED = 4 + +uint8 status diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index 6d4987b..785d674 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -7,8 +7,8 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(ament_cmake REQUIRED) find_package(behaviortree_ros2 REQUIRED) -find_package(std_msgs REQUIRED) find_package(btcpp_ros2_interfaces REQUIRED) +find_package(std_msgs REQUIRED) set(THIS_PACKAGE_DEPS behaviortree_ros2 @@ -16,7 +16,12 @@ set(THIS_PACKAGE_DEPS btcpp_ros2_interfaces ) ###################################################### -# Build a client that call the sleep action (STATIC version) +# Simple example showing how to use and customize the BtExecutionServer +add_executable(sample_bt_executor src/sample_bt_executor.cpp) +ament_target_dependencies(sample_bt_executor ${THIS_PACKAGE_DEPS}) + +###################################################### +# Build an Action Client that calls the sleep action (STATIC version) add_executable(sleep_client src/sleep_action.cpp @@ -54,6 +59,7 @@ install(TARGETS sleep_server sleep_plugin subscriber_test + sample_bt_executor DESTINATION lib/${PROJECT_NAME} ) @@ -67,6 +73,17 @@ install(TARGETS RUNTIME DESTINATION share/${PROJECT_NAME}/bt_plugins ) +###################################################### +# INSTALL Behavior.xml's, ROS config and launch files + +install(DIRECTORY + behavior_trees + config + launch + DESTINATION share/${PROJECT_NAME}/ + ) + + ament_export_dependencies(behaviortree_ros2 btcpp_ros2_interfaces) ament_package() 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/src/sample_bt_executor.cpp b/btcpp_ros2_samples/src/sample_bt_executor.cpp new file mode 100644 index 0000000..d2d160b --- /dev/null +++ b/btcpp_ros2_samples/src/sample_bt_executor.cpp @@ -0,0 +1,57 @@ +// Copyright 2024 Marq Rasmussen +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to +// permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright +// notice and this permission notice shall be included in all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +#include +#include + +// Example that shows how to customize TreeExecutionServer. +// Here, we simply add an extra logger +class MyActionServer : public BT::TreeExecutionServer +{ +public: + MyActionServer(const rclcpp::NodeOptions& options) : TreeExecutionServer(options) + {} + + void onTreeCreated(BT::Tree& tree) override + { + logger_cout_ = std::make_shared(tree); + } + + void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override + { + // NOT really needed, even if logger_cout_ may contain a dangling pointer of the tree + // at this point + logger_cout_.reset(); + } + +private: + std::shared_ptr logger_cout_; +}; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + auto action_server = std::make_shared(options); + + // TODO: This workaround is for a bug in MultiThreadedExecutor where it can deadlock when spinning without a timeout. + // Deadlock is caused when Publishers or Subscribers are dynamically removed as the node is spinning. + rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 0, false, + std::chrono::milliseconds(250)); + exec.add_node(action_server->nodeBaseInterface()); + exec.spin(); + exec.remove_node(action_server->nodeBaseInterface()); + + rclcpp::shutdown(); +}