Add frenet tests to test track

tests/test_track.py

@@ -138,3 +138,26 @@ def test_download_racetrack(self):
         # rename the backup track dir to its original name
         track_backup_dir = find_track_dir(tmp_dir.stem)
         track_backup_dir.rename(track_dir)
+
+    def test_frenet_to_cartesian(self):
+        track_name = "Spielberg"
+        track = Track.from_track_name(track_name)
+
+        # 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):
+            x_, y_, _ = track.frenet_to_cartesian(s, 0, 0)
+            self.assertAlmostEqual(x, x_, places=2)
+            self.assertAlmostEqual(y, y_, places=2)
+
+    def test_frenet_to_cartesian_to_frenet(self):
+        track_name = "Spielberg"
+        track = Track.from_track_name(track_name)
+
+        # check frenet to cartesian conversion
+        s_ = 0
+        for s in np.linspace(0, 1, 10):
+            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)