@@ -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
245247template <class ActionT >
246248bool 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: "
267+ " Couldn't open BT XML file: "
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: " what ());
306+ std::string (" Exception when creating BT tree from file : " 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 (
0 commit comments