From 2a1b81468e962eba39c86517cdabeeb4bc1f88a8 Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Mon, 18 Mar 2024 10:22:43 -0400
Subject: [PATCH 01/22] Implement calc arclength using CubicSpline1D

---
 gym/f110_gym/envs/cubic_spline.py | 27 +++++++++++++++++++++++++++
 1 file changed, 27 insertions(+)

diff --git a/gym/f110_gym/envs/cubic_spline.py b/gym/f110_gym/envs/cubic_spline.py
index 8b1a0fab..6240d0e9 100644
--- a/gym/f110_gym/envs/cubic_spline.py
+++ b/gym/f110_gym/envs/cubic_spline.py
@@ -6,6 +6,8 @@
 import math
 
 import numpy as np
+from scipy.spatial import distance as ssd
+import scipy.optimize as so
 
 
 class CubicSpline1D:
@@ -225,3 +227,28 @@ def calc_yaw(self, s):
         dy = self.sy.calc_first_derivative(s)
         yaw = math.atan2(dy, dx)
         return yaw
+
+    def calc_arclength(self, x, y):
+        """
+        calc arclength
+        Parameters
+        ----------
+        x : float
+            x position.
+        y : float
+            y position.
+        Returns
+        -------
+        s : float
+            distance from the start point for given x, y.
+        ey : float
+            lateral deviation for given x, y.
+        """
+
+        def distance_to_spline(s):
+            x_eval = self.sx.calc_position(s)
+            y_eval = self.sy.calc_position(s)
+            return ssd.euclidean([x_eval, y_eval], [x, y])
+        
+        closest_s = so.fminbound(distance_to_spline, 0, self.s[-1])
+        return closest_s, distance_to_spline(closest_s)
\ No newline at end of file

From 7a9fa7dcc0f9673ae3a7fc3de7fd951876ea1564 Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Mon, 18 Mar 2024 10:23:14 -0400
Subject: [PATCH 02/22] Implement cartesian to frenet transformation

---
 gym/f110_gym/envs/track.py | 43 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 43 insertions(+)

diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
index 541994bf..7c9d92a7 100644
--- a/gym/f110_gym/envs/track.py
+++ b/gym/f110_gym/envs/track.py
@@ -242,7 +242,50 @@ def from_track_name(track: str):
             print(ex)
             raise FileNotFoundError(f"could not load track {track}") from ex
 
+    def frenet_to_cartesian(self, s, ey, ephi):
+        """
+        Convert Frenet coordinates to Cartesian coordinates.
+        
+        s: distance along the raceline
+        ey: lateral deviation
+        ephi: heading deviation
+
+        returns:
+            x: x-coordinate
+            y: y-coordinate
+            psi: yaw angle
+        """
+        x, y = self.raceline.spline.calc_position(s)
+        psi = self.raceline.spline.calc_yaw(s)
+
+        # Adjust x,y by shifting along the normal vector
+        x -= ey * np.sin(psi)
+        y += ey * np.cos(psi)
+
+        # Adjust psi by adding the heading deviation
+        psi += ephi
+
+        return x, y, psi
+    
+    def cartesian_to_frenet(self, x, y, phi):
+        """
+        Convert Cartesian coordinates to Frenet coordinates.
+        
+        x: x-coordinate
+        y: y-coordinate
+        phi: yaw angle
+
+        returns:
+            s: distance along the raceline
+            ey: lateral deviation
+            ephi: heading deviation
+        """
+        s, ey = self.raceline.spline.calc_arclength(x, y)
+        phi = phi - self.raceline.spline.calc_yaw(s)
+
+        return s, ey, phi
 
+    
 if __name__ == "__main__":
     track = Track.from_track_name("Example")
     print("[Result] map loaded successfully")

From a9d10d960be28143fa8c442087ac5657f3c3e2bd Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Mon, 18 Mar 2024 10:43:31 -0400
Subject: [PATCH 03/22] Implement normal and tangent utils for 2d cubic spline

---
 gym/f110_gym/envs/cubic_spline.py | 42 ++++++++++++++++++++++++++++++-
 1 file changed, 41 insertions(+), 1 deletion(-)

diff --git a/gym/f110_gym/envs/cubic_spline.py b/gym/f110_gym/envs/cubic_spline.py
index 6240d0e9..64d46337 100644
--- a/gym/f110_gym/envs/cubic_spline.py
+++ b/gym/f110_gym/envs/cubic_spline.py
@@ -251,4 +251,44 @@ def distance_to_spline(s):
             return ssd.euclidean([x_eval, y_eval], [x, y])
         
         closest_s = so.fminbound(distance_to_spline, 0, self.s[-1])
-        return closest_s, distance_to_spline(closest_s)
\ No newline at end of file
+        absolute_distance = distance_to_spline(closest_s)
+        return closest_s, absolute_distance
+    
+    def _calc_tangent(self, s):
+        '''
+        calculates the tangent to the curve at a given point
+        Parameters
+        ----------
+        s : float
+            distance from the start point. if `s` is outside the data point's
+            range, return None.
+        Returns
+        -------
+        tangent : float
+            tangent vector for given s.
+        '''
+        if s < 0:
+            return None
+        elif s > self.s[-1]:
+            return None
+        dx = self.sx.calc_first_derivative(s)
+        dy = self.sy.calc_first_derivative(s)
+        tangent = np.array([dx, dy])
+        return tangent
+    
+    def _calc_normal(self, s):
+        '''
+        calculates the normal to the curve at a given point
+        Parameters
+        ----------
+        s : float
+            distance from the start point. if `s` is outside the data point's
+            range, return None.
+        Returns
+        -------
+        normal : float
+            normal vector for given s.
+        '''
+        tangent = self._calc_tangent(s)
+        normal = np.array([-tangent[1], tangent[0]])
+        return normal
\ No newline at end of file

From 0114dc49c9082ea4e3266dac07eb71689324e4f5 Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Mon, 18 Mar 2024 10:44:04 -0400
Subject: [PATCH 04/22] Implement frenet to cartesian to frenet transformations

---
 gym/f110_gym/envs/track.py | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
index 7c9d92a7..10ea753d 100644
--- a/gym/f110_gym/envs/track.py
+++ b/gym/f110_gym/envs/track.py
@@ -276,12 +276,22 @@ def cartesian_to_frenet(self, x, y, phi):
         phi: yaw angle
 
         returns:
-            s: distance along the raceline
+            s: distance along the centerline
             ey: lateral deviation
             ephi: heading deviation
         """
-        s, ey = self.raceline.spline.calc_arclength(x, y)
-        phi = phi - self.raceline.spline.calc_yaw(s)
+        s, ey = self.centerline.spline.calc_arclength(x, y)
+        
+        # Use the normal to calculate the signed lateral deviation
+        normal = self.centerline.spline._calc_normal(s)
+        x_eval = self.centerline.spline.sx.calc_position(s)
+        y_eval = self.centerline.spline.sy.calc_position(s)
+        dx = x - x_eval
+        dy = y - y_eval
+        distance_sign = np.sign(np.dot([dx, dy], normal))
+        ey = ey * distance_sign
+
+        phi = phi - self.centerline.spline.calc_yaw(s)
 
         return s, ey, phi
 

From eb10153843bece54346cb7382f9feea304a46e41 Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Mon, 18 Mar 2024 13:22:16 -0400
Subject: [PATCH 05/22] Now uses fmin instead of fminbound

---
 gym/f110_gym/envs/cubic_spline.py | 18 +++++++++++++-----
 1 file changed, 13 insertions(+), 5 deletions(-)

diff --git a/gym/f110_gym/envs/cubic_spline.py b/gym/f110_gym/envs/cubic_spline.py
index 64d46337..55111508 100644
--- a/gym/f110_gym/envs/cubic_spline.py
+++ b/gym/f110_gym/envs/cubic_spline.py
@@ -246,12 +246,20 @@ def calc_arclength(self, x, y):
         """
 
         def distance_to_spline(s):
-            x_eval = self.sx.calc_position(s)
-            y_eval = self.sy.calc_position(s)
-            return ssd.euclidean([x_eval, y_eval], [x, y])
+            if s < 0:
+                x_eval = self.sx.calc_position(0)
+                y_eval = self.sy.calc_position(0)
+            elif s > self.s[-1]:
+                x_eval = self.sx.calc_position(self.s[-1])
+                y_eval = self.sy.calc_position(self.s[-1])
+            else:
+                x_eval = self.sx.calc_position(s)
+                y_eval = self.sy.calc_position(s)
+            return np.sqrt((x - x_eval)**2 + (y - y_eval)**2)
         
