From 8108c18835ae943f934a047059a6aff562efe65a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 18 Jun 2024 17:57:49 +0200 Subject: [PATCH] Add cout bt logs Signed-off-by: Tony Najjar --- .../nav2_behavior_tree/bt_action_server.hpp | 1 + .../nav2_bt_navigator/bt_navigator.hpp | 2 ++ nav2_bt_navigator/src/bt_navigator.cpp | 2 +- .../nav2_core/behavior_tree_navigator.hpp | 19 ++++++++++++++++--- 4 files changed, 20 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 2b323a67985..546aa1c2e43 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -19,6 +19,7 @@ #include #include +#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_behavior_tree/behavior_tree_engine.hpp" #include "nav2_behavior_tree/ros_topic_logger.hpp" 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 2b552e1b028..5e12baba926 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -20,6 +20,7 @@ #include #include +#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/odometry_utils.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -92,6 +93,7 @@ class BtNavigator : public nav2_util::LifecycleNode // Odometry smoother object std::shared_ptr odom_smoother_; + std::shared_ptr logger_cout_; // Metrics for feedback std::string robot_frame_; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 8025a5e278e..e2f3a91b837 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -125,7 +125,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) navigators_.push_back(class_loader_.createUniqueInstance(navigator_type)); if (!navigators_.back()->on_configure( node, plugin_lib_names, feedback_utils, - &plugin_muxer_, odom_smoother_)) + &plugin_muxer_, odom_smoother_, logger_cout_)) { return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp index f6b8ecd62a4..b9018528bf0 100644 --- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp +++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp @@ -128,7 +128,8 @@ class NavigatorBase const std::vector & plugin_lib_names, const FeedbackUtils & feedback_utils, nav2_core::NavigatorMuxer * plugin_muxer, - std::shared_ptr odom_smoother) = 0; + std::shared_ptr odom_smoother, + std::shared_ptr logger_cout_) = 0; /** * @brief Activation of the navigator's backend BT and actions @@ -190,13 +191,19 @@ class BehaviorTreeNavigator : public NavigatorBase const std::vector & plugin_lib_names, const FeedbackUtils & feedback_utils, nav2_core::NavigatorMuxer * plugin_muxer, - std::shared_ptr odom_smoother) final + std::shared_ptr odom_smoother, + std::shared_ptr logger_cout) final { auto node = parent_node.lock(); logger_ = node->get_logger(); clock_ = node->get_clock(); feedback_utils_ = feedback_utils; plugin_muxer_ = plugin_muxer; + logger_cout_ = logger_cout; + if (!node->has_parameter("enable_cout_logger")) { + node->declare_parameter("enable_cout_logger", false); + } + enable_cout_logger_ = node->get_parameter("enable_cout_logger").as_bool(); // get the default behavior tree for this navigator std::string default_bt_xml_filename = getDefaultBTFilepath(parent_node); @@ -296,7 +303,10 @@ class BehaviorTreeNavigator : public NavigatorBase } bool goal_accepted = goalReceived(goal); - + logger_cout_.reset(); + logger_cout_ = std::make_shared(bt_action_server_->getTree()); + logger_cout_->enableTransitionToIdle(false); + logger_cout_->setEnabled(enable_cout_logger_); if (goal_accepted) { plugin_muxer_->startNavigating(getName()); } @@ -313,6 +323,7 @@ class BehaviorTreeNavigator : public NavigatorBase { plugin_muxer_->stopNavigating(getName()); goalCompleted(result, final_bt_status); + logger_cout_.reset(); } /** @@ -371,6 +382,8 @@ class BehaviorTreeNavigator : public NavigatorBase rclcpp::Clock::SharedPtr clock_; FeedbackUtils feedback_utils_; NavigatorMuxer * plugin_muxer_; + std::shared_ptr logger_cout_; + bool enable_cout_logger_; }; } // namespace nav2_core