From 16637974995beec36f53d489a9bae8a8a1f41837 Mon Sep 17 00:00:00 2001
From: Dylan Zemlin <dylan.zemlin@gmail.com>
Date: Wed, 17 Apr 2024 19:45:32 -0500
Subject: [PATCH] Added log function to node

---
 autonav_ws/src/scr/include/scr/constants.hpp |  1 +
 autonav_ws/src/scr/include/scr/node.hpp      |  3 +++
 autonav_ws/src/scr/scr/constants.py          |  1 +
 autonav_ws/src/scr/scr/node.py               | 20 +++++++++++++++-----
 autonav_ws/src/scr/src/node.cpp              |  9 +++++++++
 5 files changed, 29 insertions(+), 5 deletions(-)

diff --git a/autonav_ws/src/scr/include/scr/constants.hpp b/autonav_ws/src/scr/include/scr/constants.hpp
index ad2de722..34f17769 100644
--- a/autonav_ws/src/scr/include/scr/constants.hpp
+++ b/autonav_ws/src/scr/include/scr/constants.hpp
@@ -11,6 +11,7 @@ namespace SCR
             const std::string DEVICE_STATE = "/scr/device_state";
             const std::string CONFIG_UPDATE = "/scr/config_updated";
             const std::string PERFORMANCE_TRACK = "/scr/performance";
+            const std::string LOGGING = "/scr/logging";
         }
 
         namespace Services
diff --git a/autonav_ws/src/scr/include/scr/node.hpp b/autonav_ws/src/scr/include/scr/node.hpp
index 9917f694..b408c08a 100644
--- a/autonav_ws/src/scr/include/scr/node.hpp
+++ b/autonav_ws/src/scr/include/scr/node.hpp
@@ -8,6 +8,7 @@
 #include "scr_msgs/msg/config_updated.hpp"
 #include "scr_msgs/msg/device_state.hpp"
 #include "scr_msgs/msg/system_state.hpp"
+#include "scr_msgs/msg/log.hpp"
 #include "states.hpp"
 #include "structs.hpp"
 #include "constants.hpp"
@@ -28,6 +29,7 @@ namespace SCR
     struct NodePublishers
     {
         rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr performance_track;
+        rclcpp::Publisher<scr_msgs::msg::Log>::SharedPtr logging;
     };
 
     struct NodeClients
@@ -104,6 +106,7 @@ namespace SCR
         void device_state_callback(const scr_msgs::msg::DeviceState msg);
         void config_updated_callback(const scr_msgs::msg::ConfigUpdated msg);
         void set_system_total_state(SCR::SystemState state, SCR::SystemMode mode, bool mobility);
+        void log(std::string data);
 
     protected:
         /// @brief The current system mode
diff --git a/autonav_ws/src/scr/scr/constants.py b/autonav_ws/src/scr/scr/constants.py
index c4ef9fab..ab66edaf 100644
--- a/autonav_ws/src/scr/scr/constants.py
+++ b/autonav_ws/src/scr/scr/constants.py
@@ -3,6 +3,7 @@ class Topics:
 	DEVICE_STATE = "/scr/device_state"
 	CONFIG_UPDATE = "/scr/config_updated"
 	PERFORMANCE_TRACK = "/scr/performance"
+	LOGGING = "/scr/logging"
 
 class Services:
 	SYSTEM_STATE = "/scr/system_state_client"
diff --git a/autonav_ws/src/scr/scr/node.py b/autonav_ws/src/scr/scr/node.py
index 3f82247c..71879696 100644
--- a/autonav_ws/src/scr/scr/node.py
+++ b/autonav_ws/src/scr/scr/node.py
@@ -1,7 +1,7 @@
 import threading
 from scr.states import DeviceStateEnum, SystemStateEnum, SystemModeEnum
 from scr_msgs.srv import UpdateDeviceState, UpdateSystemState, UpdateConfig
-from scr_msgs.msg import DeviceState, SystemState, ConfigUpdated
+from scr_msgs.msg import DeviceState, SystemState, ConfigUpdated, Log
 from std_msgs.msg import Float64
 from rclpy.node import Node as ROSNode
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
@@ -36,6 +36,7 @@ def __init__(self, node_name):
         self.config_updated_client = self.create_client(UpdateConfig, scr.constants.Services.CONFIG_UPDATE, callback_group=self.config_updated_callback_group)
         
         self.performance_publisher = self.create_publisher(Float64, scr.constants.Topics.PERFORMANCE_TRACK, 10)
+        self.logging_publisher = self.create_publisher(Log, scr.constants.Topics.LOGGING, 10)
 
         self.device_state = DeviceStateEnum.OFF
         self.system_state = SystemStateEnum.DISABLED
@@ -60,6 +61,18 @@ def jdump(self, obj):
             return json.dumps(obj.__dict__)
         else:
             return json.dumps(obj)
+        
+    def log(self, data):
+        """
+        Logs a message to the logging topic.
+
+        :param data: The message to log.
+        """
+
+        log_packet = Log()
+        log_packet.data = data
+        log_packet.node = self.identifier
+        self.logging_publisher.publish(log_packet)
 
     def on_device_state(self, msg: DeviceState):
         """
@@ -282,7 +295,4 @@ def run_nodes(nodes):
             executor.add_node(node)
         executor.spin()
         for node in nodes:
-            executor.remove_node(node)
-
-    def log(self, message: str):
-        rclpy.logging.get_logger("scr." + self.identifier).info(message)
+            executor.remove_node(node)
\ No newline at end of file
diff --git a/autonav_ws/src/scr/src/node.cpp b/autonav_ws/src/scr/src/node.cpp
index 8124c5fd..cd033467 100644
--- a/autonav_ws/src/scr/src/node.cpp
+++ b/autonav_ws/src/scr/src/node.cpp
@@ -19,6 +19,7 @@ namespace SCR
 
         // Create publishers
         publishers.performance_track = this->create_publisher<std_msgs::msg::Float64>(Constants::Topics::PERFORMANCE_TRACK, 10);
+        publishers.logging = this->create_publisher<scr_msgs::msg::Log>(Constants::Topics::LOGGING, 10);
 
         // Create clients
         clients.system_state = this->create_client<scr_msgs::srv::UpdateSystemState>(Constants::Services::SYSTEM_STATE, rmw_qos_profile_services_default, callback_groups.system_state);
@@ -37,6 +38,14 @@ namespace SCR
     {
     }
 
+    void Node::log(std::string data)
+    {
+        scr_msgs::msg::Log msg;
+        msg.node = identifier;
+        msg.data = data;
+        publishers.logging->publish(msg);
+    }
+
     void Node::system_state_callback(const scr_msgs::msg::SystemState msg)
     {
         scr_msgs::msg::SystemState oldState;