-        closest_s = so.fminbound(distance_to_spline, 0, self.s[-1])
-        absolute_distance = distance_to_spline(closest_s)
+        output = so.fmin(distance_to_spline, 0, full_output=True, disp=False)
+        closest_s = output[0][0]
+        absolute_distance = output[1]
         return closest_s, absolute_distance
     
     def _calc_tangent(self, s):

From ec9834d8b65547b7b6e40e914edae1bb697c7be4 Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Thu, 4 Apr 2024 00:50:42 -0400
Subject: [PATCH 06/22] Now uses scipy cubicspline for interpolation with
 proper endpoint check. Works for full track

---
 gym/f110_gym/envs/cubic_spline.py | 195 ++++--------------------------
 gym/f110_gym/envs/track.py        |  17 +--
 2 files changed, 31 insertions(+), 181 deletions(-)

diff --git a/gym/f110_gym/envs/cubic_spline.py b/gym/f110_gym/envs/cubic_spline.py
index 55111508..aedf7435 100644
--- a/gym/f110_gym/envs/cubic_spline.py
+++ b/gym/f110_gym/envs/cubic_spline.py
@@ -1,151 +1,13 @@
 """
-Code from Cubic spline planner
-Author: Atsushi Sakai(@Atsushi_twi)
+Cubic Spline interpolation using scipy.interpolate
+Provides utilities for position, curvature, yaw, and arclength calculation
 """
-import bisect
+
 import math
 
 import numpy as np
-from scipy.spatial import distance as ssd
 import scipy.optimize as so
-
-
-class CubicSpline1D:
-    """
-    1D Cubic Spline class
-    Parameters
-    ----------
-    x : list
-        x coordinates for data points. This x coordinates must be
-        sorted in ascending order.
-    y : list
-        y coordinates for data points
-    """
-
-    def __init__(self, x, y):
-        h = np.diff(x)
-        if np.any(h < 0):
-            raise ValueError("x coordinates must be sorted in ascending order")
-
-        self.a, self.b, self.c, self.d = [], [], [], []
-        self.x = x
-        self.y = y
-        self.nx = len(x)  # dimension of x
-
-        # calc coefficient a
-        self.a = [iy for iy in y]
-
-        # calc coefficient c
-        A = self.__calc_A(h)
-        B = self.__calc_B(h, self.a)
-        self.c = np.linalg.solve(A, B)
-
-        # calc spline coefficient b and d
-        for i in range(self.nx - 1):
-            d = (self.c[i + 1] - self.c[i]) / (3.0 * h[i])
-            b = 1.0 / h[i] * (self.a[i + 1] - self.a[i]) - h[i] / 3.0 * (
-                2.0 * self.c[i] + self.c[i + 1]
-            )
-            self.d.append(d)
-            self.b.append(b)
-
-    def calc_position(self, x):
-        """
-        Calc `y` position for given `x`.
-        if `x` is outside the data point's `x` range, return None.
-        Returns
-        -------
-        y : float
-            y position for given x.
-        """
-        if x < self.x[0]:
-            return None
-        elif x > self.x[-1]:
-            return None
-
-        i = self.__search_index(x)
-        dx = x - self.x[i]
-        position = (
-            self.a[i] + self.b[i] * dx + self.c[i] * dx**2.0 + self.d[i] * dx**3.0
-        )
-
-        return position
-
-    def calc_first_derivative(self, x):
-        """
-        Calc first derivative at given x.
-        if x is outside the input x, return None
-        Returns
-        -------
-        dy : float
-            first derivative for given x.
-        """
-
-        if x < self.x[0]:
-            return None
-        elif x > self.x[-1]:
-            return None
-
-        i = self.__search_index(x)
-        dx = x - self.x[i]
-        dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx**2.0
-        return dy
-
-    def calc_second_derivative(self, x):
-        """
-        Calc second derivative at given x.
-        if x is outside the input x, return None
-        Returns
-        -------
-        ddy : float
-            second derivative for given x.
-        """
-
-        if x < self.x[0]:
-            return None
-        elif x > self.x[-1]:
-            return None
-
-        i = self.__search_index(x)
-        dx = x - self.x[i]
-        ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
-        return ddy
-
-    def __search_index(self, x):
-        """
-        search data segment index
-        """
-        return bisect.bisect(self.x[:-1], x) - 1
-
-    def __calc_A(self, h):
-        """
-        calc matrix A for spline coefficient c
-        """
-        A = np.zeros((self.nx, self.nx))
-        A[0, 0] = 1.0
-        for i in range(self.nx - 1):
-            if i != (self.nx - 2):
-                A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
-            A[i + 1, i] = h[i]
-            A[i, i + 1] = h[i]
-
-        A[0, 1] = 0.0
-        A[self.nx - 1, self.nx - 2] = 0.0
-        A[self.nx - 1, self.nx - 1] = 1.0
-        return A
-
-    def __calc_B(self, h, a):
-        """
-        calc matrix B for spline coefficient c
-        """
-        B = np.zeros(self.nx)
-        for i in range(self.nx - 2):
-            B[i + 1] = (
-                3.0 * (a[i + 2] - a[i + 1]) / h[i + 1] - 3.0 * (a[i + 1] - a[i]) / h[i]
-            )
-        return B
-
-
+from scipy import interpolate
 class CubicSpline2D:
     """
     Cubic CubicSpline2D class
@@ -158,9 +20,13 @@ class CubicSpline2D:
     """
 
     def __init__(self, x, y):
-        self.s = self.__calc_s(x, y)
-        self.sx = CubicSpline1D(self.s, x)
-        self.sy = CubicSpline1D(self.s, y)
+        points = np.c_[x, y]
+        if not np.all(points[-1] == points[0]):
+            points = np.vstack((points, points[0]))  # Ensure the path is closed
+        self.s = self.__calc_s(points[:, 0], points[:, 1])
+        # Use scipy CubicSpline to interpolate the points with periodic boundary conditions
+        # This is necessary to ensure the path is continuous
+        self.spline = interpolate.CubicSpline(self.s, points, bc_type='periodic')
 
     def __calc_s(self, x, y):
         dx = np.diff(x)
@@ -185,10 +51,7 @@ def calc_position(self, s):
         y : float
             y position for given s.
         """
-        x = self.sx.calc_position(s)
-        y = self.sy.calc_position(s)
-
-        return x, y
+        return self.spline(s)
 
     def calc_curvature(self, s):
         """
@@ -203,10 +66,8 @@ def calc_curvature(self, s):
         k : float
             curvature for given s.
         """
-        dx = self.sx.calc_first_derivative(s)
-        ddx = self.sx.calc_second_derivative(s)
-        dy = self.sy.calc_first_derivative(s)
-        ddy = self.sy.calc_second_derivative(s)
+        dx, dy = self.spline(s, 1)
+        ddx, ddy = self.spline(s, 2)
         k = (ddy * dx - ddx * dy) / ((dx**2 + dy**2) ** (3 / 2))
         return k
 
@@ -223,12 +84,11 @@ def calc_yaw(self, s):
         yaw : float
             yaw angle (tangent vector) for given s.
         """
-        dx = self.sx.calc_first_derivative(s)
-        dy = self.sy.calc_first_derivative(s)
+        dx, dy = self.spline(s, 1)
         yaw = math.atan2(dy, dx)
         return yaw
 
-    def calc_arclength(self, x, y):
+    def calc_arclength(self, x, y, s_guess=0.0):
         """
         calc arclength
         Parameters
@@ -246,18 +106,10 @@ def calc_arclength(self, x, y):
         """
 
         def distance_to_spline(s):
-            if s < 0:
-                x_eval = self.sx.calc_position(0)
-                y_eval = self.sy.calc_position(0)
-            elif s > self.s[-1]:
-                x_eval = self.sx.calc_position(self.s[-1])
-                y_eval = self.sy.calc_position(self.s[-1])
-            else:
-                x_eval = self.sx.calc_position(s)
-                y_eval = self.sy.calc_position(s)
+            x_eval, y_eval = self.spline(s)[0]
             return np.sqrt((x - x_eval)**2 + (y - y_eval)**2)
         
