cmake helper function and tool to auto generate TreeNodeModel xml files for groot

nav2_behavior_tree/CMakeLists.txt

@@ -28,7 +28,8 @@ add_library(${library_name} SHARED
 target_include_directories(${library_name}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
 target_link_libraries(${library_name} PUBLIC
   ${action_msgs_TARGETS}
   behaviortree_cpp::behaviortree_cpp
@@ -244,6 +245,7 @@ install(TARGETS ${library_name} ${plugin_libs}
 
 # we will embed the list of plugin names inside a header file
 set(GENERATED_DIR ${CMAKE_CURRENT_BINARY_DIR}/gen)
+set(TREENODES_FILE nav2_tree_nodes.xml)
 configure_file(plugins_list.hpp.in ${GENERATED_DIR}/plugins_list.hpp)
 
 add_executable(generate_nav2_tree_nodes_xml src/generate_nav2_tree_nodes_xml.cpp)
@@ -253,7 +255,22 @@ target_link_libraries(generate_nav2_tree_nodes_xml PRIVATE
 )
 # allow generate_nav2_tree_nodes_xml to find plugins_list.hpp
 target_include_directories(generate_nav2_tree_nodes_xml PRIVATE ${GENERATED_DIR})
-install(TARGETS generate_nav2_tree_nodes_xml DESTINATION lib/${PROJECT_NAME})
+
+add_executable(generate_tree_nodes_xml_cli src/generate_tree_nodes_xml_cli.cpp)
+add_executable(nav2_behavior_tree::generate_tree_nodes_xml_cli ALIAS generate_tree_nodes_xml_cli)
+target_link_libraries(generate_tree_nodes_xml_cli PRIVATE
+  behaviortree_cpp::behaviortree_cpp
+  nav2_util::nav2_util_core
+)
+
+include(cmake/generate_tree_nodes_xml.cmake)
+nav2_generate_tree_nodes_xml(
+  PLUGIN_LIBS ${plugin_libs}
+)
+
+install(TARGETS generate_nav2_tree_nodes_xml generate_tree_nodes_xml_cli
+  EXPORT ${library_name}
+  DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY include/
   DESTINATION include/${PROJECT_NAME}
@@ -263,6 +280,11 @@ install(DIRECTORY test/utils/
   DESTINATION include/${PROJECT_NAME}/nav2_behavior_tree/test/utils
 )
 
+install(DIRECTORY cmake
+  DESTINATION share/${PROJECT_NAME}
+)
+
+#NOTE: Still using manually managed nav2_tree_nodes.xml in source directory
 install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})
 install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME}/${PROJECT_NAME})
 
@@ -301,4 +323,6 @@ ament_export_dependencies(
 )
 ament_export_targets(${library_name})
 
-ament_package()
+ament_package(
+    CONFIG_EXTRAS "nav2_behavior_tree-extras.cmake"
+)

nav2_behavior_tree/cmake/generate_tree_nodes_xml.cmake

@@ -0,0 +1,85 @@
+function(nav2_generate_tree_nodes_xml)
+  # Validate arguments
+  cmake_parse_arguments(
+    ARG
+    "SKIP_INSTALL"
+    "GENERATED_DIR;TREENODES_FILE;PLUGIN_LIST_TEMPLATE_FILE;INSTALL_PATH"
+    "PLUGIN_LIBS"
+    ${ARGN}
+  )
+  if(NOT ARG_GENERATED_DIR)
+    set(ARG_GENERATED_DIR "${CMAKE_CURRENT_BINARY_DIR}/gen")
+  endif()
+  if(NOT ARG_TREENODES_FILE)
+    set(ARG_TREENODES_FILE "${PROJECT_NAME}_tree_nodes.xml")
+  endif()
+  if(NOT ARG_PLUGIN_LIST_TEMPLATE_FILE)
+    if(NOT nav2_behavior_tree_DIR)
+      set(ARG_PLUGIN_LIST_TEMPLATE_FILE "${CMAKE_SOURCE_DIR}/cmake/plugin_list.txt.in")
+    else()
+      set(ARG_PLUGIN_LIST_TEMPLATE_FILE "${nav2_behavior_tree_DIR}/plugin_list.txt.in")
+    endif()
+  endif()
+  if(NOT ARG_INSTALL_PATH)
+    set(ARG_INSTALL_PATH "share/${PROJECT_NAME}")
+  endif()
+
+  # Make sure the templates to use are available
+  if(NOT EXISTS "${ARG_PLUGIN_LIST_TEMPLATE_FILE}")
+    message(FATAL_ERROR "Can't find ${ARG_PLUGIN_LIST_TEMPLATE_FILE}. Maybe reinstall nav2_behavior_tree package.")
+  endif()
+
+  if(NOT ARG_PLUGIN_LIBS)
+    message(FATAL_ERROR "PLUGIN_LIBS option is required.")
+  endif()
+  list(SORT ARG_PLUGIN_LIBS)
+
+  # retrieve version information from <package>.xml file
+  if(NOT _AMENT_PACKAGE_NAME)
+    ament_package_xml()
+  endif()
+  string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
+  set(VERSION_STR ${${PROJECT_NAME}_VERSION})
+
+  # parse version information from the version string
+  if(NOT VERSION_STR MATCHES "([0-9]+)\.([0-9]+)\.([0-9]+)")
+    message(FATAL_ERROR "Version string must be of format MAJOR.MINOR.PATCH")
+  endif()
+  set(VERSION_MAJOR ${CMAKE_MATCH_1})
+  set(VERSION_MINOR ${CMAKE_MATCH_2})
+  set(VERSION_PATCH ${CMAKE_MATCH_3})
+
+  set(GENERATED_TREENODES_FILE "${ARG_GENERATED_DIR}/${ARG_TREENODES_FILE}")
+
+  string(REPLACE ";" "\n" plugin_libs_one_per_line "${ARG_PLUGIN_LIBS}")
+  configure_file(${ARG_PLUGIN_LIST_TEMPLATE_FILE} ${GENERATED_DIR}/plugins_list.txt @ONLY)
+
+  add_custom_command(
+    OUTPUT "${GENERATED_TREENODES_FILE}"
+    COMMAND  ${CMAKE_COMMAND} -E env LD_LIBRARY_PATH=${CMAKE_CURRENT_BINARY_DIR}:$ENV{LD_LIBRARY_PATH}
+      $<TARGET_FILE:nav2_behavior_tree::generate_tree_nodes_xml_cli>
+      "${ARG_GENERATED_DIR}/plugins_list.txt"
+      "${GENERATED_TREENODES_FILE}"
+    POST_BUILD
+    DEPENDS
+      nav2_behavior_tree::generate_tree_nodes_xml_cli
+      "${ARG_PLUGIN_LIBS}"
+      "${GENERATED_DIR}/plugins_list.txt"
+    COMMENT "Generating groot tree nodes description file ${GENERATED_TREENODES_FILE}, using ${ARG_GENERATED_DIR}/plugins_list.txt"
+  )
+
+  add_custom_target("nav2_generate_treenodes_file__${PROJECT_NAME}" ALL
+    DEPENDS
+      nav2_behavior_tree::generate_tree_nodes_xml_cli
+      "${GENERATED_DIR}/plugins_list.txt"
+      "${GENERATED_TREENODES_FILE}"
+  )
+
+  if(NOT ARG_SKIP_INSTALL)
+    install(FILES
+      "${GENERATED_TREENODES_FILE}"
+      "${GENERATED_DIR}/plugins_list.txt"
+      DESTINATION "${ARG_INSTALL_PATH}")
+  endif()
+endfunction()
+

nav2_behavior_tree/cmake/plugin_list.txt.in

@@ -0,0 +1,8 @@
+# This file was automatically generated by cmake
+# project_name: @PROJECT_NAME@
+# version: @VERSION_MAJOR@.@VERSION_MINOR@.@VERSION_PATCH@
+# Start @PROJECT_NAME@ plugin_libs
+
+@plugin_libs_one_per_line@
+
+# End  @PROJECT_NAME@ plugin_libs

nav2_behavior_tree/nav2_behavior_tree-extras.cmake

@@ -0,0 +1 @@
+include("${nav2_behavior_tree_DIR}/generate_tree_nodes_xml.cmake")

nav2_behavior_tree/plugins_list.hpp.in

@@ -1,6 +1,6 @@
 
-// This was automativally generated by cmake
-namespace nav2::details 
+// This was automatically generated by cmake
+namespace nav2::details
 {
   const char* BT_BUILTIN_PLUGINS = "@plugin_libs@";
 }

nav2_behavior_tree/src/generate_tree_nodes_xml_cli.cpp

@@ -0,0 +1,109 @@
+// Copyright (c) 2024 Davide Faconti
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <vector>
+#include <string>
+#include <fstream>
+
+#include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/utils/shared_library.h"
+#include "behaviortree_cpp/xml_parsing.h"
+
+void usage(const std::string & program_name)
+{
+  std::cout << "TreeNodesModel description file generator for behaviortree_cpp groot tool\n"
+            << "  usage: " << program_name << "[--verbose] input_file output_file\n"
+            << "    input_file - line separated list of plugin libs\n"
+            << "    output_file - TreeNodesModel xml file" << std::endl;
+}
+
+int main(int argc, char ** argv)
+{
+  BT::BehaviorTreeFactory factory;
+  bool verbose = false;
+
+  if (argc < 3) {
+    usage(argv[0]);
+    return 1;
+  }
+
+  int i = 1;
+  if (std::string(argv[i]) == "-v" || std::string(argv[i]) == "--verbose") {
+    verbose = true;
+    i++;
+  }
+  std::string input_filename = argv[i++];
+  std::string output_filename = argv[i++];
+
+  std::vector<std::string> plugins_list;
+
+  try {
+    std::ifstream file(input_filename);
+    std::string line;
+
+    if (!file.is_open()) {
+      std::cerr << "Unable to open input file: " << input_filename << std::endl;
+      return 1;
+    }
+
+    while (std::getline(file, line)) {
+      if (!line.empty() &&
+        line[0] != '#' &&
+        line.find_first_not_of(" \t\n\v\f\r") != std::string::npos)
+      {
+        plugins_list.push_back(line);
+      }
+    }
+    file.close();
+  } catch (const std::ios_base::failure & e) {
+    std::cerr << "I/O error: " << e.what() << std::endl;
+    return 1;
+  } catch (const std::exception & e) {
+    std::cerr << "Error: " << e.what() << std::endl;
+    return 1;
+  }
+
+  try {
+    for (const auto & plugin : plugins_list) {
+      if (verbose) {
+        std::cout << "Loading: " << plugin << std::endl;
+      }
+      factory.registerFromPlugin(BT::SharedLibrary::getOSName(plugin));
+    }
+  } catch (const std::exception & e) {
+    std::cerr << "Loading plugin error: " << e.what() << std::endl;
+    return 1;
+  }
+
+  try {
+    if (verbose) {
+      std::cout << "Writing TreeNodesModel file: " << output_filename
+                << "\nCompare it with the one in the git repo and update the latter if necessary.\n";
+    }
+
+    std::ofstream xml_file;
+    xml_file.open(output_filename);
+    xml_file << BT::writeTreeNodesModelXML(factory) << std::endl;
+    xml_file.close();
+  } catch (const std::ios_base::failure & e) {
+    std::cerr << "I/O error: " << e.what() << std::endl;
+    return 1;
+  } catch (const std::exception & e) {
+    std::cerr << "Error: " << e.what() << std::endl;
+    return 1;
+  }
+
+  return 0;
+}

nav2_docking/opennav_docking_bt/CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(rclcpp_action REQUIRED)
 nav2_package()
 
 add_library(opennav_dock_action_bt_node SHARED src/dock_robot.cpp)
+list(APPEND plugin_libs opennav_dock_action_bt_node)
 target_include_directories(opennav_dock_action_bt_node
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
@@ -30,6 +31,7 @@ target_link_libraries(opennav_dock_action_bt_node PUBLIC
 )
 
 add_library(opennav_undock_action_bt_node SHARED src/undock_robot.cpp)
+list(APPEND plugin_libs opennav_undock_action_bt_node)
 target_include_directories(opennav_undock_action_bt_node
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
@@ -43,6 +45,10 @@ target_link_libraries(opennav_undock_action_bt_node PUBLIC
   ${nav2_msgs_TARGETS}
 )
 
+nav2_generate_tree_nodes_xml(
+  PLUGIN_LIBS ${plugin_libs}
+)
+
 install(TARGETS opennav_dock_action_bt_node opennav_undock_action_bt_node
   EXPORT ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
@@ -64,7 +70,7 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include/${PROJECT_NAME})
-ament_export_libraries(opennav_dock_action_bt_node opennav_undock_action_bt_node)
+ament_export_libraries(${plugin_libs})
 ament_export_dependencies(
   behaviortree_cpp
   geometry_msgs

nav2_docking/opennav_docking_bt/opennav_docking_tree_nodes.xml

@@ -0,0 +1,28 @@
+<root BTCPP_format="4">
+    <TreeNodesModel>
+        <Action ID="DockRobot">
+            <input_port name="dock_id" type="std::string">Dock ID or name to use</input_port>
+            <input_port name="navigate_to_staging_pose" type="bool" default="true">Whether to autonomously navigate to staging pose</input_port>
+            <input_port name="dock_pose" type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;">The dock pose, if not using dock id</input_port>
+            <input_port name="max_staging_time" type="float" default="1000.000000">Maximum time to navigate to the staging pose</input_port>
+            <input_port name="dock_type" type="std::string">The dock plugin type, if using dock pose</input_port>
+            <output_port name="num_retries" type="unsigned short">The number of retries executed</output_port>
+            <output_port name="success" type="bool">If the action was successful</output_port>
+            <output_port name="error_msg" type="std::string">Error message</output_port>
+            <output_port name="error_code_id" type="unsigned short">Error code</output_port>
+            <input_port name="server_timeout" type="std::chrono::milliseconds"/>
+            <input_port name="use_dock_id" type="bool" default="true">Whether to use the dock's ID or dock pose fields</input_port>
+            <input_port name="server_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="UndockRobot">
+            <input_port name="dock_type" type="std::string">The dock plugin type, if not previous instance used for docking</input_port>
+            <input_port name="max_undocking_time" type="float" default="30.000000">Maximum time to get back to the staging pose</input_port>
+            <output_port name="success" type="bool">If the action was successful</output_port>
+            <output_port name="error_msg" type="std::string">Error message</output_port>
+            <output_port name="error_code_id" type="unsigned short">Error code</output_port>
+            <input_port name="server_timeout" type="std::chrono::milliseconds"/>
+            <input_port name="server_name" type="std::string">Action server name</input_port>
+        </Action>
+    </TreeNodesModel>
+</root>
+