fix issue #61: postpone registerNodesIntoFactory

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

@@ -123,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,9 +108,18 @@ 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
@@ -151,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