-        output = so.fmin(distance_to_spline, 0, full_output=True, disp=False)
+        output = so.fmin(distance_to_spline, s_guess, full_output=True, disp=False)
         closest_s = output[0][0]
         absolute_distance = output[1]
         return closest_s, absolute_distance
@@ -275,12 +127,7 @@ def _calc_tangent(self, s):
         tangent : float
             tangent vector for given s.
         '''
-        if s < 0:
-            return None
-        elif s > self.s[-1]:
-            return None
-        dx = self.sx.calc_first_derivative(s)
-        dy = self.sy.calc_first_derivative(s)
+        dx, dy = self.spline(s, 1)
         tangent = np.array([dx, dy])
         return tangent
     
@@ -297,6 +144,6 @@ def _calc_normal(self, s):
         normal : float
             normal vector for given s.
         '''
-        tangent = self._calc_tangent(s)
-        normal = np.array([-tangent[1], tangent[0]])
+        dx, dy = self.spline(s, 1)
+        normal = np.array([-dy, dx])
         return normal
\ No newline at end of file
diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
index 10ea753d..8cabfb49 100644
--- a/gym/f110_gym/envs/track.py
+++ b/gym/f110_gym/envs/track.py
@@ -255,8 +255,8 @@ def frenet_to_cartesian(self, s, ey, ephi):
             y: y-coordinate
             psi: yaw angle
         """
-        x, y = self.raceline.spline.calc_position(s)
-        psi = self.raceline.spline.calc_yaw(s)
+        x, y = self.centerline.spline.calc_position(s)
+        psi = self.centerline.spline.calc_yaw(s)
 
         # Adjust x,y by shifting along the normal vector
         x -= ey * np.sin(psi)
@@ -267,7 +267,7 @@ def frenet_to_cartesian(self, s, ey, ephi):
 
         return x, y, psi
     
-    def cartesian_to_frenet(self, x, y, phi):
+    def cartesian_to_frenet(self, x, y, phi, s_guess=0):
         """
         Convert Cartesian coordinates to Frenet coordinates.
         
@@ -280,12 +280,15 @@ def cartesian_to_frenet(self, x, y, phi):
             ey: lateral deviation
             ephi: heading deviation
         """
-        s, ey = self.centerline.spline.calc_arclength(x, y)
-        
+        s, ey = self.centerline.spline.calc_arclength(x, y, s_guess)
+        if abs(s - self.centerline.spline.s[-1]) < 1e-6 or s > self.centerline.spline.s[-1]:
+            s = 0
+        if s < 0:
+            s = self.centerline.spline.s[-1]
+
         # Use the normal to calculate the signed lateral deviation
         normal = self.centerline.spline._calc_normal(s)
-        x_eval = self.centerline.spline.sx.calc_position(s)
-        y_eval = self.centerline.spline.sy.calc_position(s)
+        x_eval, y_eval = self.centerline.spline.calc_position(s)
         dx = x - x_eval
         dy = y - y_eval
         distance_sign = np.sign(np.dot([dx, dy], normal))

From 1cba4a3bb7b3486b062cff6accbf3ea240c3be64 Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Thu, 4 Apr 2024 01:35:24 -0400
Subject: [PATCH 07/22] Add inaccurate version of cart2frenet for use in lap
 counting

---
 gym/f110_gym/envs/cubic_spline.py | 75 ++++++++++++++++++++++++++++---
 gym/f110_gym/envs/track.py        |  8 ++--
 2 files changed, 75 insertions(+), 8 deletions(-)

diff --git a/gym/f110_gym/envs/cubic_spline.py b/gym/f110_gym/envs/cubic_spline.py
index aedf7435..37ddda89 100644
--- a/gym/f110_gym/envs/cubic_spline.py
+++ b/gym/f110_gym/envs/cubic_spline.py
@@ -8,6 +8,47 @@
 import numpy as np
 import scipy.optimize as so
 from scipy import interpolate
+from numba import njit
+
+@njit(fastmath=False, cache=True)
+def nearest_point_on_trajectory(point, trajectory):
+    """
+    Return the nearest point along the given piecewise linear trajectory.
+
+    Same as nearest_point_on_line_segment, but vectorized. This method is quite fast, time constraints should
+    not be an issue so long as trajectories are not insanely long.
+
+        Order of magnitude: trajectory length: 1000 --> 0.0002 second computation (5000fps)
+
+    point: size 2 numpy array
+    trajectory: Nx2 matrix of (x,y) trajectory waypoints
+        - these must be unique. If they are not unique, a divide by 0 error will destroy the world
+    """
+    diffs = trajectory[1:, :] - trajectory[:-1, :]
+    l2s = diffs[:, 0] ** 2 + diffs[:, 1] ** 2
+    # this is equivalent to the elementwise dot product
+    # dots = np.sum((point - trajectory[:-1,:]) * diffs[:,:], axis=1)
+    dots = np.empty((trajectory.shape[0] - 1,))
+    for i in range(dots.shape[0]):
+        dots[i] = np.dot((point - trajectory[i, :]), diffs[i, :])
+    t = dots / l2s
+    t[t < 0.0] = 0.0
+    t[t > 1.0] = 1.0
+    # t = np.clip(dots / l2s, 0.0, 1.0)
+    projections = trajectory[:-1, :] + (t * diffs.T).T
+    # dists = np.linalg.norm(point - projections, axis=1)
+    dists = np.empty((projections.shape[0],))
+    for i in range(dists.shape[0]):
+        temp = point - projections[i]
+        dists[i] = np.sqrt(np.sum(temp * temp))
+    min_dist_segment = np.argmin(dists)
+    return (
+        projections[min_dist_segment],
+        dists[min_dist_segment],
+        t[min_dist_segment],
+        min_dist_segment,
+    )
+
 class CubicSpline2D:
     """
     Cubic CubicSpline2D class
@@ -20,13 +61,13 @@ class CubicSpline2D:
     """
 
     def __init__(self, x, y):
-        points = np.c_[x, y]
-        if not np.all(points[-1] == points[0]):
-            points = np.vstack((points, points[0]))  # Ensure the path is closed
-        self.s = self.__calc_s(points[:, 0], points[:, 1])
+        self.points = np.c_[x, y]
+        if not np.all(self.points[-1] == self.points[0]):
+            self.points = np.vstack((self.points, self.points[0]))  # Ensure the path is closed
+        self.s = self.__calc_s(self.points[:, 0], self.points[:, 1])
         # Use scipy CubicSpline to interpolate the points with periodic boundary conditions
         # This is necessary to ensure the path is continuous
-        self.spline = interpolate.CubicSpline(self.s, points, bc_type='periodic')
+        self.spline = interpolate.CubicSpline(self.s, self.points, bc_type='periodic')
 
     def __calc_s(self, x, y):
         dx = np.diff(x)
@@ -114,6 +155,30 @@ def distance_to_spline(s):
         absolute_distance = output[1]
         return closest_s, absolute_distance
     
+    def calc_arclength_inaccurate(self, x, y, s_guess=0.0):
+        """
+        calc arclength, use nearest_point_on_trajectory
+        Less accuarate and less smooth than calc_arclength but
+        much faster - suitable for lap counting
+        Parameters
+        ----------
+        x : float
+            x position.
+        y : float
+            y position.
+        Returns
+        -------
+        s : float
+            distance from the start point for given x, y.
+        ey : float
+            lateral deviation for given x, y.
+        """
+        _, ey, t, min_dist_segment = nearest_point_on_trajectory(np.array([x, y]), self.points)
+        # s = s at closest_point + t
+        s = self.s[min_dist_segment] + t * (self.s[min_dist_segment + 1] - self.s[min_dist_segment])
+
+        return s, 0
+
     def _calc_tangent(self, s):
         '''
         calculates the tangent to the curve at a given point
diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
index 8cabfb49..d2d1fe50 100644
--- a/gym/f110_gym/envs/track.py
+++ b/gym/f110_gym/envs/track.py
@@ -281,10 +281,12 @@ def cartesian_to_frenet(self, x, y, phi, s_guess=0):
             ephi: heading deviation
         """
         s, ey = self.centerline.spline.calc_arclength(x, y, s_guess)
