From e0cb90c8da2e926bc5bf39f29f926f29aa5a8549 Mon Sep 17 00:00:00 2001 From: Greg Gallagher Date: Thu, 1 Dec 2016 17:34:30 -0500 Subject: [PATCH] Add named loggers to rospy, this change will create (named) child loggers under rospy.rosout. This allows for each named logger to have seperate verbosity setting, this will allow for two different verboisty settings for logging in a single node. Two named loggers can log to rosout at two different verbosity setings, which can help noisy debug logs. --- clients/rospy/src/rospy/core.py | 173 +++++++++++------- .../test/rostest/test_rospy_client_online.py | 18 ++ test/test_rospy/test/unit/test_rospy_core.py | 8 +- 3 files changed, 127 insertions(+), 72 deletions(-) diff --git a/clients/rospy/src/rospy/core.py b/clients/rospy/src/rospy/core.py index 57c07b8367..1b792aaa18 100644 --- a/clients/rospy/src/rospy/core.py +++ b/clients/rospy/src/rospy/core.py @@ -37,11 +37,11 @@ import atexit -try: - import cPickle as pickle -except ImportError: - import pickle -import inspect +try: + import cPickle as pickle +except ImportError: + import pickle +import inspect import logging import os import signal @@ -72,6 +72,8 @@ from rospy.impl.validators import ParameterInvalid from rosgraph_msgs.msg import Log +from functools import partial +from collections import namedtuple _logger = logging.getLogger("rospy.core") @@ -143,78 +145,107 @@ def rospywarn(msg, *args): """Internal rospy client library warn logging""" _rospy_logger.warn(msg, *args) -logdebug = logging.getLogger('rosout').debug -logwarn = logging.getLogger('rosout').warning +def _base_logger(msg, *args, **kwargs): + + try: + name = kwargs.pop('logger_name') + except KeyError: + name = None + + try: + throttle = kwargs.pop('logger_throttle') + except KeyError: + throttle = None + + try: + level = kwargs.pop('logger_level') + except KeyError: + level = None + + rospy_logger = logging.getLogger('rosout') + if name: + rospy_logger = rospy_logger.getChild(name) + logfunc = getattr(rospy_logger, level) + + if throttle: + caller_id = _frame_record_to_caller_id(inspect.stack()[2]) + _logging_throttle(caller_id, logfunc, throttle, msg, *args) + else: + logfunc(msg, *args) + + +loginfo = partial(_base_logger, logger_level='info') -loginfo = logging.getLogger('rosout').info logout = loginfo # alias deprecated name -logerr = logging.getLogger('rosout').error +logdebug = partial(_base_logger, logger_level='debug') + +logwarn = partial(_base_logger, logger_level='warn') + +logerr = partial(_base_logger, logger_level='error') + logerror = logerr # alias logerr -logfatal = logging.getLogger('rosout').critical - - -class LoggingThrottle(object): - - last_logging_time_table = {} - - def __call__(self, caller_id, logging_func, period, msg): - """Do logging specified message periodically. - - - caller_id (str): Id to identify the caller - - logging_func (function): Function to do logging. - - period (float): Period to do logging in second unit. - - msg (object): Message to do logging. - """ - now = rospy.Time.now() - - last_logging_time = self.last_logging_time_table.get(caller_id) - - if (last_logging_time is None or - (now - last_logging_time) > rospy.Duration(period)): - logging_func(msg) - self.last_logging_time_table[caller_id] = now - - -_logging_throttle = LoggingThrottle() - - -def _frame_record_to_caller_id(frame_record): - frame, _, lineno, _, code, _ = frame_record - caller_id = ( - inspect.getabsfile(frame), - lineno, - frame.f_lasti, - ) - return pickle.dumps(caller_id) - - -def logdebug_throttle(period, msg): - caller_id = _frame_record_to_caller_id(inspect.stack()[1]) - _logging_throttle(caller_id, logdebug, period, msg) - - -def loginfo_throttle(period, msg): - caller_id = _frame_record_to_caller_id(inspect.stack()[1]) - _logging_throttle(caller_id, loginfo, period, msg) - - -def logwarn_throttle(period, msg): - caller_id = _frame_record_to_caller_id(inspect.stack()[1]) - _logging_throttle(caller_id, logwarn, period, msg) - - -def logerr_throttle(period, msg): - caller_id = _frame_record_to_caller_id(inspect.stack()[1]) - _logging_throttle(caller_id, logerr, period, msg) - - -def logfatal_throttle(period, msg): - caller_id = _frame_record_to_caller_id(inspect.stack()[1]) - _logging_throttle(caller_id, logfatal, period, msg) - +logfatal = partial(_base_logger, logger_level='critical') + + +class LoggingThrottle(object): + + LogEntry = namedtuple("LogEntry", "time digest") + + last_log_entry = {} + + def __call__(self, caller_id, logging_func, period, msg, *args): + """Do logging specified message periodically. Messages with different contents will bypass throttling + + - caller_id (str): Id to identify the caller + - logging_func (function): Function to do logging. + - period (float): Period to do logging in seconds. + - msg (object): Message to do logging. + """ + now = rospy.Time.now() + + last = self.last_log_entry.get(caller_id, self.LogEntry(time=None, digest=None)) + digest = hash(*args + tuple([str(msg)])) + + if (last.time is None or (now - last.time) > rospy.Duration(period) or digest != last.digest): + logging_func(msg, *args) + self.last_log_entry[caller_id] = self.LogEntry(time=now, digest=digest) + + +_logging_throttle = LoggingThrottle() + + +def _frame_record_to_caller_id(frame_record): + frame, _, lineno, _, code, _ = frame_record + caller_id = ( + inspect.getabsfile(frame), + lineno, + frame.f_lasti, + ) + return pickle.dumps(caller_id) + + +def logdebug_throttle(period, msg): + logdebug(msg, logger_name=None, logger_throttle=period) + + +def loginfo_throttle(period, msg): + loginfo(msg, logger_name=None, logger_throttle=period) + + +def logwarn_throttle(period, msg): + logwarn(msg, logger_name=None, logger_throttle=period) + + +def logerr_throttle(period, msg): + logerr(msg, logger_name=None, logger_throttle=period) + + +def logfatal_throttle(period, msg): + logfatal(msg, logger_name=None, logger_throttle=period) + ######################################################### # CONSTANTS diff --git a/test/test_rospy/test/rostest/test_rospy_client_online.py b/test/test_rospy/test/rostest/test_rospy_client_online.py index 028313de83..52f60d11e8 100755 --- a/test/test_rospy/test/rostest/test_rospy_client_online.py +++ b/test/test_rospy/test/rostest/test_rospy_client_online.py @@ -149,6 +149,24 @@ def test_log(self): rospy.sleep(rospy.Duration(2)) else: self.assert_("test 4" in sys.stderr.getvalue()) + + rospy.loginfo("test child logger 1", logger_name="log1") + self.assert_("test child logger 1" in sys.stdout.getvalue()) + + rospy.logwarn("test child logger 2", logger_name="log2") + self.assert_("[WARN]" in sys.stderr.getvalue()) + self.assert_("test child logger 2" in sys.stderr.getvalue()) + + sys.stderr = StringIO() + rospy.logerr("test child logger 3", logger_name="log3") + self.assert_("[ERROR]" in sys.stderr.getvalue()) + self.assert_("test child logger 3" in sys.stderr.getvalue()) + + sys.stderr = StringIO() + rospy.logfatal("test child logger 4", logger_name="log4") + self.assert_("[FATAL]" in sys.stderr.getvalue()) + self.assert_("test child logger 4" in sys.stderr.getvalue()) + finally: sys.stdout = real_stdout sys.stderr = real_stderr diff --git a/test/test_rospy/test/unit/test_rospy_core.py b/test/test_rospy/test/unit/test_rospy_core.py index bb0375ab43..d1889b8c0e 100644 --- a/test/test_rospy/test/unit/test_rospy_core.py +++ b/test/test_rospy/test/unit/test_rospy_core.py @@ -79,7 +79,13 @@ def test_loggers(self): rospy.logout('out') rospy.logerr('err') rospy.logfatal('fatal') - + #basic named logger test + rospy.logdebug('debug', logger_name="child1") + rospy.logwarn('warn', logger_name="child1") + rospy.logout('out', logger_name="child1") + rospy.logerr('err', logger_name="child1") + rospy.logfatal('fatal', logger_name="child1") + def test_add_shutdown_hook(self): def handle(reason): pass