nao_logger.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Copyright (C) 2014 Aldebaran Robotics
00003 #
00004 # Licensed under the Apache License, Version 2.0 (the "License");
00005 # you may not use this file except in compliance with the License.
00006 # You may obtain a copy of the License at
00007 #
00008 #     http://www.apache.org/licenses/LICENSE-2.0
00009 #
00010 # Unless required by applicable law or agreed to in writing, software
00011 # distributed under the License is distributed on an "AS IS" BASIS,
00012 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013 # See the License for the specific language governing permissions and
00014 # limitations under the License.
00015 #
00016 
00017 import logging
00018 
00019 from rosgraph_msgs.msg import Log
00020 import rospy
00021 
00022 from nao_driver.nao_driver_naoqi import NaoNode
00023 
00024 import qi
00025 
00026 # level 0 is actually 'silent' in NAOqi and 5 is 'verbose' but we adapt to the ROS ones
00027 LEVELS = [Log.DEBUG, Log.FATAL, Log.ERROR, Log.WARN, Log.INFO, Log.DEBUG, Log.DEBUG]
00028 try:
00029     ROSOUT_PUB = rospy.topics.Publisher('/rosout', Log, latch=True, queue_size=100)
00030 except:
00031     # Groovy compatible code
00032     ROSOUT_PUB = rospy.topics.Publisher('/rosout', Log, latch=True)
00033 
00034 def onMessageCallback(msg):
00035     """
00036     Given a NAOqi dict message, publish it in /rosout
00037     :param msg: dict corresponding to a NAOqi log message
00038     """
00039     file, function, line = msg['source'].split(':')
00040     if line:
00041         line = int(line)
00042     # adapt the category to what a topic is (kindof).
00043     l = Log(level=LEVELS[msg['level']], name=str(msg['category']), msg=msg['message'], file=file, line=line, function=function)
00044     l.header.stamp = rospy.Time(msg['timestamp']['tv_sec'], msg['timestamp']['tv_usec'])
00045     ROSOUT_PUB.publish(l)
00046 
00047 class NaoLogger(NaoNode):
00048     #This should be treated as a constant
00049     NODE_NAME = 'nao_logger'
00050 
00051     def __init__( self ):
00052         #Initialization
00053         NaoNode.__init__( self, self.NODE_NAME )
00054 
00055         from distutils.version import LooseVersion
00056         if self.get_version() < LooseVersion('2.0.0'):
00057             rospy.loginfo('The NAOqi version is inferior to 2.0, hence no log bridge possible')
00058             exit(0)
00059 
00060         rospy.init_node( self.NODE_NAME )
00061 
00062         # the log manager is only avaiable through a session (NAOqi 2.0)
00063         self.session = qi.Session()
00064         self.session.connect("tcp://%s:%s" % (self.pip, self.pport))
00065         self.logManager = self.session.service("LogManager")
00066 
00067         self.listener = self.logManager.getListener()
00068         self.listener.onLogMessage.connect(onMessageCallback)
00069         rospy.loginfo('Logger initialized')
00070 
00071 if __name__ == '__main__':
00072     try:
00073         nao_logger = NaoLogger()
00074     except RuntimeError as e:
00075         rospy.logerr('Something went wrong: %s' % str(e) )
00076 
00077     rospy.spin()


nao_driver
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Séverin Lemaignan
autogenerated on Mon Oct 6 2014 02:39:17