Add subtree registration system

behaviortree_cpp_pluginlib/CMakeLists.txt

@@ -43,6 +43,7 @@ target_link_libraries(${PROJECT_NAME}
     pluginlib::pluginlib
   PRIVATE
     rcutils::rcutils
+    ament_index_cpp::ament_index_cpp
 )
 
 # Install and export resources

behaviortree_cpp_pluginlib/README.md

@@ -68,6 +68,38 @@ Simple as 1, 2, 3: depend on this package, register your library as a plugin pro
     }
     ```
 
+## Registering Subtrees
+
+You can also ship reusable subtrees as `.xml` files. They are discovered and registered by `BT::PluginAwareFactory` automatically, so any loaded tree can reference them via `<SubTree ID="..."/>` without loading files by hand.
+
+1. Write a subtree XML with a single `<BehaviorTree ID="...">` (the `ID` is the name you reference as a `<SubTree>`):
+
+    ```xml
+    <root BTCPP_format="4">
+      <BehaviorTree ID="GoAndBeep">
+        ...
+      </BehaviorTree>
+    </root>
+    ```
+
+2. `CMakeLists.txt` - register the file(s). Use the `SUBTREES` keyword to ship them alongside a plugin library (e.g. one that provides the nodes the subtree uses):
+
+    ```cmake
+    register_behaviortree_cpp_plugin(my_plugin_library
+      SUBTREES trees/go_and_beep.xml
+    )
+    ```
+
+    Or, for a package that ships subtrees but builds no plugin library, use the standalone function:
+
+    ```cmake
+    register_behaviortree_cpp_subtrees(FILES trees/go_and_beep.xml)
+    # Pass a distinct NAME when calling more than once in a single package:
+    # register_behaviortree_cpp_subtrees(NAME navigation FILES trees/go_and_beep.xml)
+    ```
+
+Note: a subtree may only reference built-in nodes or nodes provided by a loaded plugin. Referencing a node that is registered manually after the factory is constructed is not supported — such a subtree is logged and skipped at load time.
+
 ## Loading Plugins
 
 To load all registered plugins, link against the exported library target and use the `BT::PluginAwareFactory`
@@ -90,6 +122,25 @@ To load all registered plugins, link against the exported library target and use
     ...
     ```
 
+3. All registered nodes _and_ subtrees are now available. Reference a shipped subtree by its `ID` from any tree you load - no need to load its file yourself:
+
+    ```c++
+    factory.registerBehaviorTreeFromText(R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="Main">
+          <Sequence>
+            <SubTree ID="GoAndBeep"/>
+          </Sequence>
+        </BehaviorTree>
+      </root>)");
+
+    // Or load the tree definition from a file instead:
+    // factory.registerBehaviorTreeFromFile("path/to/main.xml");
+
+    auto tree = factory.createTree("Main");
+    tree.tickWhileRunning();
+    ```
+
 # Implementation Details
 
 For more information about what's happening under the hood to enable these usage patterns, see [DEVELOPING.md](./DEVELOPING.md)

behaviortree_cpp_pluginlib/cmake/register_behaviortree_cpp_plugin.cmake

@@ -17,13 +17,19 @@
 #
 # Example usage:
 # register_behaviortree_cpp_plugin(my_library)
+# register_behaviortree_cpp_plugin(my_library SUBTREES trees/patrol.xml trees/dock.xml)
 #
 # :param TARGET: name of a valid CMake shared library target to export plugins from
 # :type TARGET: string
+# :param SUBTREES: optional list of subtree XML files to ship with this plugin. Each file is
+#   installed and registered so that BT::PluginAwareFactory loads it automatically, making its
+#   <BehaviorTree ID="..."> definitions available to any loaded tree via <SubTree ID="..."/>.
+# :type SUBTREES: list of files
 #
 # @public
 #
 function(register_behaviortree_cpp_plugin arg_TARGET)
+  cmake_parse_arguments(ARG "" "" "SUBTREES" ${ARGN})
   if(NOT arg_TARGET)
     message(FATAL_ERROR "register_behaviortree_cpp_plugin() called without TARGET argument")
   endif()
@@ -57,4 +63,65 @@ function(register_behaviortree_cpp_plugin arg_TARGET)
   )
   list(APPEND __PLUGINLIB_PLUGIN_CATEGORIES "behaviortree_cpp")
   set(__PLUGINLIB_PLUGIN_CATEGORIES "${__PLUGINLIB_PLUGIN_CATEGORIES}" PARENT_SCOPE)
