Merge pull request #60 from BehaviorTree/bt_server

Action Server to execute trees

Author: facontidavide

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.

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
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>)
+
+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()

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

behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp

@@ -316,6 +316,11 @@ inline bool RosActionNode<T>::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();

behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp

@@ -282,6 +282,11 @@ inline bool RosServiceNode<T>::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();

behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp

@@ -249,15 +249,19 @@ inline bool RosTopicSubNode<T>::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<SubscriberInstance>(shared_node, topic_name);
+    sub_instance_ = std::make_shared<SubscriberInstance>(node, topic_name);
     registry.insert({ subscriber_key_, sub_instance_ });
 
     RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(),

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 <functional>
+#include <memory>
+#include <thread>
+
+// 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 <ament_index_cpp/get_package_share_directory.hpp>
+
+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

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);
 }