Fix/register node into factory

behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp

@@ -66,14 +66,21 @@ void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directo
                     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
+ * @brief Function to load BehaviorTree plugins from a specific directory.
+ * 
+ * @param params ROS parameters that contain lists of packages to load plugins, ros_plugins 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);
+void LoadPlugins(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
+                 rclcpp::Node::SharedPtr node);
+
+/**
+ * @brief Function to register all BehaviorTrees from user specified packages
+ *
+ * @param params ROS parameters that contain lists of packages to load BehaviorTrees from
+ * @param factory BehaviorTreeFactory to register into
+ */
+void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory);
 
 }  // namespace BT

behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp

@@ -121,6 +121,7 @@ class TreeExecutionServer
 private:
   struct Pimpl;
   std::unique_ptr<Pimpl> p_;
+  bool bt_loaded_ = false;
 
   /**
    * @brief handle the goal requested: accept or reject. This implementation always accepts.

behaviortree_ros2/src/bt_utils.cpp

@@ -134,17 +134,13 @@ void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directo
   }
 }
 
-void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
-                           rclcpp::Node::SharedPtr node)
+void LoadPlugins(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);
@@ -155,7 +151,10 @@ void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& f
     }
     LoadRosPlugins(factory, plugin_directory, ros_params);
   }
+}
 
+void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory)
+{
   for(const auto& tree_dir : params.behavior_trees)
   {
     const auto tree_directory = GetDirectoryPath(tree_dir);

behaviortree_ros2/src/tree_execution_server.cpp

@@ -88,10 +88,6 @@ TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options)
       [this](const std::shared_ptr<GoalHandleExecuteTree> 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
@@ -144,11 +140,18 @@ void TreeExecutionServer::execute(
   auto action_result = std::make_shared<ExecuteTree::Result>();
 
   // Before executing check if we have new Behaviors or Subtrees to reload
-  if(p_->param_listener->is_old(p_->params))
+  if(p_->param_listener->is_old(p_->params) || !bt_loaded_)
   {
+    p_->factory.clearRegisteredBehaviorTrees();
     p_->params = p_->param_listener->get_params();
-    RegisterBehaviorTrees(p_->params, p_->factory, p_->node);
-    registerNodesIntoFactory(p_->factory);
+    LoadPlugins(p_->params, p_->factory, p_->node);
+    try {
+      registerNodesIntoFactory(p_->factory);
+    } catch (const std::exception& e) {
+      RCLCPP_ERROR(kLogger, "Failed to registerNodesIntoFactory(): %s", e.what());
+    }
+    RegisterBehaviorTrees(p_->params, p_->factory);
+    bt_loaded_ = true;
   }
 
   // Loop until something happens with ROS or the node completes