diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 72883e0a75f7a81565e5cb04a0b3ed7c11cf0b16..dccf466e8ccfcab530f63bf142a6e57eb340d982 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/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)); diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 6691025f1bc4d4f25a3e2f93d6571e9fbc534230..acf962d3561f708e2a5b295d65cd436dae80ec38 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/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) diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 8b4bcf9a1459ca2b657ebe5a9ab0bc735dea2584..0ac93a922b539d93a04b01f63ec49c136876c9b1 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/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_; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index d31ab0539e7614a72dc0a690e18badbff8492080..cf78d49aa827d1d40d61076f225289f865df2c12 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/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: