From e2f0e40c1b47429772f0fce2638aa3de5fdf6dd8 Mon Sep 17 00:00:00 2001
From: Artem30801 <artem30801@gmail.com>
Date: Thu, 18 Apr 2019 21:04:15 +0300
Subject: [PATCH] Logging with ros fix

---
 Drone/client.py      |  4 +++-
 Drone/ros_logging.py | 23 +++++++++++++++++++++++
 2 files changed, 26 insertions(+), 1 deletion(-)
 create mode 100644 Drone/ros_logging.py

diff --git a/Drone/client.py b/Drone/client.py
index 418ea07..273f8f8 100644
--- a/Drone/client.py
+++ b/Drone/client.py
@@ -16,13 +16,15 @@ parent_dir = os.path.dirname(current_dir)
 sys.path.insert(0, parent_dir) 
 
 import messaging_lib as messaging
+import ros_logging
 
 logging.basicConfig(  # TODO all prints as logs
     level=logging.DEBUG, # INFO
     format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s",
     handlers=[
         logging.FileHandler("client_logs.log"),
-        logging.StreamHandler()
+        logging.StreamHandler(),
+        ros_logging.RosHandler(),
     ])
 
 ConfigOption = collections.namedtuple("ConfigOption", ["section", "option", "value"])
diff --git a/Drone/ros_logging.py b/Drone/ros_logging.py
new file mode 100644
index 0000000..12cb634
--- /dev/null
+++ b/Drone/ros_logging.py
@@ -0,0 +1,23 @@
+import logging
+import rospy
+
+
+class RosHandler(logging.Handler):
+
+    level_map = {
+        logging.DEBUG: rospy.logdebug,
+        logging.INFO: rospy.loginfo,
+        logging.WARNING: rospy.logwarn,
+        logging.ERROR: rospy.logerr,
+        logging.CRITICAL: rospy.logfatal
+    }
+
+    def emit(self, record):
+        try:
+            self.level_map[record.levelno]("%s: %s" % (record.name, record.msg))
+        except KeyError:
+            rospy.logerr("unknown log level %s LOG: %s: %s" % (record.levelno, record.name, record.msg))
+
+
+def route_logger_to_ros(logger_name):
+    logging.getLogger(logger_name).addHandler(RosHandler())