From ed4ccbbac09e712c1afeee025f38262fd0ed57f2 Mon Sep 17 00:00:00 2001
From: Felipe Inostroza <pepeinostroza@gmail.com>
Date: Tue, 9 May 2023 15:55:34 -0400
Subject: [PATCH]  Added  a ros topic logger

---
 .../loggers/ros_topic_logger.h                | 36 +++++++++++
 msg/StatusChange.msg                          |  1 +
 msg/StatusChangeLog.msg                       |  2 +-
 src/loggers/ros_topic_logger.cpp              | 59 +++++++++++++++++++
 4 files changed, 97 insertions(+), 1 deletion(-)
 create mode 100644 include/behaviortree_ros/loggers/ros_topic_logger.h
 create mode 100644 src/loggers/ros_topic_logger.cpp

diff --git a/include/behaviortree_ros/loggers/ros_topic_logger.h b/include/behaviortree_ros/loggers/ros_topic_logger.h
new file mode 100644
index 0000000..02d1194
--- /dev/null
+++ b/include/behaviortree_ros/loggers/ros_topic_logger.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <behaviortree_cpp_v3/loggers/abstract_logger.h>
+#include <ros/ros.h>
+#include <behaviortree_ros/StatusChangeLog.h>
+#include <behaviortree_ros/StatusChange.h>
+
+
+namespace BT
+{
+
+class RosTopicLogger : public StatusChangeLogger
+{
+    static std::atomic<bool> ref_count;
+
+  public:
+    RosTopicLogger(TreeNode* root_node, ros::NodeHandle nh, const std::string topic_name = "behavior_tree_log");
+
+
+    ~RosTopicLogger() override;
+
+    virtual void callback(Duration timestamp,
+                          const TreeNode& node,
+                          NodeStatus prev_status,
+                          NodeStatus status) override;
+
+    virtual void flush() override;
+
+private:
+    ros::NodeHandle nh_;
+    std::string topic_name_;
+    ros::Publisher status_change_pub_;
+    std::vector<behaviortree_ros::StatusChange> event_log_;
+};
+
+}   // end namespace
\ No newline at end of file
diff --git a/msg/StatusChange.msg b/msg/StatusChange.msg
index 1af72a5..66a2ea0 100644
--- a/msg/StatusChange.msg
+++ b/msg/StatusChange.msg
@@ -1,4 +1,5 @@
 uint16      uid       
+string      name
 NodeStatus  prev_status 
 NodeStatus  status
 time        timestamp
diff --git a/msg/StatusChangeLog.msg b/msg/StatusChangeLog.msg
index 594d592..1c48f23 100644
--- a/msg/StatusChangeLog.msg
+++ b/msg/StatusChangeLog.msg
@@ -1,3 +1,3 @@
 
-BehaviorTree    behavior_tree
+
 StatusChange[]  state_changes
diff --git a/src/loggers/ros_topic_logger.cpp b/src/loggers/ros_topic_logger.cpp
new file mode 100644
index 0000000..7b653ae
--- /dev/null
+++ b/src/loggers/ros_topic_logger.cpp
@@ -0,0 +1,59 @@
+#include "behaviortree_ros/loggers/ros_topic_logger.h"
+
+namespace BT
+{
+std::atomic<bool> RosTopicLogger::ref_count(false);
+
+RosTopicLogger::RosTopicLogger(TreeNode* root_node, ros::NodeHandle nh, const std::string topic_name) :
+    StatusChangeLogger(root_node),
+    nh_(nh),
+    topic_name_(topic_name)
+{
+    bool expected = false;
+    if (!ref_count.compare_exchange_strong(expected, true))
+    {
+        throw std::logic_error("Only a single instance of RosTopicLogger shall be created");
+    }
+    status_change_pub_ = nh_.advertise<behaviortree_ros::StatusChangeLog>(topic_name_, 5);
+
+}
+
+
+
+RosTopicLogger::~RosTopicLogger()
+{
+    ref_count.store(false);
+}
+
+void RosTopicLogger::callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
+                             NodeStatus status)
+{
+
+    behaviortree_ros::StatusChange event;
+
+    // BT timestamps are a duration since the epoch. Need to convert to a time_point
+    // before converting to a msg.
+    
+    uint32_t sec = std::chrono::duration_cast<std::chrono::seconds>(timestamp).count();
+    auto remainder = timestamp - std::chrono::duration_cast<std::chrono::seconds>(timestamp);
+    uint32_t nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(remainder).count() ;
+    event.timestamp = ros::Time(sec, nsec);
+    event.uid = node.UID();
+    event.name = node.name();
+    event.prev_status.value = static_cast<int8_t> (prev_status);
+    event.status.value =  static_cast<int8_t> (status);
+    event_log_.push_back(std::move(event));
+    
+}
+
+void RosTopicLogger::flush()
+{
+    if (!event_log_.empty()){
+        behaviortree_ros::StatusChangeLog log_msg;
+        log_msg.state_changes = std::move(event_log_);
+        status_change_pub_.publish(log_msg);
+        event_log_.clear();
+    }
+}
+
+}   // end namespace