Fixed goalReceived

Signed-off-by: Jad El Hajj <[email protected]>

Author: Jad-ELHAJJ

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -135,6 +135,15 @@ class BtActionServer
     return default_bt_xml_filename_;
   }
 
+  /**
+   * @brief Getter function for BT search
+   * @return string Containing BT search directories
+   */
+  std::vector<std::string> getSearchDirectories() const
+  {
+    return search_directories_;
+  }
+
   /**
    * @brief Wrapper function to accept pending goal if a preempt has been requested
    * @return Shared pointer to pending action goal

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -72,6 +72,14 @@ NavigateThroughPosesNavigator::getDefaultBTFilepath(
 bool
 NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
 {
+  auto bt_xml_filename = goal->behavior_tree;
+  auto bt_search_directories = bt_action_server_->getSearchDirectories();
+  if (!bt_action_server_->loadBehaviorTree(bt_xml_filename, bt_search_directories)) {
+    bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+      "Error loading XML file: " + bt_xml_filename + ". Navigation canceled.");
+    return false;
+  }
+
   return initializeGoalPoses(goal);
 }
 

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -82,6 +82,14 @@ NavigateToPoseNavigator::cleanup()
 bool
 NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
 {
+  auto bt_xml_filename = goal->behavior_tree;
+  auto bt_search_directories = bt_action_server_->getSearchDirectories();
+  if (!bt_action_server_->loadBehaviorTree(bt_xml_filename, bt_search_directories)) {
+    bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+      std::string("Error loading XML file: ") + bt_xml_filename + ". Navigation canceled.");
+    return false;
+  }
+
   return initializeGoalPose(goal);
 }