Skip to content

Commit 8377c76

Browse files
ewakewak
committed
Add generic generate_tree_nodes_xml mechanism
Use this to generate tree nodes file for opennav_docking_bt Signed-off-by: Mike Wake <[email protected]>
1 parent 088c423 commit 8377c76

File tree

7 files changed

+228
-6
lines changed

7 files changed

+228
-6
lines changed

‎nav2_behavior_tree/CMakeLists.txt‎

Lines changed: 44 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,8 @@ add_library(${library_name} SHARED
2828
target_include_directories(${library_name}
2929
PUBLIC
3030
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
31-
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
31+
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
32+
)
3233
target_link_libraries(${library_name} PUBLIC
3334
${action_msgs_TARGETS}
3435
behaviortree_cpp::behaviortree_cpp
@@ -244,16 +245,29 @@ install(TARGETS ${library_name} ${plugin_libs}
244245

245246
# we will embed the list of plugin names inside a header file
246247
set(GENERATED_DIR ${CMAKE_CURRENT_BINARY_DIR}/gen)
248+
set(TREENODES_FILE nav2_tree_nodes.xml)
247249
configure_file(plugins_list.hpp.in ${GENERATED_DIR}/plugins_list.hpp)
248250

251+
# create a text file listing plugin names, one per line
252+
string(REPLACE ";" "\n" plugin_libs_one_per_line "${plugin_libs}")
253+
configure_file(plugins_list.txt.in ${GENERATED_DIR}/plugins_list.txt)
254+
249255
add_executable(generate_nav2_tree_nodes_xml src/generate_nav2_tree_nodes_xml.cpp)
250256
target_link_libraries(generate_nav2_tree_nodes_xml PRIVATE
251257
behaviortree_cpp::behaviortree_cpp
252258
nav2_util::nav2_util_core
253259
)
254260
# allow generate_nav2_tree_nodes_xml to find plugins_list.hpp
255261
target_include_directories(generate_nav2_tree_nodes_xml PRIVATE ${GENERATED_DIR})
256-
install(TARGETS generate_nav2_tree_nodes_xml DESTINATION lib/${PROJECT_NAME})
262+
263+
add_executable(generate_tree_nodes_xml_cli src/generate_tree_nodes_xml_cli.cpp)
264+
target_link_libraries(generate_tree_nodes_xml_cli PRIVATE
265+
behaviortree_cpp::behaviortree_cpp
266+
nav2_util::nav2_util_core
267+
)
268+
install(TARGETS generate_nav2_tree_nodes_xml generate_tree_nodes_xml_cli
269+
EXPORT ${library_name}
270+
DESTINATION lib/${PROJECT_NAME})
257271

258272
install(DIRECTORY include/
259273
DESTINATION include/${PROJECT_NAME}
@@ -263,8 +277,35 @@ install(DIRECTORY test/utils/
263277
DESTINATION include/${PROJECT_NAME}/nav2_behavior_tree/test/utils
264278
)
265279

266-
install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})
280+
add_custom_command(
281+
OUTPUT ${GENERATED_DIR}/${TREENODES_FILE}
282+
COMMAND ${CMAKE_COMMAND} -E env LD_LIBRARY_PATH=${CMAKE_CURRENT_BINARY_DIR}:$ENV{LD_LIBRARY_PATH}
283+
$<TARGET_FILE:generate_tree_nodes_xml_cli>
284+
${GENERATED_DIR}/plugins_list.txt
285+
${GENERATED_DIR}/${TREENODES_FILE}
286+
POST_BUILD
287+
DEPENDS
288+
${plugin_libs}
289+
$<TARGET_FILE:generate_tree_nodes_xml_cli>
290+
#generate_tree_nodes_xml_cli
291+
${GENERATED_DIR}/plugins_list.txt
292+
COMMENT "Generating groot tree nodes description file"
293+
VERBATIM
294+
)
295+
296+
add_custom_target(gen_tree_nodes_xml ALL
297+
DEPENDS
298+
generate_tree_nodes_xml_cli
299+
${GENERATED_DIR}/plugins_list.txt
300+
${GENERATED_DIR}/${TREENODES_FILE}
301+
)
302+
303+
#NOTE: Still using manually managed ${TREENODES_FILE} in source directory
304+
#install(FILES ${GENERATED_DIR}/${TREENODES_FILE} DESTINATION share/${PROJECT_NAME})
305+
install(FILES ${TREENODES_FILE} DESTINATION share/${PROJECT_NAME})
306+
267307
install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME}/${PROJECT_NAME})
308+
install(FILES ${GENERATED_DIR}/plugins_list.txt DESTINATION share/${PROJECT_NAME})
268309

269310
if(BUILD_TESTING)
270311
find_package(ament_lint_auto REQUIRED)

‎nav2_behavior_tree/plugins_list.hpp.in‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11

