some refactoring of parameters and plugin loading

Author: facontidavide

behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp

@@ -56,22 +56,14 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory,
                        const std::string& directory_path);
 
 /**
- * @brief Function to load BehaviorTree plugins from a specific directory
+ * @brief Function to load BehaviorTree ROS plugins (or standard BT.CPP plugins) from a specific directory
  *
  * @param factory BehaviorTreeFactory to register the plugins into
  * @param directory_path Full path to the directory to search for BehaviorTree plugins
- */
-void LoadPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path);
-
-/**
- * @brief Function to load BehaviorTree ROS plugins from a specific directory
- *
- * @param factory BehaviorTreeFactory to register the plugins into
- * @param directory_path Full path to the directory to search for BehaviorTree plugins
- * @param node node pointer that is shared with the ROS based BehaviorTree plugins
+ * @param params parameters passed to the ROS plugins
  */
 void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path,
-                    rclcpp::Node::SharedPtr node);
+                    BT::RosNodeParams params);
 
 /**
  * @brief Function to register all Behaviors and BehaviorTrees from user specified packages
@@ -81,8 +73,7 @@ void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directo
  * @param factory BehaviorTreeFactory to register into
  * @param node node pointer that is shared with the ROS based Behavior plugins
  */
-void RegisterBehaviorTrees(action_server_bt::Params& params,
-                           BT::BehaviorTreeFactory& factory,
+void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
                            rclcpp::Node::SharedPtr node);
 
 }  // namespace BT

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

behaviortree_ros2/src/bt_executor_parameters.yaml

@@ -1,11 +1,11 @@
-action_server_bt:
+bt_server:
   action_name: {
     type: string,
     default_value: "bt_action_server",
     read_only: true,
     description: "The name the Action Server takes requests from",
   }
-  behavior_tick_frequency: {
+  tick_frequency: {
     type: int,
     default_value: 100,
     read_only: true,
@@ -31,10 +31,10 @@ action_server_bt:
       unique<>: null,
     }
   }
-  ros_plugins: {
-    type: string_array,
-    default_value: [],
-    description: "List of 'package_name/subfolder' containing BehaviorTree ROS plugins to load into the factory",
+  ros_plugins_timeout: {
+    type: int,
+    default_value: 1000,
+    description: "Timeout (ms) used in BT::RosNodeParams",
     validation: {
       unique<>: null,
     }

behaviortree_ros2/src/bt_utils.cpp

@@ -12,6 +12,7 @@
 // OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 
 #include "behaviortree_ros2/bt_utils.hpp"
+#include "behaviortree_ros2/plugins.hpp"
 
 namespace
 {
@@ -92,73 +93,69 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory,
   }
 }
 
-void LoadPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path)
-{
-  using std::filesystem::directory_iterator;
-  for(const auto& entry : directory_iterator(directory_path))
-  {
-    if(entry.path().extension() == ".so")
-    {
-      try
-      {
-        factory.registerFromPlugin(entry.path().string());
-        RCLCPP_INFO(kLogger, "Loaded Plugin: %s", entry.path().filename().c_str());
-      }
-      catch(const std::exception& e)
-      {
-        RCLCPP_ERROR(kLogger, "Failed to load Plugin: %s \n %s",
-                     entry.path().filename().c_str(), e.what());
-      }
-    }
-  }
-}
-
 void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path,
-                    rclcpp::Node::SharedPtr node)
+                    BT::RosNodeParams params)
 {
   using std::filesystem::directory_iterator;
-  BT::RosNodeParams params;
-  params.nh = node;
+
   for(const auto& entry : directory_iterator(directory_path))
   {
+    const auto filename = entry.path().filename();
     if(entry.path().extension() == ".so")
     {
       try
       {
-        RegisterRosNode(factory, entry.path().string(), params);
-        RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", entry.path().filename().c_str());
+        BT::SharedLibrary loader(entry.path().string());
+        if(loader.hasSymbol(BT::PLUGIN_SYMBOL))
+        {
+          typedef void (*Func)(BehaviorTreeFactory&);
+          auto func = (Func)loader.getSymbol(BT::PLUGIN_SYMBOL);
+          func(factory);
+        }
+        else if(loader.hasSymbol(BT::ROS_PLUGIN_SYMBOL))
+        {
+          typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&);
+          auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL);
+          func(factory, params);
+        }
+        else
+        {
+          RCLCPP_ERROR(kLogger, "Failed to load Plugin from file: %s.", filename.c_str());
+          continue;
+        }
+        RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", filename.c_str());
       }
       catch(const std::exception& e)
       {
-        RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s",
-                     entry.path().filename().c_str(), e.what());
+        RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", filename.c_str(),
+                     e.what());
       }
     }
   }
 }
 