-        if abs(s - self.centerline.spline.s[-1]) < 1e-6 or s > self.centerline.spline.s[-1]:
-            s = 0
+        if s > self.centerline.spline.s[-1]:
+            # Wrap around
+            s = s - self.centerline.spline.s[-1]
         if s < 0:
-            s = self.centerline.spline.s[-1]
+            # Negative s means we are behind the start point
+            s = s + self.centerline.spline.s[-1]
 
         # Use the normal to calculate the signed lateral deviation
         normal = self.centerline.spline._calc_normal(s)

From 24b327b45040fb88b6452db1ca583b14f1f80ca1 Mon Sep 17 00:00:00 2001
From: Ahmad Amine <ahmadamine998@gmail.com>
Date: Fri, 31 May 2024 01:35:29 -0400
Subject: [PATCH 08/22] Frenet cartesian conversion for track

---
 f1tenth_gym/envs/track/track.py | 58 +++++++++++++++++++++++++++++++++
 1 file changed, 58 insertions(+)

diff --git a/f1tenth_gym/envs/track/track.py b/f1tenth_gym/envs/track/track.py
index d0bd6585..52e722c6 100644
--- a/f1tenth_gym/envs/track/track.py
+++ b/f1tenth_gym/envs/track/track.py
@@ -237,3 +237,61 @@ def from_refline(x: np.ndarray, y: np.ndarray, velx: np.ndarray,):
             raceline=refline,
             centerline=refline,
         )
+
+    def frenet_to_cartesian(self, s, ey, ephi):
+        """
+        Convert Frenet coordinates to Cartesian coordinates.
+        
+        s: distance along the raceline
+        ey: lateral deviation
+        ephi: heading deviation
+
+        returns:
+            x: x-coordinate
+            y: y-coordinate
+            psi: yaw angle
+        """
+        x, y = self.centerline.spline.calc_position(s)
+        psi = self.centerline.spline.calc_yaw(s)
+
+        # Adjust x,y by shifting along the normal vector
+        x -= ey * np.sin(psi)
+        y += ey * np.cos(psi)
+
+        # Adjust psi by adding the heading deviation
+        psi += ephi
+
+        return x, y, psi
+    
+    def cartesian_to_frenet(self, x, y, phi, s_guess=0):
+        """
+        Convert Cartesian coordinates to Frenet coordinates.
+        
+        x: x-coordinate
+        y: y-coordinate
+        phi: yaw angle
+
+        returns:
+            s: distance along the centerline
+            ey: lateral deviation
+            ephi: heading deviation
+        """
+        s, ey = self.centerline.spline.calc_arclength(x, y, s_guess)
+        if s > self.centerline.spline.s[-1]:
+            # Wrap around
+            s = s - self.centerline.spline.s[-1]
+        if s < 0:
+            # Negative s means we are behind the start point
+            s = s + self.centerline.spline.s[-1]
+
+        # Use the normal to calculate the signed lateral deviation
+        normal = self.centerline.spline._calc_normal(s)
+        x_eval, y_eval = self.centerline.spline.calc_position(s)
+        dx = x - x_eval
+        dy = y - y_eval
+        distance_sign = np.sign(np.dot([dx, dy], normal))
+        ey = ey * distance_sign
+
+        phi = phi - self.centerline.spline.calc_yaw(s)
+
+        return s, ey, phi

From 088220150a274fcb52c9aa3e6dc710efb26ced6d Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Fri, 7 Jun 2024 02:12:02 -0400
Subject: [PATCH 09/22] calc_yaw now returns [0, 2pi] angles

---
 f1tenth_gym/envs/track/cubic_spline.py | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/f1tenth_gym/envs/track/cubic_spline.py b/f1tenth_gym/envs/track/cubic_spline.py
