Initial commit
This commit is contained in:
@@ -0,0 +1,209 @@
|
||||
#!/usr/bin/python
|
||||
# Software License Agreement (BSD License)
|
||||
#
|
||||
# Copyright (C) 2012, Austin Robot Technology
|
||||
# All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# * Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# * Redistributions in binary form must reproduce the above
|
||||
# copyright notice, this list of conditions and the following
|
||||
# disclaimer in the documentation and/or other materials provided
|
||||
# with the distribution.
|
||||
# * Neither the name of Austin Robot Technology, Inc. nor the names
|
||||
# of its contributors may be used to endorse or promote products
|
||||
# derived from this software without specific prior written
|
||||
# permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
# Revision $Id$
|
||||
|
||||
"""
|
||||
Generate YAML calibration file from Velodyne db.xml.
|
||||
|
||||
The input data provided by the manufacturer are in degrees and
|
||||
centimeters. The YAML file uses radians and meters, following ROS
|
||||
standards [REP-0103].
|
||||
|
||||
"""
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
import optparse
|
||||
import os
|
||||
import sys
|
||||
from xml.etree import ElementTree
|
||||
import yaml
|
||||
|
||||
# parse the command line
|
||||
usage = """usage: %prog infile.xml [outfile.yaml]
|
||||
|
||||
Default output file is input file with .yaml suffix."""
|
||||
parser = optparse.OptionParser(usage=usage)
|
||||
options, args = parser.parse_args()
|
||||
|
||||
if len(args) < 1:
|
||||
parser.error('XML file name missing')
|
||||
sys.exit(9)
|
||||
|
||||
xmlFile = args[0]
|
||||
if len(args) >= 2:
|
||||
yamlFile = args[1]
|
||||
else:
|
||||
yamlFile, ext = os.path.splitext(xmlFile)
|
||||
yamlFile += '.yaml'
|
||||
|
||||
print('converting "' + xmlFile + '" to "' + yamlFile + '"')
|
||||
|
||||
calibrationGood = True
|
||||
def xmlError(msg):
|
||||
'handle XML calibration error'
|
||||
global calibrationGood
|
||||
calibrationGood = False
|
||||
print('gen_calibration.py: ' + msg)
|
||||
|
||||
db = None
|
||||
try:
|
||||
db = ElementTree.parse(xmlFile)
|
||||
except IOError:
|
||||
xmlError('unable to read ' + xmlFile)
|
||||
except ElementTree.ParseError:
|
||||
xmlError('XML parse failed for ' + xmlFile)
|
||||
|
||||
if not calibrationGood:
|
||||
sys.exit(2)
|
||||
|
||||
# create a dictionary to hold all relevant calibration values
|
||||
calibration = {'num_lasers': 0, 'lasers': [], 'distance_resolution': 0.2}
|
||||
cm2meters = 0.01 # convert centimeters to meters
|
||||
|
||||
def addLaserCalibration(laser_num, key, val):
|
||||
'Define key and corresponding value for laser_num'
|
||||
global calibration
|
||||
if laser_num < len(calibration['lasers']):
|
||||
calibration['lasers'][laser_num][key] = val
|
||||
else:
|
||||
calibration['lasers'].append({key: val})
|
||||
|
||||
# add enabled flags
|
||||
num_enabled = 0
|
||||
enabled_lasers = []
|
||||
enabled = db.find('DB/enabled_')
|
||||
if enabled == None:
|
||||
print('no enabled tags found: assuming all 64 enabled')
|
||||
num_enabled = 64
|
||||
enabled_lasers = [True for i in xrange(num_enabled)]
|
||||
else:
|
||||
index = 0
|
||||
for el in enabled:
|
||||
if el.tag == 'item':
|
||||
this_enabled = int(el.text) != 0
|
||||
enabled_lasers.append(this_enabled)
|
||||
index += 1
|
||||
if this_enabled:
|
||||
num_enabled += 1
|
||||
|
||||
calibration['num_lasers'] = num_enabled
|
||||
print(str(num_enabled) + ' lasers')
|
||||
|
||||
# add distance resolution (cm)
|
||||
distLSB = db.find('DB/distLSB_')
|
||||
if distLSB != None:
|
||||
calibration['distance_resolution'] = float(distLSB.text) * cm2meters
|
||||
|
||||
# add minimum laser intensities
|
||||
minIntensities = db.find('DB/minIntensity_')
|
||||
if minIntensities != None:
|
||||
index = 0
|
||||
for el in minIntensities:
|
||||
if el.tag == 'item':
|
||||
if enabled_lasers[index]:
|
||||
value = int(el.text)
|
||||
if value != 0:
|
||||
addLaserCalibration(index, 'min_intensity', value)
|
||||
index += 1
|
||||
|
||||
# add maximum laser intensities
|
||||
maxIntensities = db.find('DB/maxIntensity_')
|
||||
if maxIntensities != None:
|
||||
index = 0
|
||||
for el in maxIntensities:
|
||||
if el.tag == 'item':
|
||||
if enabled_lasers[index]:
|
||||
value = int(el.text)
|
||||
if value != 255:
|
||||
addLaserCalibration(index, 'max_intensity', value)
|
||||
index += 1
|
||||
|
||||
# add calibration information for each laser
|
||||
for el in db.find('DB/points_'):
|
||||
if el.tag == 'item':
|
||||
for px in el:
|
||||
for field in px:
|
||||
if field.tag == 'id_':
|
||||
index = int(field.text)
|
||||
if not enabled_lasers[index]:
|
||||
break # skip this laser, it is not enabled
|
||||
addLaserCalibration(index, 'laser_id', index)
|
||||
|
||||
if field.tag == 'rotCorrection_':
|
||||
addLaserCalibration(index, 'rot_correction',
|
||||
math.radians(float(field.text)))
|
||||
elif field.tag == 'vertCorrection_':
|
||||
addLaserCalibration(index, 'vert_correction',
|
||||
math.radians(float(field.text)))
|
||||
elif field.tag == 'distCorrection_':
|
||||
addLaserCalibration(index, 'dist_correction',
|
||||
float(field.text) * cm2meters)
|
||||
elif field.tag == 'distCorrectionX_':
|
||||
addLaserCalibration(index, 'dist_correction_x',
|
||||
float(field.text) * cm2meters)
|
||||
elif field.tag == 'distCorrectionY_':
|
||||
addLaserCalibration(index, 'dist_correction_y',
|
||||
float(field.text) * cm2meters)
|
||||
elif field.tag == 'vertOffsetCorrection_':
|
||||
addLaserCalibration(index, 'vert_offset_correction',
|
||||
float(field.text) * cm2meters)
|
||||
elif field.tag == 'horizOffsetCorrection_':
|
||||
addLaserCalibration(index, 'horiz_offset_correction',
|
||||
float(field.text) * cm2meters)
|
||||
elif field.tag == 'focalDistance_':
|
||||
addLaserCalibration(index, 'focal_distance',
|
||||
float(field.text) * cm2meters)
|
||||
elif field.tag == 'focalSlope_':
|
||||
addLaserCalibration(index, 'focal_slope', float(field.text))
|
||||
|
||||
# validate input data
|
||||
if calibration['num_lasers'] <= 0:
|
||||
xmlError('no lasers defined')
|
||||
elif calibration['num_lasers'] != num_enabled:
|
||||
xmlError('inconsistent number of lasers defined')
|
||||
|
||||
# TODO: make sure all required fields are present.
|
||||
# (Which ones are required?)
|
||||
|
||||
if calibrationGood:
|
||||
|
||||
# write calibration data to YAML file
|
||||
f = open(yamlFile, 'w')
|
||||
try:
|
||||
yaml.dump(calibration, f)
|
||||
finally:
|
||||
f.close()
|
||||
Reference in New Issue
Block a user