Merge pull request #65 from BehaviorTree/issues_61_63

Issues 61 and 63

Author: facontidavide

behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp

@@ -56,14 +56,23 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory,
                        const std::string& directory_path);
 
 /**
- * @brief Function to load BehaviorTree ROS plugins (or standard BT.CPP plugins) from a specific directory
+ * @brief Function to load a BehaviorTree ROS plugin (or standard BT.CPP plugins)
  *
+ * @param factory BehaviorTreeFactory to register the plugin into
+ * @param file_path Full path to the directory to search for BehaviorTree plugin
+ * @param params parameters passed to the ROS plugin
+ */
+void LoadPlugin(BT::BehaviorTreeFactory& factory, const std::filesystem::path& file_path,
+                BT::RosNodeParams params);
+
+/** @brief Function to load all plugins from the specified package
+ *
+ * @param params ROS parameters that contain the package name to load plugins from
  * @param factory BehaviorTreeFactory to register the plugins into
- * @param directory_path Full path to the directory to search for BehaviorTree plugins
- * @param params parameters passed to the ROS plugins
+ * @param node node pointer that is shared with the ROS based Behavior plugins
  */
-void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path,
-                    BT::RosNodeParams params);
+void RegisterPlugins(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
+                     rclcpp::Node::SharedPtr node);
 
 /**
  * @brief Function to register all Behaviors and BehaviorTrees from user specified packages

behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp

@@ -51,20 +51,25 @@ class TreeExecutionServer
   /**
    * @brief Gets the NodeBaseInterface of node_.
    * @details This function exists to allow running TreeExecutionServer as a component in a composable node container.
-   *
-   * @return A shared_ptr to the NodeBaseInterface of node_.
+   * The name of this method shall not change to work properly with the component composer.
    */
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr nodeBaseInterface();
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface();
+
+  /// @brief Gets the rclcpp::Node pointer
+  rclcpp::Node::SharedPtr node();
 
-  // name of the tree being executed
+  /// @brief Name of the tree being executed
   const std::string& currentTreeName() const;
 
-  // tree being executed, nullptr if it doesn't exist yet.
+  /// @brief Tree being executed, nullptr if it doesn't exist, yet.
   BT::Tree* currentTree();
 
-  // pointer to the global blackboard
+  /// @brief Pointer to the global blackboard
   BT::Blackboard::Ptr globalBlackboard();
 
+  /// @brief Pointer to the global blackboard
+  BT::BehaviorTreeFactory& factory();
+
 protected:
   /**
    * @brief Callback invoked after the tree is created.
@@ -118,6 +123,18 @@ class TreeExecutionServer
     return std::nullopt;
   }
 
+protected:
+  /**
+   * @brief Method to register the trees and BT custom nodes.
+   * It will call registerNodesIntoFactory().
+   *
+   * This callback will invoked asynchronously when this rclcpp Node is attached
+   * to a rclcpp Executor.
+   * Alternatively, it can be called synchronously in the constructor of a
+   * __derived__ class of TreeExecutionServer.
+   */
+  void executeRegistration();
+
 private:
   struct Pimpl;
   std::unique_ptr<Pimpl> p_;

behaviortree_ros2/src/bt_utils.cpp

@@ -93,53 +93,42 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory,
   }
 }
 
-void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path,
-                    BT::RosNodeParams params)
+void LoadPlugin(BT::BehaviorTreeFactory& factory, const std::filesystem::path& file_path,
+                BT::RosNodeParams params)
 {
-  using std::filesystem::directory_iterator;
-
-  for(const auto& entry : directory_iterator(directory_path))
+  const auto filename = file_path.filename();
+  try
   {
-    const auto filename = entry.path().filename();
-    if(entry.path().extension() == ".so")
+    BT::SharedLibrary loader(file_path.string());
+    if(loader.hasSymbol(BT::PLUGIN_SYMBOL))
     {
-      try
-      {
-        BT::SharedLibrary loader(entry.path().string());
-        if(loader.hasSymbol(BT::PLUGIN_SYMBOL))
-        {
-          typedef void (*Func)(BehaviorTreeFactory&);
-          auto func = (Func)loader.getSymbol(BT::PLUGIN_SYMBOL);
-          func(factory);
-        }
-        else if(loader.hasSymbol(BT::ROS_PLUGIN_SYMBOL))
-        {
-          typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&);
-          auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL);
-          func(factory, params);
-        }
-        else
-        {
-          RCLCPP_ERROR(kLogger, "Failed to load Plugin from file: %s.", filename.c_str());
-          continue;
-        }
-        RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", filename.c_str());
-      }
-      catch(const std::exception& e)
-      {
-        RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", filename.c_str(),
-                     e.what());
-      }
+      typedef void (*Func)(BehaviorTreeFactory&);
+      auto func = (Func)loader.getSymbol(BT::PLUGIN_SYMBOL);
+      func(factory);
+    }
+    else if(loader.hasSymbol(BT::ROS_PLUGIN_SYMBOL))
+    {
+      typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&);
+      auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL);
+      func(factory, params);
+    }
+    else
+    {
+      RCLCPP_ERROR(kLogger, "Failed to load Plugin from file: %s.", filename.c_str());
+      return;
     }
+    RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", filename.c_str());
+  }
+  catch(const std::exception& ex)
+  {
+    RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", filename.c_str(),
+                 ex.what());
   }
 }
 
