Add ROS2 logger

behaviortree_ros2/CMakeLists.txt

@@ -27,7 +27,8 @@ generate_parameter_library(
 add_library(${PROJECT_NAME}
     src/bt_ros2.cpp
     src/bt_utils.cpp
-    src/tree_execution_server.cpp )
+    src/tree_execution_server.cpp
+    src/bt_ros_logger.cpp )
 
 ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS})
 

behaviortree_ros2/include/behaviortree_ros2/bt_ros_logger.hpp

@@ -0,0 +1,32 @@
+#ifndef BT_ROS_LOGGER_H
+#define BT_ROS_LOGGER_H
+
+#include <cstring>
+#include <rclcpp/executors.hpp>
+#include <rclcpp/allocator/allocator_common.hpp>
+#include <rclcpp/logging.hpp>
+#include "behaviortree_cpp/loggers/abstract_logger.h"
+
+namespace BT
+{
+/**
+ * @brief RosLogger is a very simple logger that
+ * displays all the transitions on the console.
+ */
+
+class RosLogger : public StatusChangeLogger
+{
+public:
+  RosLogger(const BT::Tree& tree, std::shared_ptr<rclcpp::Node> node);
+  ~RosLogger() override;
+
+  virtual void flush() override;
+
+private:
+  virtual void callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
+                        NodeStatus status) override;
+  std::weak_ptr<rclcpp::Node> node_;
+};
+}  // namespace BT
+
+#endif  // BT_ROS_LOGGER_H

behaviortree_ros2/src/bt_ros_logger.cpp

@@ -0,0 +1,38 @@
+#include "behaviortree_ros2/bt_ros_logger.hpp"
+
+namespace BT
+{
+
+RosLogger::RosLogger(const BT::Tree& tree, std::shared_ptr<rclcpp::Node> node) : StatusChangeLogger(tree.rootNode()), node_(node)
+{}
+RosLogger::~RosLogger()
+{}
+
+void RosLogger::callback(Duration timestamp, const TreeNode& node,
+                             NodeStatus prev_status, NodeStatus status)
+{
+  using namespace std::chrono;
+
+  // get ros node pointer
+  auto ros_node = node_.lock();
+
+  if (ros_node) {
+
+    constexpr const char* whitespaces = "                         ";
+    constexpr const size_t ws_count = 25;
+
+    double since_epoch = duration<double>(timestamp).count();
+
+    RCLCPP_DEBUG(
+      ros_node->get_logger(), "[%.3f]: %s%s %s -> %s",
+      since_epoch, node.name().c_str(),
+          &whitespaces[std::min(ws_count, node.name().size())],
+          toStr(prev_status, true).c_str(), toStr(status, true).c_str());
+  }
+}
+
+void RosLogger::flush()
+{
+}
+
+}  // namespace BT