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 6f0c9bfb914..f31d89dc84e 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 @@ -19,6 +19,7 @@ #include #include #include +#include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" @@ -61,6 +62,7 @@ class BehaviorTreeEngine BT::Tree * tree, std::function onLoop, std::function cancelRequested, + rclcpp::Logger logger, std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10)); /** diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index e582222437f..9c045bd8738 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -243,8 +243,11 @@ void BtActionServer::executeCallback() }; // Execute the BT that was previously created in the configure step - nav2_behavior_tree::BtStatus rc = bt_->run(tree_, on_loop, is_canceling, bt_loop_duration_); + nav2_behavior_tree::BtStatus rc = bt_->run(tree_, on_loop, is_canceling, logger_, bt_loop_duration_); + // send remaining logs + topic_logger_->flush(); + // Make sure that the Bt is not in a running state from a previous execution // note: if all the ControlNodes are implemented correctly, this is not needed. bt_->haltAllActions(tree_->rootNode()); diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 7bfc003cf1c..d1aacd0bd27 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -38,6 +38,7 @@ BehaviorTreeEngine::run( BT::Tree * tree, std::function onLoop, std::function cancelRequested, + rclcpp::Logger logger, std::chrono::milliseconds loopTimeout) { rclcpp::WallRate loopRate(loopTimeout); @@ -59,7 +60,7 @@ BehaviorTreeEngine::run( } } catch (const std::exception & ex) { RCLCPP_ERROR( - rclcpp::get_logger("BehaviorTreeEngine"), + logger, "Behavior tree threw exception: %s. Exiting with failure.", ex.what()); return BtStatus::FAILED; }