From b7f9609e60bd234f15b253901eaa1c3e462cc5b1 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Fri, 2 Mar 2018 23:09:24 +0100
Subject: [PATCH] Set logging timestamp

Needs ROS_PYTHON_LOG_CONFIG_FILE variable, set with e.g.

export ROS_PYTHON_LOG_CONFIG_FILE=$(rospack find wild_thumper)/config/python_logging.conf
---
 CMakeLists.txt                  |  4 ++--
 config/python_logging.conf      | 35 +++++++++++++++++++++++++++++
 package.xml                     |  2 ++
 setup.py                        | 12 ++++++++++
 src/wild_thumper/__init__.py    |  0
 src/wild_thumper/ros_logging.py | 39 +++++++++++++++++++++++++++++++++
 6 files changed, 90 insertions(+), 2 deletions(-)
 create mode 100644 config/python_logging.conf
 create mode 100755 setup.py
 create mode 100644 src/wild_thumper/__init__.py
 create mode 100644 src/wild_thumper/ros_logging.py

diff --git a/CMakeLists.txt b/CMakeLists.txt
index a46fdd1..9dc330d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,7 +4,7 @@ project(wild_thumper)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED sensor_msgs cv_bridge roscpp std_msgs image_transport dynamic_reconfigure message_generation)
+find_package(catkin REQUIRED sensor_msgs cv_bridge roscpp rospy std_msgs image_transport dynamic_reconfigure message_generation)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -13,7 +13,7 @@ find_package(catkin REQUIRED sensor_msgs cv_bridge roscpp std_msgs image_transpo
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
diff --git a/config/python_logging.conf b/config/python_logging.conf
new file mode 100644
index 0000000..9240734
--- /dev/null
+++ b/config/python_logging.conf
@@ -0,0 +1,35 @@
+[loggers]
+keys=root, rosout
+
+[handlers]
+keys=streamHandler
+
+[formatters]
+keys=defaultFormatter
+
+[logger_root]
+level=INFO
+handlers=streamHandler
+
+[logger_rosout]
+level=INFO
+handlers=streamHandler
+propagate=1
+qualname=rosout
+
+[handler_fileHandler]
+class=handlers.RotatingFileHandler
+level=DEBUG
+formatter=defaultFormatter
+# log filename, mode, maxBytes, backupCount
+args=(os.environ['ROS_LOG_FILENAME'],'a', 50000000, 4)
+
+[handler_streamHandler]
+class=wild_thumper.ros_logging.RosStreamHandler
+level=DEBUG
+formatter=defaultFormatter
+# colorize output flag
+args=(True,)
+
+[formatter_defaultFormatter]
+format=[%(name)s][%(levelname)s] %(asctime)s: %(message)s
diff --git a/package.xml b/package.xml
index 0bf3ca2..4f4bc77 100644
--- a/package.xml
+++ b/package.xml
@@ -43,10 +43,12 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>cv_bridge</build_depend>
   <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>openni2_camera</build_depend>
   <build_depend>message_generation</build_depend>
+  <run_depend>rospy</run_depend>
   <run_depend>hector_sensors_description</run_depend>
   <run_depend>openni2_launch</run_depend>
   <run_depend>depthimage_to_laserscan</run_depend>
diff --git a/setup.py b/setup.py
new file mode 100755
index 0000000..26a61a7
--- /dev/null
+++ b/setup.py
@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['wild_thumper'],
+    package_dir={'': 'src'}
+)
+
+setup(**d)
diff --git a/src/wild_thumper/__init__.py b/src/wild_thumper/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/wild_thumper/ros_logging.py b/src/wild_thumper/ros_logging.py
new file mode 100644
index 0000000..a8df113
--- /dev/null
+++ b/src/wild_thumper/ros_logging.py
@@ -0,0 +1,39 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+#
+# see https://answers.ros.org/question/174892/ros-log-file-change-of-time-stamping-format/
+
+import os
+import time
+import logging
+import sys
+import datetime
+from rosgraph.roslogging import RosStreamHandler as _RosStreamHandler, _logging_to_rospy_names, _defaultFormatter
+
+
+class RosStreamHandler(_RosStreamHandler):
+	def emit(self, record):
+		level, color = _logging_to_rospy_names[record.levelname]
+		record_message = _defaultFormatter.format(record)
+		msg = os.environ.get('ROSCONSOLE_FORMAT', '[${severity}] [${time}]: ${message}')
+		msg = msg.replace('${severity}', level)
+		msg = msg.replace('${message}', str(record_message))
+		msg = msg.replace('${walltime}', '%f' % time.time())
+		msg = msg.replace('${thread}', str(record.thread))
+		msg = msg.replace('${logger}', str(record.name))
+		msg = msg.replace('${file}', str(record.pathname))
+		msg = msg.replace('${line}', str(record.lineno))
+		msg = msg.replace('${function}', str(record.funcName))
+		try:
+			from rospy import get_name
+			node_name = get_name()
+		except ImportError:
+			node_name = '<unknown_node_name>'
+		msg = msg.replace('${node}', node_name)
+		time_str = datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
+		msg = msg.replace('${time}', time_str)
+		msg += '\n'
+		if record.levelno < logging.WARNING:
+			self._write(sys.stdout, msg, color)
+		else:
+			self._write(sys.stderr, msg, color)
-- 
2.39.2