gen_calibration.py
Go to the documentation of this file.
1 #!/usr/bin/python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (C) 2012, Austin Robot Technology
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of Austin Robot Technology, Inc. nor the names
18 # of its contributors may be used to endorse or promote products
19 # derived from this software without specific prior written
20 # permission.
21 #
22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 # POSSIBILITY OF SUCH DAMAGE.
34 #
35 # Revision $Id$
36 
37 """
38 Generate YAML calibration file from Velodyne db.xml.
39 
40 The input data provided by the manufacturer are in degrees and
41 centimeters. The YAML file uses radians and meters, following ROS
42 standards [REP-0103].
43 
44 """
45 
46 from __future__ import print_function
47 
48 import math
49 import optparse
50 import os
51 import sys
52 from xml.etree import ElementTree
53 import yaml
54 
55 # parse the command line
56 usage = """usage: %prog infile.xml [outfile.yaml]
57 
58  Default output file is input file with .yaml suffix."""
59 parser = optparse.OptionParser(usage=usage)
60 options, args = parser.parse_args()
61 
62 if len(args) < 1:
63  parser.error('XML file name missing')
64  sys.exit(9)
65 
66 xmlFile = args[0]
67 if len(args) >= 2:
68  yamlFile = args[1]
69 else:
70  yamlFile, ext = os.path.splitext(xmlFile)
71  yamlFile += '.yaml'
72 
73 print('converting "' + xmlFile + '" to "' + yamlFile + '"')
74 
75 calibrationGood = True
76 def xmlError(msg):
77  'handle XML calibration error'
78  global calibrationGood
79  calibrationGood = False
80  print('gen_calibration.py: ' + msg)
81 
82 db = None
83 try:
84  db = ElementTree.parse(xmlFile)
85 except IOError:
86  xmlError('unable to read ' + xmlFile)
87 except ElementTree.ParseError:
88  xmlError('XML parse failed for ' + xmlFile)
89 
90 if not calibrationGood:
91  sys.exit(2)
92 
93 # create a dictionary to hold all relevant calibration values
94 calibration = {'num_lasers': 0, 'lasers': [], 'distance_resolution': 0.2}
95 cm2meters = 0.01 # convert centimeters to meters
96 
97 def addLaserCalibration(laser_num, key, val):
98  'Define key and corresponding value for laser_num'
99  global calibration
100  if laser_num < len(calibration['lasers']):
101  calibration['lasers'][laser_num][key] = val
102  else:
103  calibration['lasers'].append({key: val})
104 
105 # add enabled flags
106 num_enabled = 0
107 enabled_lasers = []
108 enabled = db.find('DB/enabled_')
109 if enabled == None:
110  print('no enabled tags found: assuming all 64 enabled')
111  num_enabled = 64
112  enabled_lasers = [True for i in xrange(num_enabled)]
113 else:
114  index = 0
115  for el in enabled:
116  if el.tag == 'item':
117  this_enabled = int(el.text) != 0
118  enabled_lasers.append(this_enabled)
119  index += 1
120  if this_enabled:
121  num_enabled += 1
122 
123 calibration['num_lasers'] = num_enabled
124 print(str(num_enabled) + ' lasers')
125 
126 # add distance resolution (cm)
127 distLSB = db.find('DB/distLSB_')
128 if distLSB != None:
129  calibration['distance_resolution'] = float(distLSB.text) * cm2meters
130 
131 # add minimum laser intensities
132 minIntensities = db.find('DB/minIntensity_')
133 if minIntensities != None:
134  index = 0
135  for el in minIntensities:
136  if el.tag == 'item':
137  if enabled_lasers[index]:
138  value = int(el.text)
139  if value != 0:
140  addLaserCalibration(index, 'min_intensity', value)
141  index += 1
142 
143 # add maximum laser intensities
144 maxIntensities = db.find('DB/maxIntensity_')
145 if maxIntensities != None:
146  index = 0
147  for el in maxIntensities:
148  if el.tag == 'item':
149  if enabled_lasers[index]:
150  value = int(el.text)
151  if value != 255:
152  addLaserCalibration(index, 'max_intensity', value)
153  index += 1
154 
155 # add calibration information for each laser
156 for el in db.find('DB/points_'):
157  if el.tag == 'item':
158  for px in el:
159  for field in px:
160  if field.tag == 'id_':
161  index = int(field.text)
162  if not enabled_lasers[index]:
163  break # skip this laser, it is not enabled
164  addLaserCalibration(index, 'laser_id', index)
165 
166  if field.tag == 'rotCorrection_':
167  addLaserCalibration(index, 'rot_correction',
168  math.radians(float(field.text)))
169  elif field.tag == 'vertCorrection_':
170  addLaserCalibration(index, 'vert_correction',
171  math.radians(float(field.text)))
172  elif field.tag == 'distCorrection_':
173  addLaserCalibration(index, 'dist_correction',
174  float(field.text) * cm2meters)
175  elif field.tag == 'distCorrectionX_':
176  addLaserCalibration(index, 'dist_correction_x',
177  float(field.text) * cm2meters)
178  elif field.tag == 'distCorrectionY_':
179  addLaserCalibration(index, 'dist_correction_y',
180  float(field.text) * cm2meters)
181  elif field.tag == 'vertOffsetCorrection_':
182  addLaserCalibration(index, 'vert_offset_correction',
183  float(field.text) * cm2meters)
184  elif field.tag == 'horizOffsetCorrection_':
185  addLaserCalibration(index, 'horiz_offset_correction',
186  float(field.text) * cm2meters)
187  elif field.tag == 'focalDistance_':
188  addLaserCalibration(index, 'focal_distance',
189  float(field.text) * cm2meters)
190  elif field.tag == 'focalSlope_':
191  addLaserCalibration(index, 'focal_slope', float(field.text))
192 
193 # validate input data
194 if calibration['num_lasers'] <= 0:
195  xmlError('no lasers defined')
196 elif calibration['num_lasers'] != num_enabled:
197  xmlError('inconsistent number of lasers defined')
198 
199 # TODO: make sure all required fields are present.
200 # (Which ones are required?)
201 
202 if calibrationGood:
203 
204  # write calibration data to YAML file
205  f = open(yamlFile, 'w')
206  try:
207  yaml.dump(calibration, f)
208  finally:
209  f.close()
gen_calibration.xmlError
def xmlError(msg)
Definition: gen_calibration.py:76
gen_calibration.addLaserCalibration
def addLaserCalibration(laser_num, key, val)
Definition: gen_calibration.py:97

velodyne_pointcloud
Author(s): Jack O'Quin, Piyush Khandelwal, Jesse Vera, Sebastian Pütz
autogenerated on Sun Sep 6 2020 03:25:30