adding file logger

Signed-off-by: Christian Henkel <christian.henkel2@de.bosch.com>
ct2034 · Feb 21, 2023 · b019fbc · b019fbc
1 parent 2de3f92
commit b019fbc
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 0 deletions.
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
@@ -23,6 +23,7 @@
 #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_file_logger.h>
 #include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
 
 
@@ -101,6 +102,15 @@ class BehaviorTreeEngine
    */
   void resetGrootMonitor();
 
+  /**
+   * @brief Function to add a File logger to the BT
+   * @param tree BT to add logger to
+   * @param file_name Name of the log file
+   */
+  void addFileLogger(
+    BT::Tree * tree,
+    const std::string & file_name);
+
   /**
    * @brief Function to explicitly reset all BT nodes to initial state
    * @param root_node Pointer to BT root node
@@ -112,6 +122,7 @@ class BehaviorTreeEngine
   BT::BehaviorTreeFactory factory_;
 
   static inline std::unique_ptr<BT::PublisherZMQ> groot_monitor_;
+  static inline std::unique_ptr<BT::FileLogger> file_logger_;
 };
 
 }  // namespace nav2_behavior_tree

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
@@ -236,6 +236,10 @@ class BtActionServer
   int groot_zmq_publisher_port_;
   int groot_zmq_server_port_;
 
+  // Parameters for BT logging
+  bool enable_fbl_logging_;
+  std::string fbl_log_filename_;
+
   // User-provided callbacks
   OnGoalReceivedCallback on_goal_received_callback_;
   OnLoopCallback on_loop_callback_;

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
@@ -68,6 +68,12 @@ BtActionServer<ActionT>::BtActionServer(
   if (!node->has_parameter("groot_zmq_server_port")) {
     node->declare_parameter("groot_zmq_server_port", 1667);
   }
+  if (!node->has_parameter("enable_fbl_logging")) {
+    node->declare_parameter("enable_fbl_logging", false);
+  }
+  if (!node->has_parameter("fbl_log_filename")) {
+    node->declare_parameter("fbl_log_filename", "/tmp/bt_trace.fbl");
+  }
 }
 
 template<class ActionT>
@@ -107,6 +113,10 @@ bool BtActionServer<ActionT>::on_configure()
   node->get_parameter("groot_zmq_publisher_port", groot_zmq_publisher_port_);
   node->get_parameter("groot_zmq_server_port", groot_zmq_server_port_);
 
+  // Get parameter for logging to fbl file
+  node->get_parameter("enable_fbl_logging", enable_fbl_logging_);
+  node->get_parameter("fbl_log_filename", fbl_log_filename_);
+
   // Get parameters for BT timeouts
   int timeout;
   node->get_parameter("bt_loop_duration", timeout);
@@ -204,6 +214,11 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
     }
   }
 
+  // Enable logging to file
+  if (enable_fbl_logging_) {
+    bt_->addFileLogger(&tree_, "/tmp/bt_trace.fbl");
+  }
+
   return true;
 }
 

diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp
@@ -104,6 +104,14 @@ BehaviorTreeEngine::resetGrootMonitor()
   }
 }
 
+void
+BehaviorTreeEngine::addFileLogger(
+    BT::Tree * tree,
+    const std::string & file_name)
+{
+  file_logger_ = std::make_unique<BT::FileLogger>(*tree, file_name.c_str());
+}
+
 // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
 void
 BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node)