2-
// This was automativally generated by cmake
3-
namespace nav2::details
2+
// This was automatically generated by cmake
3+
namespace nav2::details
44
{
55
const char* BT_BUILTIN_PLUGINS = "@plugin_libs@";
66
}

‎nav2_behavior_tree/plugins_list.txt.in‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
# This file was automatically generated by cmake
2+
# Start @PROJECT_NAME@ plugin_libs
3+
4+
@plugin_libs_one_per_line@
5+
6+
# End @PROJECT_NAME@ plugin_libs

‎nav2_behavior_tree/src/generate_tree_nodes_xml_cli.cpp‎

Lines changed: 109 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,109 @@
1+
// Copyright (c) 2024 Davide Faconti
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License. Reserved.
14+
15+
#include <vector>
16+
#include <string>
17+
#include <fstream>
18+
19+
#include "behaviortree_cpp/behavior_tree.h"
20+
#include "behaviortree_cpp/bt_factory.h"
21+
#include "behaviortree_cpp/utils/shared_library.h"
22+
#include "behaviortree_cpp/xml_parsing.h"
23+
24+
void usage(const std::string & program_name)
25+
{
26+
std::cout << "TreeNodesModel description file generator for behaviortree_cpp groot tool\n"
27+
<< " usage: " << program_name << "[--verbose] input_file output_file\n"
28+
<< " input_file - line separated list of plugin libs\n"
29+
<< " output_file - TreeNodesModel xml file" << std::endl;
30+
}
31+
32+
int main(int argc, char ** argv)
33+
{
34+
BT::BehaviorTreeFactory factory;
35+
bool verbose = false;
36+
37+
if (argc < 3) {
38+
usage(argv[0]);
39+
return 1;
40+
}
41+
42+
int i = 1;
43+
if (std::string(argv[i]) == "-v" || std::string(argv[i]) == "--verbose") {
44+
verbose = true;
45+
i++;
46+
}
47+
std::string input_filename = argv[i++];
48+
std::string output_filename = argv[i++];
49+
50+
std::vector<std::string> plugins_list;
51+
52+
try {
53+
std::ifstream file(input_filename);
54+
std::string line;
55+
56+
if (!file.is_open()) {
57+
std::cerr << "Unable to open input file: " << input_filename << std::endl;
58+
return 1;
59+
}
60+
61+
while (std::getline(file, line)) {
62+
if (!line.empty() &&
63+
line[0] != '#' &&
64+
line.find_first_not_of(" \t\n\v\f\r") != std::string::npos)
65+
{
66+
plugins_list.push_back(line);
67+
}
68+
}
69+
file.close();
70+
} catch (const std::ios_base::failure & e) {
71+
std::cerr << "I/O error: " << e.what() << std::endl;
72+
return 1;
73+
} catch (const std::exception & e) {
74+
std::cerr << "Error: " << e.what() << std::endl;
75+
return 1;
76+
}
77+
78+
try {
79+
for (const auto & plugin : plugins_list) {
80+
if (verbose) {
81+
std::cout << "Loading: " << plugin << std::endl;
82+
}
83+
factory.registerFromPlugin(BT::SharedLibrary::getOSName(plugin));
84+
}
85+
} catch (const std::exception & e) {
86+
std::cerr << "Loading plugin error: " << e.what() << std::endl;
87+
return 1;
88+
}
89+
90+
try {
91+
if (verbose) {
92+
std::cout << "Writing TreeNodesModel file: " << output_filename
93+
<< "\nCompare it with the one in the git repo and update the latter if necessary.\n";
94+
}
95+
96+
std::ofstream xml_file;
97+
xml_file.open(output_filename);
98+
xml_file << BT::writeTreeNodesModelXML(factory) << std::endl;
99+
xml_file.close();
100+
} catch (const std::ios_base::failure & e) {
101+
std::cerr << "I/O error: " << e.what() << std::endl;
102+
return 1;
103+
} catch (const std::exception & e) {
104+
std::cerr << "Error: " << e.what() << std::endl;
105+
return 1;
106+
}
107+
108+
return 0;
109+
}

‎nav2_docking/opennav_docking_bt/CMakeLists.txt‎

Lines changed: 33 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ find_package(rclcpp_action REQUIRED)
1616
nav2_package()
1717