+
+  # Ship subtree XML alongside this plugin, keyed by the (package-unique) target name.
+  if(ARG_SUBTREES)
+    _register_behaviortree_cpp_subtrees("${arg_TARGET}" ${ARG_SUBTREES})
+  endif()
+endfunction()
+
+#
+# Register BehaviorTree.CPP subtree XML files without a C++ plugin target.
+#
+# Use this for packages that ship reusable subtrees but build no node plugin library. The subtrees
+# are installed and registered so that BT::PluginAwareFactory loads them automatically.
+#
+# Example usage:
+# register_behaviortree_cpp_subtrees(FILES trees/patrol.xml trees/dock.xml)
+# register_behaviortree_cpp_subtrees(NAME navigation FILES trees/patrol.xml)
+#
+# :param NAME: optional group suffix, used to build a unique resource marker. Defaults to "subtrees".
+#   Pass distinct NAMEs when calling this more than once in a single package.
+# :type NAME: string
+# :param FILES: list of subtree XML files to install and register.
+# :type FILES: list of files
+#
+# @public
+#
+function(register_behaviortree_cpp_subtrees)
+  cmake_parse_arguments(ARG "" "NAME" "FILES" ${ARGN})
+  if(NOT ARG_FILES)
+    message(FATAL_ERROR "register_behaviortree_cpp_subtrees() called without FILES argument")
+  endif()
+  set(marker_suffix "subtrees")
+  if(ARG_NAME)
+    set(marker_suffix "${ARG_NAME}")
+  endif()
+  _register_behaviortree_cpp_subtrees("${marker_suffix}" ${ARG_FILES})
+endfunction()
+
+#
+# Internal helper: install subtree XML files and register them in the "behaviortree_cpp_subtrees"
+# ament resource index category for BT::PluginAwareFactory to discover at runtime.
+#
+# The marker is named "<PROJECT_NAME>__<marker_suffix>" so repeated calls in one package don't
+# collide (ament_index_register_resource uses file(GENERATE), which errors on a reused path).
+#
+function(_register_behaviortree_cpp_subtrees marker_suffix)
+  set(marker_content "")
+  foreach(subtree_xml ${ARGN})
+    get_filename_component(subtree_abs "${subtree_xml}" ABSOLUTE)
+    if(NOT EXISTS "${subtree_abs}")
+      message(FATAL_ERROR "register subtrees: file does not exist: ${subtree_abs}")
+    endif()
+    get_filename_component(subtree_name "${subtree_abs}" NAME)
+    install(FILES "${subtree_abs}" DESTINATION share/${PROJECT_NAME}/behaviortree_subtrees)
+    # Share-relative path including the package folder, resolved at runtime as <prefix>/share/<line>.
+    string(APPEND marker_content "${PROJECT_NAME}/behaviortree_subtrees/${subtree_name}\n")
+  endforeach()
+
+  ament_index_register_resource("behaviortree_cpp_subtrees"
+    CONTENT "${marker_content}"
+    PACKAGE_NAME "${PROJECT_NAME}__${marker_suffix}"
+  )
 endfunction()

behaviortree_cpp_pluginlib/package.xml

@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <depend>ament_index_cpp</depend>
   <depend>behaviortree_cpp</depend>
   <depend>pluginlib</depend>
   <depend>rcutils</depend>

behaviortree_cpp_pluginlib/src/factory.cpp

@@ -14,10 +14,14 @@
 
 #include "behaviortree_cpp_pluginlib/factory.hpp"
 
+#include <filesystem>
 #include <memory>
+#include <sstream>
 #include <string>
 #include <vector>
 
+#include "ament_index_cpp/get_resource.hpp"
+#include "ament_index_cpp/get_resources.hpp"
 #include "behaviortree_cpp/bt_factory.h"
 #include "behaviortree_cpp_pluginlib/plugin.hpp"
 #include "rcutils/logging_macros.h"
@@ -40,10 +44,42 @@ PluginAwareFactory::PluginAwareFactory(const std::vector<std::string> & plugin_x
       class_library_path.c_str());
     plugin->registerTypes(*this);
   }
