AGVFM-64 Behaviour tree: investigate debugging options (#42)

* add groot real-time monitor

* add bt navigator params

* add more logging options

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<BT::PublisherZMQ> groot_monitor_;
+  // This logger saves state changes on file
+  std::unique_ptr<BT::FileLogger> file_logger_;
+  // This logger prints state changes on console
+  std::unique_ptr<BT::StdCoutLogger> stdcout_logger_;
+  // This logger stores the execution time of each node
+  std::unique_ptr<BT::MinitraceLogger> minitrace_logger_;
 };
 
 }  // namespace nav2_behavior_tree

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<BT::PublisherZMQ>(*tree);
+}
+
+void BehaviorTreeEngine::resetGrootMonitoring()
+{
+    groot_monitor_.reset();
+}
+
+void BehaviorTreeEngine::addFileLogging(BT::Tree * tree) {
+    file_logger_ = std::make_unique<BT::FileLogger>(*tree, "/data/logs/bt_trace.fbl");
+}
+
+void BehaviorTreeEngine::resetFileLogging() {
+    file_logger_.reset();
+}
+
+void BehaviorTreeEngine::addStdCoutLogging(BT::Tree * tree) {
+    stdcout_logger_ = std::make_unique<BT::StdCoutLogger>(*tree);
+}
+
+void BehaviorTreeEngine::resetStdCoutLogging(){
+    stdcout_logger_.reset();
+}
+
+void BehaviorTreeEngine::addMinitraceLogging(BT::Tree * tree) {
+    minitrace_logger_ = std::make_unique<BT::MinitraceLogger>(*tree, "/data/logs/bt_trace.json");
+}
+
+void BehaviorTreeEngine::resetMinitraceLogging() {
+    minitrace_logger_.reset();
 }
 
 }  // namespace nav2_behavior_tree

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

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");