From 1b520179a558540bab171a8bd53d92bb3e410bcc Mon Sep 17 00:00:00 2001 From: "Berducci, Luigi" Date: Fri, 14 Jun 2024 16:59:51 +0200 Subject: [PATCH] add test frenet-cartesian-frenet with non-zero lateral offset --- tests/test_track.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) 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)