-void RegisterBehaviorTrees(action_server_bt::Params& params,
-                           BT::BehaviorTreeFactory& factory, rclcpp::Node::SharedPtr node)
+void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
+                           rclcpp::Node::SharedPtr node)
 {
-  // clear the factory and load/reload it with the Behaviors and Trees specified by the user in their action_server_bt config yaml
+  // clear the factory and load/reload it with the Behaviors and Trees specified by the user in their [bt_action_server] config yaml
   factory.clearRegisteredBehaviorTrees();
 
+  BT::RosNodeParams ros_params;
+  ros_params.nh = node;
+  ros_params.server_timeout = std::chrono::milliseconds(params.ros_plugins_timeout);
+  ros_params.wait_for_server_timeout = ros_params.server_timeout;
+
   for(const auto& plugin : params.plugins)
   {
     const auto plugin_directory = GetDirectoryPath(plugin);
     // skip invalid plugins directories
     if(plugin_directory.empty())
+    {
       continue;
-    LoadPlugins(factory, plugin_directory);
-  }
-  for(const auto& plugin : params.ros_plugins)
-  {
-    const auto plugin_directory = GetDirectoryPath(plugin);
-    // skip invalid plugins directories
-    if(plugin_directory.empty())
-      continue;
-    LoadRosPlugins(factory, plugin_directory, node);
+    }
+    LoadRosPlugins(factory, plugin_directory, ros_params);
   }
+
   for(const auto& tree_dir : params.behavior_trees)
   {
     const auto tree_directory = GetDirectoryPath(tree_dir);

behaviortree_ros2/src/tree_execution_server.cpp

@@ -38,8 +38,8 @@ struct TreeExecutionServer::Pimpl
   // thread safe bool to keep track of cancel requests
   std::atomic<bool> cancel_requested{ false };
 
-  std::shared_ptr<action_server_bt::ParamListener> param_listener;
-  action_server_bt::Params params;
+  std::shared_ptr<bt_server::ParamListener> param_listener;
+  bt_server::Params params;
 
   BT::BehaviorTreeFactory factory;
   std::shared_ptr<BT::Groot2Publisher> groot_publisher;
@@ -62,7 +62,7 @@ struct TreeExecutionServer::Pimpl
 TreeExecutionServer::Pimpl::Pimpl(const rclcpp::NodeOptions& node_options)
   : node(std::make_unique<rclcpp::Node>("bt_action_server", node_options))
 {
-  param_listener = std::make_shared<action_server_bt::ParamListener>(node);
+  param_listener = std::make_shared<bt_server::ParamListener>(node);
   params = param_listener->get_params();
   global_blackboard = BT::Blackboard::create();
 }
@@ -168,8 +168,8 @@ void TreeExecutionServer::execute(
         std::make_shared<BT::Groot2Publisher>(*(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<int>(1000.0 / p_->params.behavior_tick_frequency));
+    const auto period =
+        std::chrono::milliseconds(static_cast<int>(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

behaviortree_ros2/tree_execution_server.md

@@ -0,0 +1,99 @@
+# TreeExecutionServer
+
+This base class is used to implement a Behavior Tree executor wrapped inside a `rclcpp_action::Server`.
+
+Users are expected to create a derived class to improve its functionalities, but often it can be used
+out of the box directly.
+
+Further, the terms "load" will be equivalent to "register into the `BT::BehaviorTreeFactory`".
+
+The `TreeExecutionServer`offers the following features:
+
+- Configurable using ROS parameters (see below).
+- Load Behavior Trees definitions (XML files) from a list of folders.
+- Load BT plugins from a list of folders. These plugins may depend or not on ROS.
+- Invoke the execution of a Tree from an external ROS Node, using `rclcpp_action::Client`.
+
+Furthermore, the user can customize it to:
+
+- Register custom BT Nodes directly (static linking).
+- Attach additional loggers. The **Groot2** publisher will be attached by default.
+- Use the "global blackboard", a new idiom/pattern explained in [this tutorial](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/examples/t19_global_blackboard.cpp).
+- Customize the feedback of the `rclcpp_action::Server`.
+
+
+## ROS Parameters
+
+Default Config
+
+```yaml
+bt_action_server:
+  ros__parameters:
+    action_name: bt_action_server
+    behavior_tick_frequency: 100.0
+    behavior_trees: '{}'
+    groot2_port: 1667.0
+    plugins: '{}'
+    ros_plugins: '{}'
+
+```
+
+## action_name
+
+The name the Action Server takes requests from
+
+* Type: `string`
+* Default Value: "bt_action_server"
+* Read only: True
+
+## behavior_tick_frequency
+
+Frequency in Hz to tick() the Behavior tree at
+
+* Type: `int`
+* Default Value: 100
+* Read only: True
+
+*Constraints:*
+ - parameter must be within bounds 1
+
+## groot2_port
+
+Server port value to publish Groot2 messages on
+
+* Type: `int`
+* Default Value: 1667
+* Read only: True
+
+*Constraints:*
+ - parameter must be within bounds 1
+
+## plugins
+
+List of 'package_name/subfolder' containing BehaviorTree plugins to load into the factory
+
+* Type: `string_array`
+* Default Value: {}
+
+*Constraints:*
+ - contains no duplicates
+
+## ros_plugins
+
+List of 'package_name/subfolder' containing BehaviorTree ROS plugins to load into the factory
+
+* Type: `string_array`
+* Default Value: {}
+
+*Constraints:*
+ - contains no duplicates
+
+## behavior_trees
+
+List of 'package_name/subfolder' containing SubTrees to load into the BehaviorTree factory
+
+* Type: `string_array`
+* Default Value: {}
+
+*Constraints:*
+ - contains no duplicates

btcpp_ros2_samples/config/sample_bt_executor.yaml

@@ -1,16 +1,13 @@
-action_server_bt:
+bt_action_server:
   ros__parameters:
-    action_name: "behavior_server" # Optional (defaults to `action_server_bt`)
-    behavior_tick_frequency: 100 # Optional (defaults to 100 Hz)
+    action_name: "behavior_server" # Optional (defaults to `bt_action_server`)
+    tick_frequency: 100 # Optional (defaults to 100 Hz)
     groot2_port: 1667 # Optional (defaults to 1667)
+    ros_plugins_timeout: 1000  # Optional (defaults 1000 ms)
 
-    # Below are a list plugins and BehaviorTrees to load
-    # (you are not required to have all 3 types)
-    # These are dynamic parameters and can be changed at runtime via rosparam
-    # see `action_server_bt/parameters.md` for documentation.
     plugins:
       - behaviortree_cpp/bt_plugins
-    ros_plugins:
       - btcpp_ros2_samples/bt_plugins
+
     behavior_trees:
       - btcpp_ros2_samples/behavior_trees