Skip to content

Commit 20eceb0

Browse files
Jad-ELHAJJSteveMacenski
Jad-ELHAJJ
authored and
SteveMacenski
committed
Support loading multiple behavior tree files as subtrees (#5426)
* Support loading multiple behavior tree files as subtrees Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Fix code style Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Added default param value Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Added recursive check to loadBehaviorTree and adapted unit test accordingly Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Removed nested loadBehaviorTree check in navigators Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Removed whitespace cpplint Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Fixed goalReceived Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Let loadbehaviorTree use its own search_directories var Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * PR fixes-format-lint and test Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * fix pointer Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Added unit test for BT xml validity Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * CPPLint Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Check non existent search directory for bt Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * CPPLint Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Fixed BT tests Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Fixed BT tests Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> --------- Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai>
1 parent 3c68548 commit 20eceb0

File tree

9 files changed

+232
-81
lines changed

9 files changed

+232
-81
lines changed

‎nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -98,6 +98,12 @@ class BehaviorTreeEngine
9898
*/
9999
void resetGrootMonitor();
100100

101+
/**
102+
* @brief Function to register a BT from an XML file
103+
* @param file_path Path to BT XML file
104+
*/
105+
void registerTreeFromFile(const std::string & file_path);
106+
101107
/**
102108
* @brief Function to explicitly reset all BT nodes to initial state
103109
* @param tree Tree to halt

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,7 @@ class BtActionServer
5555
const std::string & action_name,
5656
const std::vector<std::string> & plugin_lib_names,
5757
const std::string & default_bt_xml_filename,
58+
const std::vector<std::string> & search_directories,
5859
OnGoalReceivedCallback on_goal_received_callback,
5960
OnLoopCallback on_loop_callback,
6061
OnPreemptCallback on_preempt_callback,
@@ -104,7 +105,8 @@ class BtActionServer
104105
* @return bool true if the resulting BT correspond to the one in bt_xml_filename. false
105106
* if something went wrong, and previous BT is maintained
106107
*/
107-
bool loadBehaviorTree(const std::string & bt_xml_filename = "");
108+
bool loadBehaviorTree(
109+
const std::string & bt_xml_filename = "");
108110

109111
/**
110112
* @brief Getter function for BT Blackboard
@@ -247,6 +249,7 @@ class BtActionServer
247249
// The XML file that contains the Behavior Tree to create
248250
std::string current_bt_xml_filename_;
249251
std::string default_bt_xml_filename_;
252+
std::vector<std::string> search_directories_;
250253

251254
// The wrapper class for the BT functionality
252255
std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp‎

Lines changed: 30 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -39,12 +39,14 @@ BtActionServer<ActionT>::BtActionServer(
3939
const std::string & action_name,
4040
const std::vector<std::string> & plugin_lib_names,
4141
const std::string & default_bt_xml_filename,
42+
const std::vector<std::string> & search_directories,
4243
OnGoalReceivedCallback on_goal_received_callback,
4344
OnLoopCallback on_loop_callback,
4445
OnPreemptCallback on_preempt_callback,
4546
OnCompletionCallback on_completion_callback)
4647
: action_name_(action_name),
4748
default_bt_xml_filename_(default_bt_xml_filename),
49+
search_directories_(search_directories),
4850
plugin_lib_names_(plugin_lib_names),
4951
node_(parent),
5052
on_goal_received_callback_(on_goal_received_callback),
@@ -245,6 +247,8 @@ void BtActionServer<ActionT>::setGrootMonitoring(const bool enable, const unsign
245247
template<class ActionT>
246248
bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filename)
247249
{
250+
namespace fs = std::filesystem;
251+
248252
// Empty filename is default for backward compatibility
249253
auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename;
250254

@@ -254,19 +258,38 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
254258
return true;
255259
}
256260

257-
// if a new tree is created, than the Groot2 Publisher must be destroyed
261+
// Reset any existing Groot2 monitoring
258262
bt_->resetGrootMonitor();
259263

260-
// Read the input BT XML from the specified file into a string
261264
std::ifstream xml_file(filename);
262-
263265
if (!xml_file.good()) {
264266
setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
265-
"Couldn't open input XML file: " + filename);
267+
"Couldn't open BT XML file: " + filename);
266268
return false;
267269
}
268270

269-
// Create the Behavior Tree from the XML input
271+
const auto canonical_main_bt = fs::canonical(filename);
272+
273+
// Register all XML behavior Subtrees found in the given directories
274+
for (const auto & directory : search_directories_) {
275+
try {
276+
for (const auto & entry : fs::directory_iterator(directory)) {
277+
if (entry.path().extension() == ".xml") {
278+
// Skip registering the main tree file
279+
if (fs::equivalent(fs::canonical(entry.path()), canonical_main_bt)) {
280+
continue;
281+
}
282+
bt_->registerTreeFromFile(entry.path().string());
283+
}
284+
}
285+
} catch (const std::exception & e) {
286+
setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
287+
"Exception reading behavior tree directory: " + std::string(e.what()));
288+
return false;
289+
}
290+
}
291+
292+
// Try to load the main BT tree
270293
try {
271294
tree_ = bt_->createTreeFromFile(filename, blackboard_);
272295
for (auto & subtree : tree_.subtrees) {
@@ -280,15 +303,15 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
280303
}
281304
} catch (const std::exception & e) {
282305
setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
283-
std::string("Exception when loading BT: ") + e.what());
306+
std::string("Exception when creating BT tree from file: ") + e.what());
284307
return false;
285308
}
286309

310+
// Optional logging and monitoring
287311
topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
288312

289313
current_bt_xml_filename_ = filename;
290314

291-
// Enable monitoring with Groot2
292315
if (enable_groot_monitoring_) {
293316
bt_->addGrootMonitoring(&tree_, groot_server_port_);
294317
RCLCPP_DEBUG(

‎nav2_behavior_tree/src/behavior_tree_engine.cpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -101,6 +101,13 @@ BehaviorTreeEngine::createTreeFromFile(
101101
return factory_.createTreeFromFile(file_path, blackboard);
102102
}
103103

104+
/// @brief Register a tree from an XML file and return the tree
105+
void BehaviorTreeEngine::registerTreeFromFile(
106+
const std::string & file_path)
107+
{
108+
factory_.registerBehaviorTreeFromFile(file_path);
109+
}
110+
104111
void
105112
BehaviorTreeEngine::addGrootMonitoring(
106113
BT::Tree * tree,

‎nav2_bringup/params/nav2_params.yaml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,8 @@ bt_navigator:
5858
plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
5959
enable_groot_monitoring: false
6060
groot_server_port: 1669
61+
bt_search_directories:
62+
- $(find-pkg-share nav2_bt_navigator)/behavior_trees
6163
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
6264
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
6365
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml

‎nav2_bt_navigator/src/navigators/navigate_through_poses.cpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -92,7 +92,6 @@ bool
9292
NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
9393
{
9494
auto bt_xml_filename = goal->behavior_tree;
95-
9695
if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
9796
bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
9897
"Error loading XML file: " + bt_xml_filename + ". Navigation canceled.");

‎nav2_bt_navigator/src/navigators/navigate_to_pose.cpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -99,7 +99,6 @@ bool
9999
NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
100100
{
101101
auto bt_xml_filename = goal->behavior_tree;
102-
103102
if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
104103
bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
105104
std::string("Error loading XML file: ") + bt_xml_filename + ". Navigation canceled.");

‎nav2_core/include/nav2_core/behavior_tree_navigator.hpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -201,12 +201,19 @@ class BehaviorTreeNavigator : public NavigatorBase
201201
// get the default behavior tree for this navigator
202202
std::string default_bt_xml_filename = getDefaultBTFilepath(parent_node);
203203

204+
auto search_directories = node->declare_or_get_parameter(
205+
"bt_search_directories",
206+
std::vector<std::string>{ament_index_cpp::get_package_share_directory(
207+
"nav2_bt_navigator") + "/behavior_trees"}
208+
);
209+
204210
// Create the Behavior Tree Action Server for this navigator
205211
bt_action_server_ = std::make_unique<nav2_behavior_tree::BtActionServer<ActionT>>(
206212
node,
207213
getName(),
208214
plugin_lib_names,
209215
default_bt_xml_filename,
216+
search_directories,
210217
std::bind(&BehaviorTreeNavigator::onGoalReceived, this, std::placeholders::_1),
211218
std::bind(&BehaviorTreeNavigator::onLoop, this),
212219
std::bind(&BehaviorTreeNavigator::onPreempt, this, std::placeholders::_1),

0 commit comments

Comments
 (0)