1818
add_library(opennav_dock_action_bt_node SHARED src/dock_robot.cpp)
19+
list(APPEND plugin_libs opennav_dock_action_bt_node)
1920
target_include_directories(opennav_dock_action_bt_node
2021
PUBLIC
2122
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
@@ -30,6 +31,7 @@ target_link_libraries(opennav_dock_action_bt_node PUBLIC
3031
)
3132

3233
add_library(opennav_undock_action_bt_node SHARED src/undock_robot.cpp)
34+
list(APPEND plugin_libs opennav_undock_action_bt_node)
3335
target_include_directories(opennav_undock_action_bt_node
3436
PUBLIC
3537
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
@@ -43,6 +45,36 @@ target_link_libraries(opennav_undock_action_bt_node PUBLIC
4345
${nav2_msgs_TARGETS}
4446
)
4547

48+
set(GENERATED_DIR ${CMAKE_CURRENT_BINARY_DIR}/gen)
49+
set(TREENODES_FILE opennav_docking_tree_nodes.xml)
50+
string(REPLACE ";" "\n" plugin_libs_one_per_line "${plugin_libs}")
51+
configure_file(plugins_list.txt.in ${GENERATED_DIR}/plugins_list.txt)
52+
53+
add_custom_command(
54+
OUTPUT ${GENERATED_DIR}/${TREENODES_FILE}
55+
COMMAND ${CMAKE_COMMAND} -E env LD_LIBRARY_PATH=${CMAKE_CURRENT_BINARY_DIR}:$ENV{LD_LIBRARY_PATH}
56+
$<TARGET_FILE:nav2_behavior_tree::generate_tree_nodes_xml_cli>
57+
${GENERATED_DIR}/plugins_list.txt
58+
${GENERATED_DIR}/${TREENODES_FILE}
59+
POST_BUILD
60+
DEPENDS
61+
${plugin_libs}
62+
nav2_behavior_tree::generate_tree_nodes_xml_cli
63+
${GENERATED_DIR}/plugins_list.txt
64+
COMMENT "Generating groot tree nodes description file - ${plugin_libs}"
65+
VERBATIM
66+
)
67+
68+
add_custom_target(gen_tree_nodes_xml ALL
69+
DEPENDS
70+
nav2_behavior_tree::generate_tree_nodes_xml_cli
71+
${GENERATED_DIR}/plugins_list.txt
72+
${GENERATED_DIR}/${TREENODES_FILE}
73+
)
74+
75+
install(FILES ${GENERATED_DIR}/${TREENODES_FILE} DESTINATION share/${PROJECT_NAME})
76+
install(FILES ${GENERATED_DIR}/plugins_list.txt DESTINATION share/${PROJECT_NAME})
77+
4678
install(TARGETS opennav_dock_action_bt_node opennav_undock_action_bt_node
4779
EXPORT ${PROJECT_NAME}
4880
ARCHIVE DESTINATION lib
@@ -64,7 +96,7 @@ if(BUILD_TESTING)
6496
endif()
6597

6698
ament_export_include_directories(include/${PROJECT_NAME})
67-
ament_export_libraries(opennav_dock_action_bt_node opennav_undock_action_bt_node)
99+
ament_export_libraries(${plugin_libs})
68100
ament_export_dependencies(
69101
behaviortree_cpp
70102
geometry_msgs

‎nav2_docking/opennav_docking_bt/opennav_docking_tree_nodes.xml‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
<root BTCPP_format="4">
2+
<TreeNodesModel>
3+
<Action ID="DockRobot">
4+
<input_port name="dock_id" type="std::string">Dock ID or name to use</input_port>
5+
<input_port name="navigate_to_staging_pose" type="bool" default="true">Whether to autonomously navigate to staging pose</input_port>
6+
<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>
7+
<input_port name="max_staging_time" type="float" default="1000.000000">Maximum time to navigate to the staging pose</input_port>
8+
<input_port name="dock_type" type="std::string">The dock plugin type, if using dock pose</input_port>
9+
<output_port name="num_retries" type="unsigned short">The number of retries executed</output_port>
10+
<output_port name="success" type="bool">If the action was successful</output_port>
11+
<output_port name="error_msg" type="std::string">Error message</output_port>
12+
<output_port name="error_code_id" type="unsigned short">Error code</output_port>
13+
<input_port name="server_timeout" type="std::chrono::milliseconds"/>
14+
<input_port name="use_dock_id" type="bool" default="true">Whether to use the dock's ID or dock pose fields</input_port>
15+
<input_port name="server_name" type="std::string">Action server name</input_port>
16+
</Action>
17+
<Action ID="UndockRobot">
18+
<input_port name="dock_type" type="std::string">The dock plugin type, if not previous instance used for docking</input_port>
19+
<input_port name="max_undocking_time" type="float" default="30.000000">Maximum time to get back to the staging pose</input_port>
20+
<output_port name="success" type="bool">If the action was successful</output_port>
21+
<output_port name="error_msg" type="std::string">Error message</output_port>
22+
<output_port name="error_code_id" type="unsigned short">Error code</output_port>
23+
<input_port name="server_timeout" type="std::chrono::milliseconds"/>
24+
<input_port name="server_name" type="std::string">Action server name</input_port>
25+
</Action>
26+
</TreeNodesModel>
27+
</root>
28+

‎nav2_docking/opennav_docking_bt/plugins_list.txt.in‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
# This file was automatically generated by cmake
2+
# Start @PROJECT_NAME@ plugin_libs
3+
4+
@plugin_libs_one_per_line@
5+
6+
# End @PROJECT_NAME@ plugin_libs

0 commit comments

Comments
 (0)