+
+  // Load shipped subtree XML only after every node type is registered above: BT.CPP verifies
+  // XML at registration time and rejects subtrees referencing an unregistered node.
+  // Content is a newline-separated list of share-relative paths registered by the
+  // register_behaviortree_cpp_subtrees() CMake helper; the prefix comes from the resource, so it
+  // resolves under both merged and isolated installs.
+  const std::string subtree_resource = "behaviortree_cpp_subtrees";
+  for (const auto & [marker_name, install_prefix] : ament_index_cpp::get_resources(subtree_resource)) {
+    std::string content;
+    if (!ament_index_cpp::get_resource(subtree_resource, marker_name, content)) {
+      continue;
+    }
+    std::istringstream stream(content);
+    std::string relative_path;
+    while (std::getline(stream, relative_path)) {
+      if (relative_path.empty()) {
+        continue;
+      }
+      const std::filesystem::path subtree_path = std::filesystem::path(install_prefix) / "share" / relative_path;
+      try {
+        registerBehaviorTreeFromFile(subtree_path);
+        RCUTILS_LOG_INFO_NAMED("behaviortree_cpp_pluginlib", "Registered subtree(s) from %s", subtree_path.c_str());
+      } catch (const std::exception & e) {
+        // Skip a malformed subtree, or one referencing a node no loaded plugin provides,
+        // rather than failing construction.
+        RCUTILS_LOG_ERROR_NAMED(
+          "behaviortree_cpp_pluginlib", "Failed to register subtree from %s: %s", subtree_path.c_str(), e.what());
+      }
+    }
+  }
 }
 
 PluginAwareFactory::~PluginAwareFactory()
 {
+  clearRegisteredBehaviorTrees();
+
   // First grab all the IDs, since unregistering them modifies the map and invalidates iterators
   std::vector<std::string> ids_to_unregister;
   for (const auto & [id, _] : builders()) {

behaviortree_cpp_pluginlib_tests/CMakeLists.txt

@@ -35,12 +35,18 @@ if(BUILD_TESTING)
   # Create test libraries
   add_library(test_plugin_a SHARED test/plugin_a.cpp)
   target_link_libraries(test_plugin_a PUBLIC behaviortree_cpp_pluginlib::behaviortree_cpp_pluginlib)
-  register_behaviortree_cpp_plugin(test_plugin_a)
+  # Subtree referencing CustomNodeA1, shipped via the SUBTREES keyword.
+  register_behaviortree_cpp_plugin(test_plugin_a
+    SUBTREES ${CMAKE_CURRENT_SOURCE_DIR}/test/subtrees/subtree_uses_a.xml)
 
   add_library(test_plugin_b SHARED test/plugin_b.cpp)
   target_link_libraries(test_plugin_b PRIVATE behaviortree_cpp_pluginlib::behaviortree_cpp_pluginlib)
   register_behaviortree_cpp_plugin(test_plugin_b)
 
+  # Exercise the standalone subtree registration path (and a second marker in one package).
+  register_behaviortree_cpp_subtrees(NAME standalone
+    FILES ${CMAKE_CURRENT_SOURCE_DIR}/test/subtrees/standalone_subtree.xml)
+
   # In the special case of a test-only package that isn't normally installed on target systems,
   # we may install the test targets to run install-space testing on them (pluginlib loading)
   install(

behaviortree_cpp_pluginlib_tests/test/subtrees/standalone_subtree.xml

@@ -0,0 +1,7 @@
+<root BTCPP_format="4">
+  <BehaviorTree ID="StandaloneSubtree">
+    <Sequence>
+      <CustomNodeB1/>
+    </Sequence>
+  </BehaviorTree>
+</root>

behaviortree_cpp_pluginlib_tests/test/subtrees/subtree_uses_a.xml

@@ -0,0 +1,7 @@
+<root BTCPP_format="4">
+  <BehaviorTree ID="SubtreeUsesA">
+    <Sequence>
+      <CustomNodeA1/>
+    </Sequence>
+  </BehaviorTree>
+</root>

behaviortree_cpp_pluginlib_tests/test/test_factory.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <algorithm>
 #include <fstream>
 #include <string>
 #include <vector>
@@ -29,3 +30,32 @@ TEST(Factory, AutofactoryEndToEnd)
   ASSERT_NO_THROW(builders.at("CustomNodeB2"));
   ASSERT_THROW(builders.at("NonexistentNode"), std::out_of_range);
 }
+
+TEST(Factory, SubtreesShippedByPluginsAreRegistered)
+{
+  BT::PluginAwareFactory factory;
+
+  const auto trees = factory.registeredBehaviorTrees();
+  // Shipped via the SUBTREES keyword on test_plugin_a.
+  ASSERT_NE(std::find(trees.begin(), trees.end(), "SubtreeUsesA"), trees.end());
+  // Shipped via the standalone register_behaviortree_cpp_subtrees() path.
+  ASSERT_NE(std::find(trees.begin(), trees.end(), "StandaloneSubtree"), trees.end());
+
+  // Each subtree instantiates directly: its concrete node was registered before it was loaded.
+  ASSERT_NO_THROW(factory.createTree("SubtreeUsesA"));
+  ASSERT_NO_THROW(factory.createTree("StandaloneSubtree"));
+}
+
+TEST(Factory, ShippedSubtreeIsUsableFromAnotherTree)
+{
+  BT::PluginAwareFactory factory;
+
+  // A tree loaded later can reference the shipped subtree "for free" via <SubTree>.
+  factory.registerBehaviorTreeFromText(
+    R"(<root BTCPP_format="4">
+         <BehaviorTree ID="Main">
+           <SubTree ID="SubtreeUsesA"/>
+         </BehaviorTree>
+       </root>)");
+  ASSERT_NO_THROW(factory.createTree("Main"));
+}