-void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
-                           rclcpp::Node::SharedPtr node)
+void RegisterPlugins(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
+                     rclcpp::Node::SharedPtr node)
 {
-  // clear the factory and load/reload it with the Behaviors and Trees specified by the user in their [bt_action_server] config yaml
-  factory.clearRegisteredBehaviorTrees();
-
   BT::RosNodeParams ros_params;
   ros_params.nh = node;
   ros_params.server_timeout = std::chrono::milliseconds(params.ros_plugins_timeout);
@@ -153,9 +142,21 @@ void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& f
     {
       continue;
     }
-    LoadRosPlugins(factory, plugin_directory, ros_params);
+    using std::filesystem::directory_iterator;
+
+    for(const auto& entry : directory_iterator(plugin_directory))
+    {
+      if(entry.path().extension() == ".so")
+      {
+        LoadPlugin(factory, entry.path(), ros_params);
+      }
+    }
   }
+}
 
+void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
+                           rclcpp::Node::SharedPtr node)
+{
   for(const auto& tree_dir : params.behavior_trees)
   {
     const auto tree_directory = GetDirectoryPath(tree_dir);

behaviortree_ros2/src/tree_execution_server.cpp

@@ -47,6 +47,9 @@ struct TreeExecutionServer::Pimpl
   std::string current_tree_name;
   std::shared_ptr<BT::Tree> tree;
   BT::Blackboard::Ptr global_blackboard;
+  bool factory_initialized_ = false;
+
+  rclcpp::WallTimer<rclcpp::VoidCallbackType>::SharedPtr single_shot_timer;
 
   rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid,
                                           std::shared_ptr<const ExecuteTree::Goal> goal);
@@ -70,6 +73,22 @@ TreeExecutionServer::Pimpl::Pimpl(const rclcpp::NodeOptions& node_options)
 TreeExecutionServer::~TreeExecutionServer()
 {}
 
+void TreeExecutionServer::executeRegistration()
+{
+  // Before executing check if we have new Behaviors or Subtrees to reload
+  p_->factory.clearRegisteredBehaviorTrees();
+
+  p_->params = p_->param_listener->get_params();
+  // user defined method
+  registerNodesIntoFactory(p_->factory);
+  // load plugins from multiple directories
+  RegisterPlugins(p_->params, p_->factory, p_->node);
+  // load trees (XML) from multiple directories
+  RegisterBehaviorTrees(p_->params, p_->factory, p_->node);
+
+  p_->factory_initialized_ = true;
+}
+
 TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options)
   : p_(new Pimpl(options))
 {
@@ -89,17 +108,31 @@ TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options)
         handle_accepted(std::move(goal_handle));
       });
 
-  // register the users Plugins and BehaviorTree.xml files into the factory
-  RegisterBehaviorTrees(p_->params, p_->factory, p_->node);
-  registerNodesIntoFactory(p_->factory);
+  // we use a wall timer to run asynchronously executeRegistration();
+  rclcpp::VoidCallbackType callback = [this]() {
+    if(!p_->factory_initialized_)
+    {
+      executeRegistration();
+    }
+    // we must cancel the timer after the first execution
+    p_->single_shot_timer->cancel();
+  };
+
+  p_->single_shot_timer =
+      p_->node->create_wall_timer(std::chrono::milliseconds(1), callback);
 }
 
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
-TreeExecutionServer::nodeBaseInterface()
+TreeExecutionServer::get_node_base_interface()
 {
   return p_->node->get_node_base_interface();
 }
 
+rclcpp::Node::SharedPtr TreeExecutionServer::node()
+{
+  return p_->node;
+}
+
 rclcpp_action::GoalResponse
 TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */,
                                  std::shared_ptr<const ExecuteTree::Goal> goal)
@@ -146,9 +179,7 @@ void TreeExecutionServer::execute(
   // Before executing check if we have new Behaviors or Subtrees to reload
   if(p_->param_listener->is_old(p_->params))
   {
-    p_->params = p_->param_listener->get_params();
-    RegisterBehaviorTrees(p_->params, p_->factory, p_->node);
-    registerNodesIntoFactory(p_->factory);
+    executeRegistration();
   }
 
   // Loop until something happens with ROS or the node completes
@@ -262,4 +293,9 @@ BT::Blackboard::Ptr TreeExecutionServer::globalBlackboard()
   return p_->global_blackboard;
 }
 
+BT::BehaviorTreeFactory& TreeExecutionServer::factory()
+{
+  return p_->factory;
+}
+
 }  // namespace BT

btcpp_ros2_samples/src/sample_bt_executor.cpp

@@ -28,8 +28,7 @@ class MyActionServer : public BT::TreeExecutionServer
   MyActionServer(const rclcpp::NodeOptions& options) : TreeExecutionServer(options)
   {
     // here we assume that the battery voltage is published as a std_msgs::msg::Float32
-    auto node = std::dynamic_pointer_cast<rclcpp::Node>(nodeBaseInterface());
-    sub_ = node->create_subscription<std_msgs::msg::Float32>(
+    sub_ = node()->create_subscription<std_msgs::msg::Float32>(
         "battery_level", 10, [this](const std_msgs::msg::Float32::SharedPtr msg) {
           // Update the global blackboard
           globalBlackboard()->set("battery_level", msg->data);
@@ -68,9 +67,9 @@ int main(int argc, char* argv[])
   // Deadlock is caused when Publishers or Subscribers are dynamically removed as the node is spinning.
   rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 0, false,
                                                 std::chrono::milliseconds(250));
-  exec.add_node(action_server->nodeBaseInterface());
+  exec.add_node(action_server->node());
   exec.spin();
-  exec.remove_node(action_server->nodeBaseInterface());
+  exec.remove_node(action_server->node());
 
   rclcpp::shutdown();
 }