gen_calibration.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (C) 2012, Austin Robot Technology
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Austin Robot Technology, Inc. nor the names
00018 #    of its contributors may be used to endorse or promote products
00019 #    derived from this software without specific prior written
00020 #    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 # Revision $Id$
00036 
00037 """
00038 Generate YAML calibration file from Velodyne db.xml.
00039 
00040 The input data provided by the manufacturer are in degrees and
00041 centimeters. The YAML file uses radians and meters, following ROS
00042 standards [REP-0103].
00043 
00044 """
00045 
00046 from __future__ import print_function
00047 
00048 import math
00049 import optparse
00050 import os
00051 import sys
00052 from xml.etree import ElementTree
00053 import yaml
00054 
00055 # parse the command line
00056 usage = """usage: %prog infile.xml [outfile.yaml]
00057 
00058        Default output file is input file with .yaml suffix."""
00059 parser = optparse.OptionParser(usage=usage)
00060 options, args = parser.parse_args()
00061 
00062 if len(args) < 1:
00063     parser.error('XML file name missing')
00064     sys.exit(9)
00065 
00066 xmlFile = args[0]
00067 if len(args) >= 2:
00068     yamlFile = args[1]
00069 else:
00070     yamlFile, ext = os.path.splitext(xmlFile)
00071     yamlFile += '.yaml'
00072 
00073 print('converting "' + xmlFile + '" to "' + yamlFile + '"')
00074 
00075 calibrationGood = True
00076 def xmlError(msg):
00077     'handle XML calibration error'
00078     global calibrationGood
00079     calibrationGood = False
00080     print('gen_calibration.py: ' + msg)
00081 
00082 db = None
00083 try:
00084     db = ElementTree.parse(xmlFile)
00085 except IOError:
00086     xmlError('unable to read ' + xmlFile)
00087 except ElementTree.ParseError:
00088     xmlError('XML parse failed for ' + xmlFile)
00089 
00090 if not calibrationGood:
00091     sys.exit(2)
00092 
00093 # create a dictionary to hold all relevant calibration values
00094 calibration = {'num_lasers': 0, 'lasers': []}
00095 cm2meters = 0.01                       # convert centimeters to meters
00096 
00097 def addLaserCalibration(laser_num, key, val):
00098     'Define key and corresponding value for laser_num'
00099     global calibration
00100     if laser_num < len(calibration['lasers']):
00101         calibration['lasers'][laser_num][key] = val
00102     else:
00103         calibration['lasers'].append({key: val})
00104 
00105 # add enabled flags
00106 num_enabled = 0
00107 enabled_lasers = []
00108 enabled = db.find('DB/enabled_')
00109 if enabled == None:
00110     print('no enabled tags found: assuming all 64 enabled')
00111     num_enabled = 64
00112     enabled_lasers = [True for i in xrange(num_enabled)]
00113 else:
00114     index = 0
00115     for el in enabled:
00116         if el.tag == 'item':
00117             this_enabled = int(el.text) != 0
00118             enabled_lasers.append(this_enabled)
00119             index += 1
00120             if this_enabled:
00121                 num_enabled += 1
00122 
00123 calibration['num_lasers'] = num_enabled
00124 print(str(num_enabled) + ' lasers')
00125 
00126 # add minimum laser intensities
00127 minIntensities = db.find('DB/minIntensity_')
00128 if minIntensities != None:
00129     index = 0
00130     for el in minIntensities:
00131         if el.tag == 'item':
00132             if enabled_lasers[index]:
00133                 value = int(el.text)
00134                 if value != 0:
00135                     addLaserCalibration(index, 'min_intensity', value)
00136             index += 1
00137 
00138 # add maximum laser intensities
00139 maxIntensities = db.find('DB/maxIntensity_')
00140 if maxIntensities != None:
00141     index = 0
00142     for el in maxIntensities:
00143         if el.tag == 'item':
00144             if enabled_lasers[index]:
00145                 value = int(el.text)
00146                 if value != 255:
00147                     addLaserCalibration(index, 'max_intensity', value)
00148                 index += 1
00149 
00150 # add calibration information for each laser
00151 for el in db.find('DB/points_'):
00152     if el.tag == 'item':
00153         for px in el:
00154             for field in px:
00155                 if field.tag == 'id_':
00156                     index = int(field.text)
00157                     if not enabled_lasers[index]:
00158                         break   # skip this laser, it is not enabled
00159                     addLaserCalibration(index, 'laser_id', index)
00160 
00161                 if field.tag == 'rotCorrection_':
00162                     addLaserCalibration(index, 'rot_correction',
00163                                         math.radians(float(field.text)))
00164                 elif field.tag == 'vertCorrection_':
00165                     addLaserCalibration(index, 'vert_correction',
00166                                         math.radians(float(field.text)))
00167                 elif field.tag == 'distCorrection_':
00168                     addLaserCalibration(index, 'dist_correction', 
00169                                         float(field.text) * cm2meters)
00170                 elif field.tag == 'distCorrectionX_':
00171                     addLaserCalibration(index, 'dist_correction_x',
00172                                         float(field.text) * cm2meters)
00173                 elif field.tag == 'distCorrectionY_':
00174                     addLaserCalibration(index, 'dist_correction_y',
00175                                         float(field.text) * cm2meters)
00176                 elif field.tag == 'vertOffsetCorrection_':
00177                     addLaserCalibration(index, 'vert_offset_correction',
00178                                         float(field.text) * cm2meters)
00179                 elif field.tag == 'horizOffsetCorrection_':
00180                     addLaserCalibration(index, 'horiz_offset_correction',
00181                                         float(field.text) * cm2meters)
00182                 elif field.tag == 'focalDistance_':
00183                     addLaserCalibration(index, 'focal_distance', 
00184                                         float(field.text) * cm2meters)
00185                 elif field.tag == 'focalSlope_':
00186                     addLaserCalibration(index, 'focal_slope', float(field.text))
00187 
00188 # validate input data
00189 if calibration['num_lasers'] <= 0:
00190     xmlError('no lasers defined')
00191 elif calibration['num_lasers'] != num_enabled:
00192     xmlError('inconsistent number of lasers defined')
00193 
00194 # TODO: make sure all required fields are present.
00195 # (Which ones are required?)
00196 
00197 if calibrationGood:
00198 
00199     # write calibration data to YAML file
00200     f = open(yamlFile, 'w')
00201     try:
00202         yaml.dump(calibration, f)
00203     finally:
00204         f.close()


velodyne_pointcloud
Author(s): Jack O'Quin, Piyush Khandelwal, Jesse Vera
autogenerated on Thu Aug 27 2015 15:37:05