00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 from angles import normalize_angle_positive, normalize_angle, shortest_angular_distance, two_pi_complement, shortest_angular_distance_with_limits
00036 from angles import _find_min_max_delta
00037 import sys
00038 import unittest
00039 from math import pi, fabs
00040
00041
00042 class TestAngles(unittest.TestCase):
00043 def test_shortestDistanceWithLimits(self):
00044 result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.5,-0.25,0.25)
00045 self.assertFalse(result)
00046
00047 result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.5,0.25,0.25)
00048 self.assertFalse(result)
00049
00050 result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.5,0.25,-0.25)
00051 self.assertTrue(result)
00052 self.assertAlmostEqual(shortest_angle, -2*pi+1.0)
00053
00054 result, shortest_angle = shortest_angular_distance_with_limits(0.5, 0.5,0.25,-0.25)
00055 self.assertTrue(result)
00056 self.assertAlmostEqual(shortest_angle, 0)
00057
00058 result, shortest_angle = shortest_angular_distance_with_limits(0.5, 0,0.25,-0.25)
00059 self.assertFalse(result)
00060 self.assertAlmostEqual(shortest_angle, -0.5)
00061
00062 result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0,0.25,-0.25)
00063 self.assertFalse(result)
00064 self.assertAlmostEqual(shortest_angle, 0.5)
00065
00066 result, shortest_angle = shortest_angular_distance_with_limits(-0.2,0.2,0.25,-0.25)
00067 self.assertFalse(result)
00068 self.assertAlmostEqual(shortest_angle, -2*pi+0.4)
00069
00070 result, shortest_angle = shortest_angular_distance_with_limits(0.2,-0.2,0.25,-0.25)
00071 self.assertFalse(result)
00072 self.assertAlmostEqual(shortest_angle,2*pi-0.4)
00073
00074 result, shortest_angle = shortest_angular_distance_with_limits(0.2,0,0.25,-0.25)
00075 self.assertFalse(result)
00076 self.assertAlmostEqual(shortest_angle,2*pi-0.2)
00077
00078 result, shortest_angle = shortest_angular_distance_with_limits(-0.2,0,0.25,-0.25)
00079 self.assertFalse(result)
00080 self.assertAlmostEqual(shortest_angle,-2*pi+0.2)
00081
00082 result, shortest_angle = shortest_angular_distance_with_limits(-0.25,-0.5,0.25,-0.25)
00083 self.assertTrue(result)
00084 self.assertAlmostEqual(shortest_angle,-0.25)
00085
00086 result, shortest_angle = shortest_angular_distance_with_limits(-0.25,0.5,0.25,-0.25)
00087 self.assertTrue(result)
00088 self.assertAlmostEqual(shortest_angle,-2*pi+0.75)
00089
00090 result, shortest_angle = shortest_angular_distance_with_limits(-0.2500001,0.5,0.25,-0.25)
00091 self.assertTrue(result)
00092 self.assertAlmostEqual(shortest_angle,-2*pi+0.5+0.2500001)
00093
00094 result, shortest_angle = shortest_angular_distance_with_limits(-0.6, 0.5,-0.25,0.25)
00095 self.assertFalse(result)
00096
00097 result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.6,-0.25,0.25)
00098 self.assertFalse(result)
00099
00100 result, shortest_angle = shortest_angular_distance_with_limits(-0.6, 0.75,-0.25,0.3)
00101 self.assertFalse(result)
00102
00103 result, shortest_angle = shortest_angular_distance_with_limits(-0.6, pi*3.0/4.0,-0.25,0.3)
00104 self.assertFalse(result)
00105
00106 result, shortest_angle = shortest_angular_distance_with_limits(-pi, pi,-pi,pi)
00107 self.assertTrue(result)
00108 self.assertAlmostEqual(shortest_angle,0.0)
00109
00110 def test_normalize_angle_positive(self):
00111 self.assertAlmostEqual(0, normalize_angle_positive(0))
00112 self.assertAlmostEqual(pi, normalize_angle_positive(pi))
00113 self.assertAlmostEqual(0, normalize_angle_positive(2*pi))
00114 self.assertAlmostEqual(pi, normalize_angle_positive(3*pi))
00115 self.assertAlmostEqual(0, normalize_angle_positive(4*pi))
00116
00117 self.assertAlmostEqual(0, normalize_angle_positive(-0))
00118 self.assertAlmostEqual(pi, normalize_angle_positive(-pi))
00119 self.assertAlmostEqual(0, normalize_angle_positive(-2*pi))
00120 self.assertAlmostEqual(pi, normalize_angle_positive(-3*pi))
00121 self.assertAlmostEqual(0, normalize_angle_positive(-4*pi))
00122
00123 self.assertAlmostEqual(0, normalize_angle_positive(-0))
00124 self.assertAlmostEqual(3*pi/2, normalize_angle_positive(-pi/2))
00125 self.assertAlmostEqual(pi, normalize_angle_positive(-pi))
00126 self.assertAlmostEqual(pi/2, normalize_angle_positive(-3*pi/2))
00127 self.assertAlmostEqual(0, normalize_angle_positive(-4*pi/2))
00128
00129 self.assertAlmostEqual(0, normalize_angle_positive(0))
00130 self.assertAlmostEqual(pi/2, normalize_angle_positive(pi/2))
00131 self.assertAlmostEqual(pi/2, normalize_angle_positive(5*pi/2))
00132 self.assertAlmostEqual(pi/2, normalize_angle_positive(9*pi/2))
00133 self.assertAlmostEqual(pi/2, normalize_angle_positive(-3*pi/2))
00134
00135 def test_normalize_angle(self):
00136 self.assertAlmostEqual(0, normalize_angle(0))
00137 self.assertAlmostEqual(pi, normalize_angle(pi))
00138 self.assertAlmostEqual(0, normalize_angle(2*pi))
00139 self.assertAlmostEqual(pi, normalize_angle(3*pi))
00140 self.assertAlmostEqual(0, normalize_angle(4*pi))
00141
00142 self.assertAlmostEqual(0, normalize_angle(-0))
00143 self.assertAlmostEqual(pi, normalize_angle(-pi))
00144 self.assertAlmostEqual(0, normalize_angle(-2*pi))
00145 self.assertAlmostEqual(pi, normalize_angle(-3*pi))
00146 self.assertAlmostEqual(0, normalize_angle(-4*pi))
00147
00148 self.assertAlmostEqual(0, normalize_angle(-0))
00149 self.assertAlmostEqual(-pi/2, normalize_angle(-pi/2))
00150 self.assertAlmostEqual(pi, normalize_angle(-pi))
00151 self.assertAlmostEqual(pi/2, normalize_angle(-3*pi/2))
00152 self.assertAlmostEqual(0, normalize_angle(-4*pi/2))
00153
00154 self.assertAlmostEqual(0, normalize_angle(0))
00155 self.assertAlmostEqual(pi/2, normalize_angle(pi/2))
00156 self.assertAlmostEqual(pi/2, normalize_angle(5*pi/2))
00157 self.assertAlmostEqual(pi/2, normalize_angle(9*pi/2))
00158 self.assertAlmostEqual(pi/2, normalize_angle(-3*pi/2))
00159
00160 def test_shortest_angular_distance(self):
00161 self.assertAlmostEqual(pi/2, shortest_angular_distance(0, pi/2))
00162 self.assertAlmostEqual(-pi/2, shortest_angular_distance(0, -pi/2))
00163 self.assertAlmostEqual(-pi/2, shortest_angular_distance(pi/2, 0))
00164 self.assertAlmostEqual(pi/2, shortest_angular_distance(-pi/2, 0))
00165
00166 self.assertAlmostEqual(-pi/2, shortest_angular_distance(pi, pi/2))
00167 self.assertAlmostEqual(pi/2, shortest_angular_distance(pi, -pi/2))
00168 self.assertAlmostEqual(pi/2, shortest_angular_distance(pi/2, pi))
00169 self.assertAlmostEqual(-pi/2, shortest_angular_distance(-pi/2, pi))
00170
00171 self.assertAlmostEqual(-pi/2, shortest_angular_distance(5*pi, pi/2))
00172 self.assertAlmostEqual(pi/2, shortest_angular_distance(7*pi, -pi/2))
00173 self.assertAlmostEqual(pi/2, shortest_angular_distance(9*pi/2, pi))
00174 self.assertAlmostEqual(pi/2, shortest_angular_distance(-3*pi/2, pi))
00175
00176
00177 self.assertAlmostEqual(-pi/2, shortest_angular_distance(-3*pi/4, 3*pi/4))
00178 self.assertAlmostEqual(pi/2, shortest_angular_distance(3*pi/4, -3*pi/4))
00179
00180 def test_two_pi_complement(self):
00181 epsilon = 1e-9
00182 self.assertAlmostEqual(two_pi_complement(0), 2*pi)
00183 self.assertAlmostEqual(two_pi_complement(2*pi), 0)
00184 self.assertAlmostEqual(two_pi_complement(-2*pi), 0)
00185 self.assertAlmostEqual(two_pi_complement(2*pi-epsilon), -epsilon)
00186 self.assertAlmostEqual(two_pi_complement(-2*pi+epsilon), epsilon)
00187 self.assertAlmostEqual(two_pi_complement(pi/2), -3*pi/2)
00188 self.assertAlmostEqual(two_pi_complement(pi), -pi)
00189 self.assertAlmostEqual(two_pi_complement(-pi), pi)
00190 self.assertAlmostEqual(two_pi_complement(-pi/2), 3*pi/2)
00191
00192 self.assertAlmostEqual(two_pi_complement(3*pi), -pi)
00193 self.assertAlmostEqual(two_pi_complement(-3.0*pi), pi)
00194 self.assertAlmostEqual(two_pi_complement(-5.0*pi/2.0), 3*pi/2)
00195
00196 def test_find_min_max_delta(self):
00197 epsilon = 1e-9
00198
00199 flag, min_delta, max_delta = _find_min_max_delta( 0, -pi, pi)
00200 self.assertTrue(flag)
00201 self.assertAlmostEqual(min_delta, -pi)
00202 self.assertAlmostEqual(max_delta, pi)
00203
00204
00205 flag, min_delta, max_delta = _find_min_max_delta( pi/2, -pi, pi)
00206 self.assertTrue(flag)
00207 self.assertAlmostEqual(min_delta, -3*pi/2)
00208 self.assertAlmostEqual(max_delta, pi/2)
00209
00210
00211 flag, min_delta, max_delta = _find_min_max_delta( -pi/2, -pi, pi)
00212 self.assertTrue(flag)
00213 self.assertAlmostEqual(min_delta, -pi/2)
00214 self.assertAlmostEqual(max_delta, 3*pi/2)
00215
00216
00217 flag, min_delta, max_delta = _find_min_max_delta( 0, -pi/2, pi/2)
00218 self.assertTrue(flag)
00219 self.assertAlmostEqual(min_delta, -pi/2)
00220 self.assertAlmostEqual(max_delta, pi/2)
00221
00222
00223 flag, min_delta, max_delta = _find_min_max_delta( pi/4, -pi/2, pi/2)
00224 self.assertTrue(flag)
00225 self.assertAlmostEqual(min_delta, -3*pi/4)
00226 self.assertAlmostEqual(max_delta, pi/4)
00227
00228
00229 flag, min_delta, max_delta = _find_min_max_delta( -pi/4, -pi/2, pi/2)
00230 self.assertTrue(flag)
00231 self.assertAlmostEqual(min_delta, -pi/4)
00232 self.assertAlmostEqual(max_delta, 3*pi/4)
00233
00234
00235 flag, min_delta, max_delta = _find_min_max_delta( -pi, -pi, pi)
00236 self.assertTrue(flag)
00237 self.assertTrue((fabs(min_delta) <= epsilon and fabs(max_delta - 2*pi) <= epsilon) or (fabs(min_delta+2*pi) <= epsilon and fabs(max_delta) <= epsilon))
00238 self.assertAlmostEqual(min_delta, 0.0)
00239 self.assertAlmostEqual(max_delta, 2*pi)
00240
00241 flag, min_delta, max_delta = _find_min_max_delta(-0.25,0.25,-0.25)
00242 self.assertTrue(flag)
00243 self.assertAlmostEqual(min_delta, -2*pi+0.5)
00244 self.assertAlmostEqual(max_delta, 0.0)
00245
00246
00247 flag, min_delta, max_delta = _find_min_max_delta( pi-epsilon, -pi, pi)
00248 self.assertTrue(flag)
00249
00250 self.assertAlmostEqual(min_delta, -2*pi+epsilon)
00251 self.assertAlmostEqual(max_delta, epsilon)
00252
00253
00254 flag, min_delta, max_delta = _find_min_max_delta( -pi, -pi, pi)
00255 self.assertTrue(flag)
00256 self.assertAlmostEqual(min_delta, 0)
00257 self.assertAlmostEqual(max_delta, 2*pi)
00258
00259
00260 flag, min_delta, max_delta = _find_min_max_delta( -pi/2, -pi/2, pi/2)
00261 self.assertTrue(flag)
00262 self.assertAlmostEqual(min_delta, 0.0)
00263 self.assertAlmostEqual(max_delta, pi)
00264
00265
00266 flag, min_delta, max_delta = _find_min_max_delta( -pi, -pi/2, pi/2)
00267 self.assertFalse(flag)
00268
00269 flag, min_delta, max_delta = _find_min_max_delta( pi, -pi/2, pi/2)
00270 self.assertFalse(flag)
00271
00272
00273 flag, min_delta, max_delta = _find_min_max_delta( 3*pi/4, pi/2, -pi/2)
00274 self.assertTrue(flag)
00275 self.assertAlmostEqual(min_delta, -pi/4)
00276 self.assertAlmostEqual(max_delta, 3*pi/4)
00277
00278 if __name__ == '__main__':
00279 import rosunit
00280 rosunit.unitrun('angles', 'test_python_angles', TestAngles)