tilting_laser_sensor_unittest.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage, Inc. nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 
34 
35 import roslib; roslib.load_manifest('calibration_estimation')
36 
37 import sys
38 import unittest
39 import rospy
40 import numpy
41 import yaml
42 from calibration_estimation.sensors.tilting_laser_sensor import TiltingLaserBundler, TiltingLaserSensor
43 from calibration_msgs.msg import *
44 from sensor_msgs.msg import JointState, CameraInfo
45 from calibration_estimation.urdf_params import UrdfParams
46 
47 from calibration_estimation.tilting_laser import TiltingLaser
48 
49 import numpy
50 from numpy import *
51 
53 
54  config_yaml = '''
55 - laser_id: laser1
56  sensor_id: laser1
57 '''
58  config_dict = yaml.load(config_yaml)
59  return config_dict
60 
61 class TestTiltingLaserBundler(unittest.TestCase):
62  def test_basic(self):
63  config_list = loadConfigList()
64 
65  bundler = TiltingLaserBundler(config_list)
66 
67  M_robot = RobotMeasurement()
68  M_robot.M_laser.append( LaserMeasurement(laser_id="laser1"))\
69 
70  blocks = bundler.build_blocks(M_robot)
71 
72  self.assertEqual( len(blocks), 1)
73  self.assertEqual( blocks[0]._M_laser.laser_id, "laser1")
74 
75 def loadSystem():
76  urdf = '''
77 <robot>
78  <link name="base_link"/>
79  <joint name="j0" type="fixed">
80  <origin xyz="0 0 0" rpy="0 0 0"/>
81  <parent link="base_link"/>
82  <child link="j0_link"/>
83  </joint>
84  <link name="j0_link"/>
85  <joint name="j1" type="revolute">
86  <axis xyz="0 0 1"/>
87  <origin xyz="0 0 0" rpy="0 0 0"/>
88  <parent link="j0_link"/>
89  <child link="j1_link"/>
90  </joint>
91  <link name="j1_link"/>
92  <joint name="j2" type="fixed">
93  <origin xyz="0 0 0" rpy="0 0 0"/>
94  <parent link="j1_link"/>
95  <child link="j2_link"/>
96  </joint>
97  <link name="j2_link"/>
98 </robot>
99 '''
100  config = yaml.load('''
101 sensors:
102  chains: {}
103  rectified_cams: {}
104  tilting_lasers:
105  laserA:
106  sensor_id: laserA
107  joint: j1
108  frame_id: j2_link
109  gearing: 1
110  cov:
111  bearing: 1
112  range: 1
113  tilt: 1
114 transforms: {}
115 checkerboards: {}
116 ''')
117 
118  return config["sensors"], UrdfParams(urdf, config)
119 
120 
121 class TestTiltingLaser(unittest.TestCase):
122 
123  def test_cov(self):
124  print ""
125  config, robot_params = loadSystem()
126 
127  joint_points = [ JointState(position=[0,0,1]),
128  JointState(position=[pi/2,0,2]) ]
129 
130  sensor = TiltingLaserSensor(config["tilting_lasers"]["laserA"], LaserMeasurement(laser_id="laserA",
131  joint_points=joint_points))
132 
133  sensor.update_config(robot_params)
134 
135  cov = sensor.compute_cov(None)
136 
137  print "Cov:"
138  print cov
139 
140  self.assertAlmostEqual(cov[0,0], 1.0, 6)
141  self.assertAlmostEqual(cov[1,1], 1.0, 6)
142  self.assertAlmostEqual(cov[2,2], 1.0, 6)
143  self.assertAlmostEqual(cov[3,3], 4.0, 6)
144  self.assertAlmostEqual(cov[4,4], 4.0, 6)
145  self.assertAlmostEqual(cov[5,5], 1.0, 6)
146 
147  def test_gamma(self):
148  print ""
149  config, robot_params = loadSystem()
150 
151  joint_points = [ JointState(position=[0,0,1]),
152  JointState(position=[pi/2,0,2]) ]
153 
154  sensor = TiltingLaserSensor(config["tilting_lasers"]["laserA"], LaserMeasurement(laser_id="laserA",
155  joint_points=joint_points))
156 
157  sensor.update_config(robot_params)
158  gamma = sensor.compute_marginal_gamma_sqrt(None)
159 
160  print "Gamma:"
161  print gamma
162  self.assertAlmostEqual(gamma[0,0], 1.0, 6)
163  self.assertAlmostEqual(gamma[1,1], 1.0, 6)
164  self.assertAlmostEqual(gamma[2,2], 1.0, 6)
165  self.assertAlmostEqual(gamma[3,3], 0.5, 6)
166  self.assertAlmostEqual(gamma[4,4], 0.5, 6)
167  self.assertAlmostEqual(gamma[5,5], 1.0, 6)
168 
170  print ""
171  config, robot_params = loadSystem()
172 
173  joint_points = [ JointState(position=[0,0,0]),
174  JointState(position=[0,pi/2,1]),
175  JointState(position=[pi/2,0,1]) ]
176 
177  sensor = TiltingLaserSensor(config["tilting_lasers"]["laserA"], LaserMeasurement(laser_id="laserA",
178  joint_points=joint_points))
179 
180  sensor.update_config(robot_params)
181 
182  target_pts = matrix( [ [ 0, 0, 0 ],
183  [ 0, 1, 0 ],
184  [ 0, 0, -1 ],
185  [ 1, 1, 1 ] ] )
186 
187  h = sensor.compute_expected(target_pts)
188  z = sensor.get_measurement()
189  r = sensor.compute_residual(target_pts)
190 
191  self.assertAlmostEqual(numpy.linalg.norm(h-target_pts), 0.0, 6)
192  self.assertAlmostEqual(numpy.linalg.norm(z-target_pts), 0.0, 6)
193  self.assertAlmostEqual(numpy.linalg.norm(r), 0.0, 6)
194 
195  # Test Sparsity
196  sparsity = sensor.build_sparsity_dict()
197  self.assertEqual(sparsity['transforms']['j0'], [1,1,1,1,1,1])
198  self.assertEqual(sparsity['transforms']['j1'], [1,1,1,1,1,1])
199  self.assertEqual(sparsity['transforms']['j2'], [1,1,1,1,1,1])
200  self.assertEqual(sparsity['tilting_lasers']['laserA']['gearing'], 1)
201 
202 
203 if __name__ == '__main__':
204  import rostest
205  rostest.unitrun('calibration_estimation', 'test_TiltingLaserBundler', TestTiltingLaserBundler, coverage_packages=['calibration_estimation.sensors.tilting_laser_sensor'])
206  rostest.unitrun('calibration_estimation', 'test_TiltingLaser', TestTiltingLaser, coverage_packages=['calibration_estimation.sensors.tilting_laser_sensor'])


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Thu Jun 6 2019 19:17:16