index 9ce46a3c..36fb3c08 100644
--- a/f1tenth_gym/envs/track/cubic_spline.py
+++ b/f1tenth_gym/envs/track/cubic_spline.py
@@ -149,6 +149,9 @@ def calc_yaw(self, s: float) -> float | None:
         """
         dx, dy = self.spline(s, 1)
         yaw = math.atan2(dy, dx)
+        # Convert yaw to [0, 2pi]
+        yaw = yaw % (2 * math.pi)
+        
         return yaw
 
     def calc_arclength(self, x, y, s_guess=0.0):

From 7b3d297407987f9908a8f4e5d7ac250e1394d2a6 Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Fri, 7 Jun 2024 02:18:20 -0400
Subject: [PATCH 10/22] Add curvature test

---
 tests/test_frenet_utils.py | 24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)
 create mode 100644 tests/test_frenet_utils.py

diff --git a/tests/test_frenet_utils.py b/tests/test_frenet_utils.py
new file mode 100644
index 00000000..78fa53d7
--- /dev/null
+++ b/tests/test_frenet_utils.py
@@ -0,0 +1,24 @@
+import pathlib
+import time
+import unittest
+
+import numpy as np
+import os
+import sys
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+from f1tenth_gym.envs.track import Track, cubic_spline
+
+class TestFrenet(unittest.TestCase):
+    def test_calc_curvature(self):
+        circle_x = np.cos(np.linspace(0, 2 * np.pi, 100))[:-1]
+        circle_y = np.sin(np.linspace(0, 2 * np.pi, 100))[:-1]
+        track = cubic_spline.CubicSpline2D(circle_x, circle_y)
+        # Test the curvature at the four corners of the circle
+        # The curvature of a circle is 1/radius
+        self.assertAlmostEqual(track.calc_curvature(0), 1, places=3)
+        self.assertAlmostEqual(track.calc_curvature(np.pi / 2), 1, places=3)
+        self.assertAlmostEqual(track.calc_curvature(np.pi), 1, places=3)
+        self.assertAlmostEqual(track.calc_curvature(3 * np.pi / 2), 1, places=3)
+
+if __name__ == "__main__":
+    TestFrenet().test_calc_curvature()
\ No newline at end of file

From 350373f2e64f516bd39690f602b8b70504feb2df Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Fri, 7 Jun 2024 02:18:35 -0400
Subject: [PATCH 11/22] Add yaw test

---
 tests/test_frenet_utils.py | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/tests/test_frenet_utils.py b/tests/test_frenet_utils.py
index 78fa53d7..0ab79ccc 100644
--- a/tests/test_frenet_utils.py
+++ b/tests/test_frenet_utils.py
@@ -20,5 +20,17 @@ def test_calc_curvature(self):
         self.assertAlmostEqual(track.calc_curvature(np.pi), 1, places=3)
         self.assertAlmostEqual(track.calc_curvature(3 * np.pi / 2), 1, places=3)
 
+    def test_calc_yaw(self):
+        circle_x = np.cos(np.linspace(0, 2 * np.pi, 100))[:-1]
+        circle_y = np.sin(np.linspace(0, 2 * np.pi, 100))[:-1]
+        track = cubic_spline.CubicSpline2D(circle_x, circle_y)
+        # Test the yaw at the four corners of the circle
+        # The yaw of a circle is s + pi/2
+        self.assertAlmostEqual(track.calc_yaw(0), np.pi / 2, places=2)
+        self.assertAlmostEqual(track.calc_yaw(np.pi / 2), np.pi, places=2)
+        self.assertAlmostEqual(track.calc_yaw(np.pi), 3 * np.pi / 2, places=2)
+        self.assertAlmostEqual(track.calc_yaw(3 * np.pi / 2), 0, places=2)
+    
 if __name__ == "__main__":
-    TestFrenet().test_calc_curvature()
\ No newline at end of file
+    TestFrenet().test_calc_curvature()
+    TestFrenet().test_calc_yaw()
\ No newline at end of file

From 446569bc9c0cf8428d31ad36f4ef5168e7cfa8ef Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Fri, 7 Jun 2024 02:40:21 -0400
Subject: [PATCH 12/22] Add test for calc position

---
 tests/test_frenet_utils.py | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/tests/test_frenet_utils.py b/tests/test_frenet_utils.py
index 0ab79ccc..d69de936 100644
--- a/tests/test_frenet_utils.py
+++ b/tests/test_frenet_utils.py
@@ -31,6 +31,18 @@ def test_calc_yaw(self):
         self.assertAlmostEqual(track.calc_yaw(np.pi), 3 * np.pi / 2, places=2)
         self.assertAlmostEqual(track.calc_yaw(3 * np.pi / 2), 0, places=2)
     
+    def test_calc_position(self):
+        circle_x = np.cos(np.linspace(0, 2 * np.pi, 100))[:-1]
+        circle_y = np.sin(np.linspace(0, 2 * np.pi, 100))[:-1]
+        track = cubic_spline.CubicSpline2D(circle_x, circle_y)
+        # Test the position at the four corners of the circle
+        # The position of a circle is (x, y) = (cos(s), sin(s))
+        self.assertTrue(np.allclose(track.calc_position(0), np.array([1, 0]), atol=1e-3))
+        self.assertTrue(np.allclose(track.calc_position(np.pi / 2), np.array([0, 1]), atol=1e-3))
+        self.assertTrue(np.allclose(track.calc_position(np.pi), np.array([-1, 0]), atol=1e-3))
+        self.assertTrue(np.allclose(track.calc_position(3 * np.pi / 2), np.array([0, -1]), atol=1e-3))
+
 if __name__ == "__main__":
     TestFrenet().test_calc_curvature()
-    TestFrenet().test_calc_yaw()
\ No newline at end of file
+    TestFrenet().test_calc_yaw()
+    TestFrenet().test_calc_position()
\ No newline at end of file

From e6d95c95026ef24f66eb313dcecc7bbc44700e89 Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Fri, 7 Jun 2024 02:54:51 -0400
Subject: [PATCH 13/22] Add test for calc arclength accurate and inaccurate

---
 tests/test_frenet_utils.py | 24 +++++++++++++++++++++++-
 1 file changed, 23 insertions(+), 1 deletion(-)

diff --git a/tests/test_frenet_utils.py b/tests/test_frenet_utils.py
index d69de936..bdfe2603 100644
--- a/tests/test_frenet_utils.py
+++ b/tests/test_frenet_utils.py
@@ -42,7 +42,29 @@ def test_calc_position(self):
         self.assertTrue(np.allclose(track.calc_position(np.pi), np.array([-1, 0]), atol=1e-3))
         self.assertTrue(np.allclose(track.calc_position(3 * np.pi / 2), np.array([0, -1]), atol=1e-3))
 
+    def test_calc_arclength(self):
+        circle_x = np.cos(np.linspace(0, 2 * np.pi, 100))[:-1]
+        circle_y = np.sin(np.linspace(0, 2 * np.pi, 100))[:-1]
+        track = cubic_spline.CubicSpline2D(circle_x, circle_y)
+        # Test the arclength at the four corners of the circle
+        self.assertAlmostEqual(track.calc_arclength(1, 0, 0)[0], 0, places=2)
+        self.assertAlmostEqual(track.calc_arclength(0, 1, 0)[0], np.pi/2, places=2)
+        self.assertAlmostEqual(track.calc_arclength(-1, 0, np.pi/2)[0], np.pi, places=2)
+        self.assertAlmostEqual(track.calc_arclength(0, -1, np.pi)[0], 3*np.pi/2, places=2)
+
+    def test_calc_arclength_inaccurate(self):
+        circle_x = np.cos(np.linspace(0, 2 * np.pi, 100))[:-1]
+        circle_y = np.sin(np.linspace(0, 2 * np.pi, 100))[:-1]
+        track = cubic_spline.CubicSpline2D(circle_x, circle_y)
+        # Test the arclength at the four corners of the circle
+        self.assertAlmostEqual(track.calc_arclength_inaccurate(1, 0)[0], 0, places=2)
+        self.assertAlmostEqual(track.calc_arclength_inaccurate(0, 1)[0], np.pi/2, places=2)
+        self.assertAlmostEqual(track.calc_arclength_inaccurate(-1, 0)[0], np.pi, places=2)
+        self.assertAlmostEqual(track.calc_arclength_inaccurate(0, -1)[0], 3*np.pi/2, places=2)
+
 if __name__ == "__main__":
     TestFrenet().test_calc_curvature()
     TestFrenet().test_calc_yaw()
-    TestFrenet().test_calc_position()
\ No newline at end of file
+    TestFrenet().test_calc_position()
+    TestFrenet().test_calc_arclength()
+    TestFrenet().test_calc_arclength_inaccurate()
\ No newline at end of file

From 27914b8886528853b926b44dc5edcaabcbb2ea28 Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Fri, 7 Jun 2024 02:56:53 -0400
Subject: [PATCH 14/22] REnamed frenet utils test to cubic spline test

---
 tests/{test_frenet_utils.py => test_cubic_spline.py} | 11 ++---------
 1 file changed, 2 insertions(+), 9 deletions(-)
 rename tests/{test_frenet_utils.py => test_cubic_spline.py} (91%)

diff --git a/tests/test_frenet_utils.py b/tests/test_cubic_spline.py
similarity index 91%
rename from tests/test_frenet_utils.py
rename to tests/test_cubic_spline.py
index bdfe2603..1ea20538 100644
--- a/tests/test_frenet_utils.py
+++ b/tests/test_cubic_spline.py
@@ -6,9 +6,9 @@
 import os
 import sys
 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
-from f1tenth_gym.envs.track import Track, cubic_spline
+from f1tenth_gym.envs.track import cubic_spline
 
-class TestFrenet(unittest.TestCase):
+class TestCubicSpline(unittest.TestCase):
     def test_calc_curvature(self):
         circle_x = np.cos(np.linspace(0, 2 * np.pi, 100))[:-1]
         circle_y = np.sin(np.linspace(0, 2 * np.pi, 100))[:-1]
@@ -61,10 +61,3 @@ def test_calc_arclength_inaccurate(self):
         self.assertAlmostEqual(track.calc_arclength_inaccurate(0, 1)[0], np.pi/2, places=2)
         self.assertAlmostEqual(track.calc_arclength_inaccurate(-1, 0)[0], np.pi, places=2)
         self.assertAlmostEqual(track.calc_arclength_inaccurate(0, -1)[0], 3*np.pi/2, places=2)
-
-if __name__ == "__main__":
-    TestFrenet().test_calc_curvature()
-    TestFrenet().test_calc_yaw()
-    TestFrenet().test_calc_position()
-    TestFrenet().test_calc_arclength()
-    TestFrenet().test_calc_arclength_inaccurate()
\ No newline at end of file

From 1f4f3e7d5d05ebf08ebae4142879e4b4a81d46da Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Fri, 7 Jun 2024 03:12:34 -0400
Subject: [PATCH 15/22] Minor cleanup

---
 tests/test_cubic_spline.py | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/tests/test_cubic_spline.py b/tests/test_cubic_spline.py
index 1ea20538..4867bfbf 100644
--- a/tests/test_cubic_spline.py
+++ b/tests/test_cubic_spline.py
@@ -1,11 +1,6 @@
-import pathlib
-import time
 import unittest
 
 import numpy as np
-import os
-import sys
-sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
 from f1tenth_gym.envs.track import cubic_spline
 
 class TestCubicSpline(unittest.TestCase):

From 8d9f1dbfc8659fff969f66ee4d37db4e5b1c866e Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Fri, 7 Jun 2024 03:12:48 -0400
Subject: [PATCH 16/22] Add frenet tests to test track

---
 tests/test_track.py | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/tests/test_track.py b/tests/test_track.py
index 98ff7bbb..3c610229 100644
--- a/tests/test_track.py
+++ b/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)
\ No newline at end of file

From 597b0f82f762a81d71b73d6b17d9ce7642a07865 Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Fri, 7 Jun 2024 03:25:32 -0400
Subject: [PATCH 17/22] Use union for python3.8 compatibility

---
 f1tenth_gym/envs/track/cubic_spline.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/f1tenth_gym/envs/track/cubic_spline.py b/f1tenth_gym/envs/track/cubic_spline.py
index 36fb3c08..c14e41c9 100644
--- a/f1tenth_gym/envs/track/cubic_spline.py
+++ b/f1tenth_gym/envs/track/cubic_spline.py
@@ -8,6 +8,7 @@
 import scipy.optimize as so
 from scipy import interpolate
 from numba import njit
+from typing import Union
 
 @njit(fastmath=False, cache=True)
 def nearest_point_on_trajectory(point, trajectory):
@@ -94,7 +95,7 @@ def __calc_s(self, x: np.ndarray, y: np.ndarray) -> np.ndarray:
         s.extend(np.cumsum(self.ds))
         return np.array(s)
 
-    def calc_position(self, s: float) -> tuple[float | None, float | None]:
+    def calc_position(self, s: float) -> tuple[Union[float, None], Union[float, None]]:
         """
         Calc position at the given s.
 

