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 ca627bbe7e..1b06bd21d5 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 @@ -22,6 +22,10 @@ #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" #include "behaviortree_cpp_v3/xml_parsing.h" +#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h" +#include "behaviortree_cpp_v3/loggers/bt_minitrace_logger.h" +#include "behaviortree_cpp_v3/loggers/bt_file_logger.h" +#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" namespace nav2_behavior_tree { @@ -44,6 +48,22 @@ class BehaviorTreeEngine const std::string & xml_string, BT::Blackboard::Ptr blackboard); + void addGrootMonitoring(BT::Tree * tree); + + void resetGrootMonitoring(); + + void addFileLogging(BT::Tree * tree); + + void resetFileLogging(); + + void addStdCoutLogging(BT::Tree * tree); + + void resetStdCoutLogging(); + + void addMinitraceLogging(BT::Tree * tree); + + void resetMinitraceLogging(); + // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state void haltAllActions(BT::TreeNode * root_node) { @@ -62,6 +82,14 @@ class BehaviorTreeEngine protected: // The factory that will be used to dynamically construct the behavior tree BT::BehaviorTreeFactory factory_; + // This logger publish status changes using ZeroMQ. Used by Groot + std::unique_ptr groot_monitor_; + // This logger saves state changes on file + std::unique_ptr file_logger_; + // This logger prints state changes on console + std::unique_ptr stdcout_logger_; + // This logger stores the execution time of each node + std::unique_ptr minitrace_logger_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 36b852e61c..c9a84490ce 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -66,9 +66,41 @@ BehaviorTreeEngine::buildTreeFromText( const std::string & xml_string, BT::Blackboard::Ptr blackboard) { - BT::XMLParser p(factory_); - p.loadFromText(xml_string); - return p.instantiateTree(blackboard); + return factory_.createTreeFromText(xml_string, blackboard); +} + +void BehaviorTreeEngine::addGrootMonitoring(BT::Tree * tree) +{ + groot_monitor_ = std::make_unique(*tree); +} + +void BehaviorTreeEngine::resetGrootMonitoring() +{ + groot_monitor_.reset(); +} + +void BehaviorTreeEngine::addFileLogging(BT::Tree * tree) { + file_logger_ = std::make_unique(*tree, "/data/logs/bt_trace.fbl"); +} + +void BehaviorTreeEngine::resetFileLogging() { + file_logger_.reset(); +} + +void BehaviorTreeEngine::addStdCoutLogging(BT::Tree * tree) { + stdcout_logger_ = std::make_unique(*tree); +} + +void BehaviorTreeEngine::resetStdCoutLogging(){ + stdcout_logger_.reset(); +} + +void BehaviorTreeEngine::addMinitraceLogging(BT::Tree * tree) { + minitrace_logger_ = std::make_unique(*tree, "/data/logs/bt_trace.json"); +} + +void BehaviorTreeEngine::resetMinitraceLogging() { + minitrace_logger_.reset(); } } // namespace nav2_behavior_tree diff --git a/nav2_bringup/bringup/params/forklift_params.yaml b/nav2_bringup/bringup/params/forklift_params.yaml index 37acfb6d5a..4aa24b59e6 100644 --- a/nav2_bringup/bringup/params/forklift_params.yaml +++ b/nav2_bringup/bringup/params/forklift_params.yaml @@ -386,6 +386,10 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom + enable_groot_monitoring: True + enable_stdcout_logging: True + enable_file_logging: False + enable_minitrace_logging: False default_bt_xml_filename: "/code/ros2_ws/src/navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning_v1.xml" plugin_lib_names: - nav2_adjust_pallet_goal_action_bt_node diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 904158acf2..137e53ad9a 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -75,6 +75,10 @@ BtNavigator::BtNavigator() declare_parameter("global_frame", std::string("map")); declare_parameter("robot_base_frame", std::string("base_link")); declare_parameter("odom_topic", std::string("odom")); + declare_parameter("enable_groot_monitoring", true); + declare_parameter("enable_stdcout_logging", true); + declare_parameter("enable_file_logging", false); + declare_parameter("enable_minitrace_logging", false); } BtNavigator::~BtNavigator() @@ -155,7 +159,10 @@ BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename) if (current_bt_xml_filename_ == bt_xml_filename) { return true; } - + bt_->resetGrootMonitoring(); + bt_->resetFileLogging(); + bt_->resetStdCoutLogging(); + bt_->resetMinitraceLogging(); // Read the input BT XML from the specified file into a string std::ifstream xml_file(bt_xml_filename); @@ -175,6 +182,18 @@ BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename) tree_ = bt_->buildTreeFromText(xml_string, blackboard_); current_bt_xml_filename_ = bt_xml_filename; + if (get_parameter("enable_groot_monitoring").as_bool()) { + bt_->addGrootMonitoring(&tree_); + } + if (get_parameter("enable_stdcout_logging").as_bool()) { + bt_->addStdCoutLogging(&tree_); + } + if (get_parameter("enable_file_logging").as_bool()) { + bt_->addFileLogging(&tree_); + } + if (get_parameter("enable_minitrace_logging").as_bool()) { + bt_->addMinitraceLogging(&tree_); + } return true; } @@ -218,6 +237,10 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) current_bt_xml_filename_.clear(); blackboard_.reset(); bt_->haltAllActions(tree_.rootNode()); + bt_->resetGrootMonitoring(); + bt_->resetFileLogging(); + bt_->resetStdCoutLogging(); + bt_->resetMinitraceLogging(); bt_.reset(); RCLCPP_INFO(get_logger(), "Completed Cleaning up");