exporter.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2009, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of the Willow Garage nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 ##\author Kevin Watts
00036 ##\brief LogExporter class does diagnostics logfile conversion to CSV
00037 
00038 PKG = 'diagnostic_analysis'
00039 
00040 import roslib; roslib.load_manifest(PKG)
00041 import rosbag
00042 import diagnostic_msgs.msg
00043 import time, sys, os
00044 import operator, tempfile, subprocess
00045 
00046 ##\brief Converts and processes diagnostics logs to CSV format
00047 ##
00048 ## Used by scripts/export_csv.py to convert diagnostics log files to CSV format
00049 class LogExporter:
00050     ##\param output_dir str : Complete path of output dir. If None, uses temp dir
00051     ##\param logfile str : path of logfile
00052     def __init__(self, output_dir, logfile):
00053         self._temp = False
00054         self._stats = {}
00055         self.logfile = logfile
00056 
00057         self.output_dir = output_dir
00058         if self.output_dir is None:
00059             self.output_dir = tempfile.mkdtemp()
00060             self._temp = True
00061         
00062         if not os.path.isdir(self.output_dir):
00063             os.makedirs(self.output_dir)
00064 
00065     ##\brief Removes all output files. Removes directory if temp
00066     def remove_files(self):
00067         for name in self._stats:
00068             file = self._stats[name]['file_name']
00069             os.remove(file)
00070         if self._temp:
00071             os.rmdir(self.output_dir)
00072         
00073     ##\brief Return filename of output
00074     ##\param name str : DiagnosticStatus name ex: 'Mechanism Control'
00075     def get_filename(self, name):
00076         if not self._stats.has_key(name):
00077             return None # self.output_dir + '/%s.csv' % name.replace(' ', '_')
00078         return self._stats[name]['file_name']
00079 
00080     ##\brief Use rosrecord to play back bagfile
00081     def process_log(self):
00082         bag = rosbag.Bag(self.logfile)
00083         for (topic, msg, t) in bag.read_messages():
00084             self._update(topic, msg)
00085 
00086     ##\brief Creates and updates data files with new messages
00087     def _update(self, topic, msg):
00088         if (not (topic == '/diagnostics')):
00089             print "Discarding message on topic: %s" % topic
00090             return
00091         
00092         t = time.localtime(float(str(msg.header.stamp)) / 1000000000.0)
00093         
00094         for status in msg.status:
00095             name = status.name
00096 
00097             if (not self._stats.has_key(name)):
00098                 self._stats[name] = {}
00099                 
00100                 fields = {}
00101                 for index, s in enumerate(status.values):
00102                     fields[s.key] = index
00103                     
00104                 self._stats[name]['fields'] = fields
00105                 
00106                 self._stats[name]['level'] = status.level
00107                 self._stats[name]['message'] = status.message
00108                 self._stats[name]['hardware_id'] = status.hardware_id
00109 
00110                 # Use named temp file, will cat this to header on close
00111                 datafile, tmp_name = tempfile.mkstemp()
00112                 self._stats[name]['data_file'] = os.fdopen(datafile, 'w')
00113                 self._stats[name]['data_name'] = tmp_name
00114                 
00115 
00116             # Check to see if fields have changed. Add new fields to map
00117             if (not [s.key for s in status.values] == self._stats[name]['fields'].keys()):
00118                 for s in status.values:
00119                     if not self._stats[name]['fields'].has_key(s.key):
00120                         self._stats[name]['fields'][s.key] = len(self._stats[name]['fields'])
00121 
00122             # Add values in correct place for header index
00123             # Key/Value pairs can move around, this makes sure values are 
00124             # added to correct keys
00125             vals = []
00126             for key, val in self._stats[name]['fields'].iteritems():
00127                 vals.append('')
00128             for s in status.values:
00129                 vals[self._stats[name]['fields'][s.key]] = s.value.replace('\n','  ').replace(',',' ')
00130         
00131             msg = status.message.replace(',',' ').strip()
00132             hw_id = status.hardware_id.replace(',', ' ')
00133         
00134             self._stats[name]['data_file'].write(','.join([time.strftime("%Y/%m/%d %H:%M:%S", t)] + 
00135                                             [str(status.level), msg, hw_id] + vals) + '\n')
00136 
00137     ##\brief Close logfile, append data to header
00138     def finish_logfile(self):
00139         for name in self._stats:
00140             # Sort fields by correct index, add to header
00141             field_dict = sorted(self._stats[name]['fields'].iteritems(), key=operator.itemgetter(1))
00142             fields = map(operator.itemgetter(0), field_dict)
00143             
00144             header_line = ','.join(['Timestamp'] + ['Level', 'Message', 'Hardware ID'] + 
00145                                     [f.replace(',','').replace('\n', ' ') for f in fields]) + '\n'
00146             
00147             file_name = os.path.join(self.output_dir, name.replace(' ', '_').replace('(', '').replace(')', '').replace('/', '__').replace('.', '').replace('#', '') + '.csv')
00148             
00149             output_file = file(file_name, 'w')
00150             output_file.write(header_line)
00151             output_file.close()
00152 
00153 
00154             self._stats[name]['data_file'].close() # Close data
00155             
00156             # Append the tmp data file to the header
00157             subprocess.call("cat %s >> %s" % (self._stats[name]['data_name'], file_name), shell=True)
00158             # Remove tmp file
00159             os.remove(self._stats[name]['data_name'])
00160             # Store filename
00161             self._stats[name]['file_name'] = file_name
00162     


diagnostic_analysis
Author(s): Eric Berger, Kevin Watts
autogenerated on Fri Jan 3 2014 11:19:15