From d6f60e08c1749197077eff28d7c1f806427d7602 Mon Sep 17 00:00:00 2001
From: Ahmad Amine <AhmadAmine998@gmail.com>
Date: Fri, 7 Jun 2024 03:29:06 -0400
Subject: [PATCH 18/22] Add unions to calc yaw and calc curvature

---
 f1tenth_gym/envs/track/cubic_spline.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/f1tenth_gym/envs/track/cubic_spline.py b/f1tenth_gym/envs/track/cubic_spline.py
index c14e41c9..fd61e644 100644
--- a/f1tenth_gym/envs/track/cubic_spline.py
+++ b/f1tenth_gym/envs/track/cubic_spline.py
@@ -114,7 +114,7 @@ def calc_position(self, s: float) -> tuple[Union[float, None], Union[float, None
         """
         return self.spline(s)
 
-    def calc_curvature(self, s: float) -> float | None:
+    def calc_curvature(self, s: float) -> Union[float , None]:
         """
         Calc curvature at the given s.
 
@@ -134,7 +134,7 @@ def calc_curvature(self, s: float) -> float | None:
         k = (ddy * dx - ddx * dy) / ((dx**2 + dy**2) ** (3 / 2))
         return k
 
-    def calc_yaw(self, s: float) -> float | None:
+    def calc_yaw(self, s: float) -> Union[float, None]:
         """
         Calc yaw angle at the given s.
 

From 7528d560e00ecb24d954c73abd91814c249c3a6d Mon Sep 17 00:00:00 2001
From: "Berducci, Luigi" <luigi.berducci@tuwien.ac.at>
Date: Fri, 14 Jun 2024 16:42:08 +0200
Subject: [PATCH 19/22] linting, docstrings and minor changes

---
 f1tenth_gym/envs/track/cubic_spline.py | 106 +++++++++++++++++--------
 tests/test_cubic_spline.py             |  43 +++++++---
 2 files changed, 103 insertions(+), 46 deletions(-)

diff --git a/f1tenth_gym/envs/track/cubic_spline.py b/f1tenth_gym/envs/track/cubic_spline.py
index fd61e644..fa87d9a6 100644
--- a/f1tenth_gym/envs/track/cubic_spline.py
+++ b/f1tenth_gym/envs/track/cubic_spline.py
@@ -2,6 +2,7 @@
 Cubic Spline interpolation using scipy.interpolate
 Provides utilities for position, curvature, yaw, and arclength calculation
 """
+
 import math
 
 import numpy as np
@@ -10,8 +11,9 @@
 from numba import njit
 from typing import Union
 
+
 @njit(fastmath=False, cache=True)
-def nearest_point_on_trajectory(point, trajectory):
+def nearest_point_on_trajectory(point: np.ndarray, trajectory: np.ndarray) -> tuple:
     """
     Return the nearest point along the given piecewise linear trajectory.
 
@@ -20,9 +22,23 @@ def nearest_point_on_trajectory(point, trajectory):
 
         Order of magnitude: trajectory length: 1000 --> 0.0002 second computation (5000fps)
 
-    point: size 2 numpy array
-    trajectory: Nx2 matrix of (x,y) trajectory waypoints
-        - these must be unique. If they are not unique, a divide by 0 error will destroy the world
+    Parameters
+    ----------
+    point: np.ndarray
+        The 2d point to project onto the trajectory
+    trajectory: np.ndarray
+        The trajectory to project the point onto, shape (N, 2)
+        The points must be unique. If they are not unique, a divide by 0 error will destroy the world
+
+    Returns
+    -------
+    nearest_point: np.ndarray
+        The nearest point on the trajectory
+    distance: float
+        The distance from the point to the nearest point on the trajectory
+    t: float
+    min_dist_segment: int
+        The index of the nearest point on the trajectory
     """
     diffs = trajectory[1:, :] - trajectory[:-1, :]
     l2s = diffs[:, 0] ** 2 + diffs[:, 1] ** 2
@@ -49,6 +65,7 @@ def nearest_point_on_trajectory(point, trajectory):
         min_dist_segment,
     )
 
+
 class CubicSpline2D:
     """
     Cubic CubicSpline2D class.
@@ -66,11 +83,13 @@ class CubicSpline2D:
     def __init__(self, x, y):
         self.points = np.c_[x, y]
         if not np.all(self.points[-1] == self.points[0]):
-            self.points = np.vstack((self.points, self.points[0]))  # Ensure the path is closed
+            self.points = np.vstack(
+                (self.points, self.points[0])
+            )  # Ensure the path is closed
         self.s = self.__calc_s(self.points[:, 0], self.points[:, 1])
         # Use scipy CubicSpline to interpolate the points with periodic boundary conditions
         # This is necessary to ensure the path is continuous
-        self.spline = interpolate.CubicSpline(self.s, self.points, bc_type='periodic')
+        self.spline = interpolate.CubicSpline(self.s, self.points, bc_type="periodic")
 
     def __calc_s(self, x: np.ndarray, y: np.ndarray) -> np.ndarray:
         """
@@ -114,7 +133,7 @@ def calc_position(self, s: float) -> tuple[Union[float, None], Union[float, None
         """
         return self.spline(s)
 
-    def calc_curvature(self, s: float) -> Union[float , None]:
+    def calc_curvature(self, s: float) -> Union[float, None]:
         """
         Calc curvature at the given s.
 
@@ -152,18 +171,24 @@ def calc_yaw(self, s: float) -> Union[float, None]:
         yaw = math.atan2(dy, dx)
         # Convert yaw to [0, 2pi]
         yaw = yaw % (2 * math.pi)
-        
+
         return yaw
 
-    def calc_arclength(self, x, y, s_guess=0.0):
+    def calc_arclength(
+        self, x: float, y: float, s_guess: float = 0.0
+    ) -> tuple[float, float]:
         """
-        calc arclength
+        Calculate arclength for a given point (x, y) on the trajectory.
+
         Parameters
         ----------
         x : float
             x position.
         y : float
             y position.
+        s_guess : float
+            initial guess for s.
+
         Returns
         -------
         s : float
@@ -174,24 +199,26 @@ def calc_arclength(self, x, y, s_guess=0.0):
 
         def distance_to_spline(s):
             x_eval, y_eval = self.spline(s)[0]
-            return np.sqrt((x - x_eval)**2 + (y - y_eval)**2)
-        
+            return np.sqrt((x - x_eval) ** 2 + (y - y_eval) ** 2)
+
         output = so.fmin(distance_to_spline, s_guess, full_output=True, disp=False)
-        closest_s = output[0][0]
+        closest_s = float(output[0][0])
         absolute_distance = output[1]
         return closest_s, absolute_distance
-    
-    def calc_arclength_inaccurate(self, x, y, s_guess=0.0):
+
+    def calc_arclength_inaccurate(self, x: float, y: float) -> tuple[float, float]:
         """
-        calc arclength, use nearest_point_on_trajectory
-        Less accuarate and less smooth than calc_arclength but
-        much faster - suitable for lap counting
+        Fast calculation of arclength for a given point (x, y) on the trajectory.
+        Less accuarate and less smooth than calc_arclength but much faster.
+        Suitable for lap counting.
+
         Parameters
         ----------
         x : float
             x position.
         y : float
             y position.
+
         Returns
         -------
         s : float
@@ -199,42 +226,51 @@ def calc_arclength_inaccurate(self, x, y, s_guess=0.0):
         ey : float
             lateral deviation for given x, y.
         """
-        _, ey, t, min_dist_segment = nearest_point_on_trajectory(np.array([x, y]), self.points)
+        _, ey, t, min_dist_segment = nearest_point_on_trajectory(
+            np.array([x, y]), self.points
+        )
         # s = s at closest_point + t
-        s = self.s[min_dist_segment] + t * (self.s[min_dist_segment + 1] - self.s[min_dist_segment])
+        s = float(
+            self.s[min_dist_segment]
+            + t * (self.s[min_dist_segment + 1] - self.s[min_dist_segment])
+        )
 
-        return s, 0
+        return s, 0.0
+
+    def _calc_tangent(self, s: float) -> np.ndarray:
+        """
+        Calculates the tangent to the curve at a given point.
 
-    def _calc_tangent(self, s):
-        '''
-        calculates the tangent to the curve at a given point
         Parameters
         ----------
         s : float
-            distance from the start point. if `s` is outside the data point's
-            range, return None.
+            distance from the start point.
+            If `s` is outside the data point's range, return None.
+
         Returns
         -------
         tangent : float
             tangent vector for given s.
-        '''
+        """
         dx, dy = self.spline(s, 1)
         tangent = np.array([dx, dy])
         return tangent
-    
-    def _calc_normal(self, s):
-        '''
-        calculates the normal to the curve at a given point
+
+    def _calc_normal(self, s: float) -> np.ndarray:
+        """
+        Calculate the normal to the curve at a given point.
+
         Parameters
         ----------
         s : float
-            distance from the start point. if `s` is outside the data point's
-            range, return None.
+            distance from the start point.
+            If `s` is outside the data point's range, return None.
+
         Returns
         -------
         normal : float
             normal vector for given s.
-        '''
+        """
         dx, dy = self.spline(s, 1)
         normal = np.array([-dy, dx])
-        return normal
\ No newline at end of file
+        return normal
diff --git a/tests/test_cubic_spline.py b/tests/test_cubic_spline.py
index 4867bfbf..a9b3ca94 100644
--- a/tests/test_cubic_spline.py
+++ b/tests/test_cubic_spline.py
@@ -3,6 +3,7 @@
 import numpy as np
 from f1tenth_gym.envs.track import cubic_spline
 
+
 class TestCubicSpline(unittest.TestCase):
     def test_calc_curvature(self):
         circle_x = np.cos(np.linspace(0, 2 * np.pi, 100))[:-1]
@@ -25,17 +26,27 @@ def test_calc_yaw(self):
         self.assertAlmostEqual(track.calc_yaw(np.pi / 2), np.pi, places=2)
         self.assertAlmostEqual(track.calc_yaw(np.pi), 3 * np.pi / 2, places=2)
         self.assertAlmostEqual(track.calc_yaw(3 * np.pi / 2), 0, places=2)
-    
+
     def test_calc_position(self):
         circle_x = np.cos(np.linspace(0, 2 * np.pi, 100))[:-1]
         circle_y = np.sin(np.linspace(0, 2 * np.pi, 100))[:-1]
         track = cubic_spline.CubicSpline2D(circle_x, circle_y)
         # Test the position at the four corners of the circle
         # The position of a circle is (x, y) = (cos(s), sin(s))
-        self.assertTrue(np.allclose(track.calc_position(0), np.array([1, 0]), atol=1e-3))
-        self.assertTrue(np.allclose(track.calc_position(np.pi / 2), np.array([0, 1]), atol=1e-3))
-        self.assertTrue(np.allclose(track.calc_position(np.pi), np.array([-1, 0]), atol=1e-3))
-        self.assertTrue(np.allclose(track.calc_position(3 * np.pi / 2), np.array([0, -1]), atol=1e-3))
+        self.assertTrue(
+            np.allclose(track.calc_position(0), np.array([1, 0]), atol=1e-3)
+        )
+        self.assertTrue(
+            np.allclose(track.calc_position(np.pi / 2), np.array([0, 1]), atol=1e-3)
+        )
+        self.assertTrue(
+            np.allclose(track.calc_position(np.pi), np.array([-1, 0]), atol=1e-3)
+        )
+        self.assertTrue(
+            np.allclose(
+                track.calc_position(3 * np.pi / 2), np.array([0, -1]), atol=1e-3
+            )
+        )
 
     def test_calc_arclength(self):
         circle_x = np.cos(np.linspace(0, 2 * np.pi, 100))[:-1]
@@ -43,9 +54,13 @@ def test_calc_arclength(self):
         track = cubic_spline.CubicSpline2D(circle_x, circle_y)
         # Test the arclength at the four corners of the circle
         self.assertAlmostEqual(track.calc_arclength(1, 0, 0)[0], 0, places=2)
-        self.assertAlmostEqual(track.calc_arclength(0, 1, 0)[0], np.pi/2, places=2)
-        self.assertAlmostEqual(track.calc_arclength(-1, 0, np.pi/2)[0], np.pi, places=2)
-        self.assertAlmostEqual(track.calc_arclength(0, -1, np.pi)[0], 3*np.pi/2, places=2)
+        self.assertAlmostEqual(track.calc_arclength(0, 1, 0)[0], np.pi / 2, places=2)
+        self.assertAlmostEqual(
+            track.calc_arclength(-1, 0, np.pi / 2)[0], np.pi, places=2
+        )
+        self.assertAlmostEqual(
+            track.calc_arclength(0, -1, np.pi)[0], 3 * np.pi / 2, places=2
+        )
 
     def test_calc_arclength_inaccurate(self):
         circle_x = np.cos(np.linspace(0, 2 * np.pi, 100))[:-1]
@@ -53,6 +68,12 @@ def test_calc_arclength_inaccurate(self):
         track = cubic_spline.CubicSpline2D(circle_x, circle_y)
         # Test the arclength at the four corners of the circle
         self.assertAlmostEqual(track.calc_arclength_inaccurate(1, 0)[0], 0, places=2)
-        self.assertAlmostEqual(track.calc_arclength_inaccurate(0, 1)[0], np.pi/2, places=2)
-        self.assertAlmostEqual(track.calc_arclength_inaccurate(-1, 0)[0], np.pi, places=2)
-        self.assertAlmostEqual(track.calc_arclength_inaccurate(0, -1)[0], 3*np.pi/2, places=2)
+        self.assertAlmostEqual(
+            track.calc_arclength_inaccurate(0, 1)[0], np.pi / 2, places=2
+        )
+        self.assertAlmostEqual(
+            track.calc_arclength_inaccurate(-1, 0)[0], np.pi, places=2
+        )
+        self.assertAlmostEqual(
+            track.calc_arclength_inaccurate(0, -1)[0], 3 * np.pi / 2, places=2
+        )

From 23ae07c3a4fac75378dfda7bebaae021b52b5d3e Mon Sep 17 00:00:00 2001
From: "Berducci, Luigi" <luigi.berducci@tuwien.ac.at>
Date: Fri, 14 Jun 2024 16:49:46 +0200
Subject: [PATCH 20/22] move nearest_point util to track.utils, refactor type
 hint with optional instead of Union[type, None]

---
 f1tenth_gym/envs/track/cubic_spline.py | 63 ++------------------------
 f1tenth_gym/envs/track/utils.py        | 56 +++++++++++++++++++++++
 2 files changed, 61 insertions(+), 58 deletions(-)

diff --git a/f1tenth_gym/envs/track/cubic_spline.py b/f1tenth_gym/envs/track/cubic_spline.py
index fa87d9a6..a8af32da 100644
--- a/f1tenth_gym/envs/track/cubic_spline.py
+++ b/f1tenth_gym/envs/track/cubic_spline.py
@@ -8,62 +8,9 @@
 import numpy as np
 import scipy.optimize as so
 from scipy import interpolate
-from numba import njit
-from typing import Union
+from typing import Union, Optional
 
-
-@njit(fastmath=False, cache=True)
-def nearest_point_on_trajectory(point: np.ndarray, trajectory: np.ndarray) -> tuple:
-    """
-    Return the nearest point along the given piecewise linear trajectory.
-
-    Same as nearest_point_on_line_segment, but vectorized. This method is quite fast, time constraints should
-    not be an issue so long as trajectories are not insanely long.
-
-        Order of magnitude: trajectory length: 1000 --> 0.0002 second computation (5000fps)
-
-    Parameters
-    ----------
-    point: np.ndarray
-        The 2d point to project onto the trajectory
-    trajectory: np.ndarray
-        The trajectory to project the point onto, shape (N, 2)
-        The points must be unique. If they are not unique, a divide by 0 error will destroy the world
-
-    Returns
-    -------
-    nearest_point: np.ndarray
-        The nearest point on the trajectory
-    distance: float
-        The distance from the point to the nearest point on the trajectory
-    t: float
-    min_dist_segment: int
-        The index of the nearest point on the trajectory
-    """
-    diffs = trajectory[1:, :] - trajectory[:-1, :]
-    l2s = diffs[:, 0] ** 2 + diffs[:, 1] ** 2
-    # this is equivalent to the elementwise dot product
-    # dots = np.sum((point - trajectory[:-1,:]) * diffs[:,:], axis=1)
-    dots = np.empty((trajectory.shape[0] - 1,))
-    for i in range(dots.shape[0]):
-        dots[i] = np.dot((point - trajectory[i, :]), diffs[i, :])
-    t = dots / l2s
-    t[t < 0.0] = 0.0
-    t[t > 1.0] = 1.0
-    # t = np.clip(dots / l2s, 0.0, 1.0)
-    projections = trajectory[:-1, :] + (t * diffs.T).T
-    # dists = np.linalg.norm(point - projections, axis=1)
-    dists = np.empty((projections.shape[0],))
-    for i in range(dists.shape[0]):
-        temp = point - projections[i]
-        dists[i] = np.sqrt(np.sum(temp * temp))
-    min_dist_segment = np.argmin(dists)
-    return (
-        projections[min_dist_segment],
-        dists[min_dist_segment],
-        t[min_dist_segment],
-        min_dist_segment,
-    )
+from f1tenth_gym.envs.track.utils import nearest_point_on_trajectory
 
 
 class CubicSpline2D:
@@ -114,7 +61,7 @@ def __calc_s(self, x: np.ndarray, y: np.ndarray) -> np.ndarray:
         s.extend(np.cumsum(self.ds))
         return np.array(s)
 
-    def calc_position(self, s: float) -> tuple[Union[float, None], Union[float, None]]:
+    def calc_position(self, s: float) -> np.ndarray:
         """
         Calc position at the given s.
 
@@ -133,7 +80,7 @@ def calc_position(self, s: float) -> tuple[Union[float, None], Union[float, None
         """
         return self.spline(s)
 
-    def calc_curvature(self, s: float) -> Union[float, None]:
+    def calc_curvature(self, s: float) -> Optional[float]:
         """
         Calc curvature at the given s.
 
@@ -153,7 +100,7 @@ def calc_curvature(self, s: float) -> Union[float, None]:
         k = (ddy * dx - ddx * dy) / ((dx**2 + dy**2) ** (3 / 2))
         return k
 
-    def calc_yaw(self, s: float) -> Union[float, None]:
+    def calc_yaw(self, s: float) -> Optional[float]:
         """
         Calc yaw angle at the given s.
 
diff --git a/f1tenth_gym/envs/track/utils.py b/f1tenth_gym/envs/track/utils.py
index e3e88a09..6c8a0bf4 100644
--- a/f1tenth_gym/envs/track/utils.py
+++ b/f1tenth_gym/envs/track/utils.py
@@ -2,7 +2,9 @@
 import tarfile
 import tempfile
 
+import numpy as np
 import requests
+from numba import njit
 
 
 def find_track_dir(track_name: str) -> pathlib.Path:
@@ -49,3 +51,57 @@ def find_track_dir(track_name: str) -> pathlib.Path:
             return subdir
 
     raise FileNotFoundError(f"no mapdir matching {track_name} in {[map_dir]}")
+
+
+@njit(fastmath=False, cache=True)
+def nearest_point_on_trajectory(point: np.ndarray, trajectory: np.ndarray) -> tuple:
+    """
+    Return the nearest point along the given piecewise linear trajectory.
+
+    Same as nearest_point_on_line_segment, but vectorized. This method is quite fast, time constraints should
+    not be an issue so long as trajectories are not insanely long.
+
+        Order of magnitude: trajectory length: 1000 --> 0.0002 second computation (5000fps)
+
+    Parameters
+    ----------
+    point: np.ndarray
+        The 2d point to project onto the trajectory
+    trajectory: np.ndarray
+        The trajectory to project the point onto, shape (N, 2)
+        The points must be unique. If they are not unique, a divide by 0 error will destroy the world
+
+    Returns
+    -------
+    nearest_point: np.ndarray
+        The nearest point on the trajectory
+    distance: float
+        The distance from the point to the nearest point on the trajectory
+    t: float
+    min_dist_segment: int
+        The index of the nearest point on the trajectory
+    """
+    diffs = trajectory[1:, :] - trajectory[:-1, :]
+    l2s = diffs[:, 0] ** 2 + diffs[:, 1] ** 2
+    # this is equivalent to the elementwise dot product
+    # dots = np.sum((point - trajectory[:-1,:]) * diffs[:,:], axis=1)
+    dots = np.empty((trajectory.shape[0] - 1,))
+    for i in range(dots.shape[0]):
+        dots[i] = np.dot((point - trajectory[i, :]), diffs[i, :])
+    t = dots / l2s
+    t[t < 0.0] = 0.0
+    t[t > 1.0] = 1.0
+    # t = np.clip(dots / l2s, 0.0, 1.0)
+    projections = trajectory[:-1, :] + (t * diffs.T).T
+    # dists = np.linalg.norm(point - projections, axis=1)
+    dists = np.empty((projections.shape[0],))
+    for i in range(dists.shape[0]):
+        temp = point - projections[i]
+        dists[i] = np.sqrt(np.sum(temp * temp))
+    min_dist_segment = np.argmin(dists)
+    return (
+        projections[min_dist_segment],
+        dists[min_dist_segment],
+        t[min_dist_segment],
+        min_dist_segment,
+    )

From a31f52c6afab8dce4629d1f077e7776462a68b4a Mon Sep 17 00:00:00 2001
From: "Berducci, Luigi" <luigi.berducci@tuwien.ac.at>
Date: Fri, 14 Jun 2024 16:54:51 +0200
Subject: [PATCH 21/22] type hint

---
 f1tenth_gym/envs/track/track.py | 22 +++++++++++-----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/f1tenth_gym/envs/track/track.py b/f1tenth_gym/envs/track/track.py
index 52e722c6..c6a0d973 100644
--- a/f1tenth_gym/envs/track/track.py
+++ b/f1tenth_gym/envs/track/track.py
@@ -16,8 +16,8 @@
 
 @dataclass
 class TrackSpec(YamlDataClassConfig):
-    name: str
-    image: str
+    name: Optional[str]
+    image: Optional[str]
     resolution: float
     origin: Tuple[float, float, float]
     negate: int
@@ -28,8 +28,8 @@ class TrackSpec(YamlDataClassConfig):
 @dataclass
 class Track:
     spec: TrackSpec
-    filepath: str
-    ext: str
+    filepath: Optional[str]
+    ext: Optional[str]
     occupancy_map: np.ndarray
     centerline: Raceline
     raceline: Raceline
@@ -37,9 +37,9 @@ class Track:
     def __init__(
         self,
         spec: TrackSpec,
-        filepath: str,
-        ext: str,
         occupancy_map: np.ndarray,
+        filepath: Optional[str] = None,
+        ext: Optional[str] = None,
         centerline: Optional[Raceline] = None,
         raceline: Optional[Raceline] = None,
     ):
@@ -154,7 +154,7 @@ def from_track_name(track: str):
             raise FileNotFoundError(f"It could not load track {track}") from ex
 
     @staticmethod
-    def from_refline(x: np.ndarray, y: np.ndarray, velx: np.ndarray,):
+    def from_refline(x: np.ndarray, y: np.ndarray, velx: np.ndarray):
         """
         Create an empty track reference line.
 
@@ -169,7 +169,7 @@ def from_refline(x: np.ndarray, y: np.ndarray, velx: np.ndarray,):
 
         Returns
         -------
-        Track
+        track: Track
             track object
         """
         ds = 0.1
@@ -241,7 +241,7 @@ def from_refline(x: np.ndarray, y: np.ndarray, velx: np.ndarray,):
     def frenet_to_cartesian(self, s, ey, ephi):
         """
         Convert Frenet coordinates to Cartesian coordinates.
-        
+
         s: distance along the raceline
         ey: lateral deviation
         ephi: heading deviation
@@ -262,11 +262,11 @@ def frenet_to_cartesian(self, s, ey, ephi):
         psi += ephi
 
         return x, y, psi
-    
+
     def cartesian_to_frenet(self, x, y, phi, s_guess=0):
         """
         Convert Cartesian coordinates to Frenet coordinates.
-        
+
         x: x-coordinate
         y: y-coordinate
         phi: yaw angle

From 1b520179a558540bab171a8bd53d92bb3e410bcc Mon Sep 17 00:00:00 2001
From: "Berducci, Luigi" <luigi.berducci@tuwien.ac.at>
Date: Fri, 14 Jun 2024 16:59:51 +0200
Subject: [PATCH 22/22] 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)