cherry-picking mjeronimo:fix_bt_crash_after_reset (#1515)

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -35,7 +35,7 @@ public:
   virtual ~BehaviorTreeEngine() {}
 
   BtStatus run(
-    std::unique_ptr<BT::Tree> & tree,
+    BT::Tree * tree,
     std::function<void()> onLoop,
     std::function<bool()> cancelRequested,
     std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -36,7 +36,7 @@ BehaviorTreeEngine::BehaviorTreeEngine(const std::vector<std::string> & plugin_l
 
 BtStatus
 BehaviorTreeEngine::run(
-  std::unique_ptr<BT::Tree> & tree,
+  BT::Tree * tree,
   std::function<void()> onLoop,
   std::function<bool()> cancelRequested,
   std::chrono::milliseconds loopTimeout)

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -119,9 +119,6 @@ protected:
   // The wrapper class for the BT functionality
   std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;
 
-  // The complete behavior tree that results from parsing the incoming XML
-  std::unique_ptr<BT::Tree> tree_;
-
   // Libraries to pull plugins (BT Nodes) from
   std::vector<std::string> plugin_lib_names_;
 

nav2_bt_navigator/src/bt_navigator.cpp

@@ -112,16 +112,6 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   RCLCPP_DEBUG(get_logger(), "Behavior Tree file: '%s'", bt_xml_filename.c_str());
   RCLCPP_DEBUG(get_logger(), "Behavior Tree XML: %s", xml_string_.c_str());
 
-  // Create the Behavior Tree from the XML input (after registering our own node types)
-  BT::Tree temp_tree = bt_->buildTreeFromText(xml_string_, blackboard_);
-
-  // Unfortunately, the BT library provides the tree as a struct instead of a pointer. So, we will
-  // createa new BT::Tree ourselves and move the data over
-  tree_ = std::make_unique<BT::Tree>();
-  tree_->root_node = temp_tree.root_node;
-  tree_->nodes = std::move(temp_tree.nodes);
-  temp_tree.root_node = nullptr;
-
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -164,10 +154,6 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   action_server_.reset();
   plugin_lib_names_.clear();
   xml_string_.clear();
-
-  RCLCPP_INFO(get_logger(), "Cleaning tree");
-
-  tree_.reset();
   blackboard_.reset();
   bt_.reset();
 
@@ -208,7 +194,11 @@ BtNavigator::navigateToPose()
       return action_server_->is_cancel_requested();
     };
 
-  RosTopicLogger topic_logger(client_node_, *tree_);
+
+  // Create the Behavior Tree from the XML input
+  BT::Tree tree = bt_->buildTreeFromText(xml_string_, blackboard_);
+
+  RosTopicLogger topic_logger(client_node_, tree);
 
   auto on_loop = [&]() {
       if (action_server_->is_preempt_requested()) {
@@ -220,7 +210,7 @@ BtNavigator::navigateToPose()
     };
 
   // Execute the BT that was previously created in the configure step
-  nav2_behavior_tree::BtStatus rc = bt_->run(tree_, on_loop, is_canceling);
+  nav2_behavior_tree::BtStatus rc = bt_->run(&tree, on_loop, is_canceling);
 
   switch (rc) {
     case nav2_behavior_tree::BtStatus::SUCCEEDED:
@@ -236,8 +226,6 @@ BtNavigator::navigateToPose()
     case nav2_behavior_tree::BtStatus::CANCELED:
       RCLCPP_INFO(get_logger(), "Navigation canceled");
       action_server_->terminate_all();
-      // Reset the BT so that it can be run again in the future
-      bt_->resetTree(tree_->root_node);
       break;
 
     default: