diff --git a/tests/test_track.py b/tests/test_track.py index 3c610229..97ac6e7c 100644 --- a/tests/test_track.py +++ b/tests/test_track.py @@ -145,7 +145,9 @@ def test_frenet_to_cartesian(self): # Check frenet to cartesian conversion # using the track's xs, ys - for s, x, y in zip(track.centerline.ss, track.centerline.xs, track.centerline.ys): + for s, x, y in zip( + track.centerline.ss, track.centerline.xs, track.centerline.ys + ): x_, y_, _ = track.frenet_to_cartesian(s, 0, 0) self.assertAlmostEqual(x, x_, places=2) self.assertAlmostEqual(y, y_, places=2) @@ -160,4 +162,14 @@ def test_frenet_to_cartesian_to_frenet(self): x, y, psi = track.frenet_to_cartesian(s, 0, 0) s_, d, _ = track.cartesian_to_frenet(x, y, psi, s_guess=s_) self.assertAlmostEqual(s, s_, places=2) - self.assertAlmostEqual(d, 0, places=2) \ No newline at end of file + self.assertAlmostEqual(d, 0, places=2) + + # check frenet to cartesian conversion + # with non-zero lateral offset + s_ = 0 + for s in np.linspace(0, 1, 10): + d = np.random.uniform(-1.0, 1.0) + x, y, psi = track.frenet_to_cartesian(s, d, 0) + s_, d_, _ = track.cartesian_to_frenet(x, y, psi, s_guess=s_) + self.assertAlmostEqual(s, s_, places=2) + self.assertAlmostEqual(d, d_, places=2)