diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 3aaec265445..6d5dcae44df 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -28,7 +28,8 @@ add_library(${library_name} SHARED target_include_directories(${library_name} PUBLIC "$" - "$") + "$" +) 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" +) diff --git a/nav2_behavior_tree/cmake/generate_tree_nodes_xml.cmake b/nav2_behavior_tree/cmake/generate_tree_nodes_xml.cmake new file mode 100644 index 00000000000..fd91143fef0 --- /dev/null +++ b/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 .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} + $ + "${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() + diff --git a/nav2_behavior_tree/cmake/plugin_list.txt.in b/nav2_behavior_tree/cmake/plugin_list.txt.in new file mode 100644 index 00000000000..f9d50186cb0 --- /dev/null +++ b/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 diff --git a/nav2_behavior_tree/nav2_behavior_tree-extras.cmake b/nav2_behavior_tree/nav2_behavior_tree-extras.cmake new file mode 100644 index 00000000000..a0c16ae447e --- /dev/null +++ b/nav2_behavior_tree/nav2_behavior_tree-extras.cmake @@ -0,0 +1 @@ +include("${nav2_behavior_tree_DIR}/generate_tree_nodes_xml.cmake") diff --git a/nav2_behavior_tree/plugins_list.hpp.in b/nav2_behavior_tree/plugins_list.hpp.in index 148583b9271..0ca4a4f92f4 100644 --- a/nav2_behavior_tree/plugins_list.hpp.in +++ b/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@"; } diff --git a/nav2_behavior_tree/src/generate_tree_nodes_xml_cli.cpp b/nav2_behavior_tree/src/generate_tree_nodes_xml_cli.cpp new file mode 100644 index 00000000000..978fd914740 --- /dev/null +++ b/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 +#include +#include + +#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 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; +} diff --git a/nav2_docking/opennav_docking_bt/CMakeLists.txt b/nav2_docking/opennav_docking_bt/CMakeLists.txt index a852c4124b7..10c1d9a8d39 100644 --- a/nav2_docking/opennav_docking_bt/CMakeLists.txt +++ b/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 "$" @@ -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 "$" @@ -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 diff --git a/nav2_docking/opennav_docking_bt/opennav_docking_tree_nodes.xml b/nav2_docking/opennav_docking_bt/opennav_docking_tree_nodes.xml new file mode 100644 index 00000000000..db1e6ba21b5 --- /dev/null +++ b/nav2_docking/opennav_docking_bt/opennav_docking_tree_nodes.xml @@ -0,0 +1,28 @@ + + + + Dock ID or name to use + Whether to autonomously navigate to staging pose + The dock pose, if not using dock id + Maximum time to navigate to the staging pose + The dock plugin type, if using dock pose + The number of retries executed + If the action was successful + Error message + Error code + + Whether to use the dock's ID or dock pose fields + Action server name + + + The dock plugin type, if not previous instance used for docking + Maximum time to get back to the staging pose + If the action was successful + Error message + Error code + + Action server name + + + +