diff --git a/f1tenth_gym/envs/track/cubic_spline.py b/f1tenth_gym/envs/track/cubic_spline.py index b148c269..a8af32da 100644 --- a/f1tenth_gym/envs/track/cubic_spline.py +++ b/f1tenth_gym/envs/track/cubic_spline.py @@ -1,225 +1,16 @@ """ -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 """ -from __future__ import annotations -import bisect + import math import numpy as np +import scipy.optimize as so +from scipy import interpolate +from typing import Union, Optional - -class CubicSpline1D: - """ - 1D Cubic Spline class - - Attributes - ---------- - x : list - x coordinates for data points. This x coordinates must be - sorted in ascending order. - y : list - y coordinates for data points - a : list - coefficient a - b : list - coefficient b - c : list - coefficient c - d : list - coefficient d - nx : int - dimension of x - """ - - def __init__(self, x: np.ndarray, y: np.ndarray): - """ - Returns a 1D cubic spline object. - - Parameters - ---------- - x : list - x coordinates for data points. This x coordinates must be - sorted in ascending order. - y : list - y coordinates for data points - - Raises - ------ - ValueError - if x is not sorted in ascending order - """ - 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=h) - B = self.__calc_B(h=h, a=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: float) -> float | None: - """ - Calc `y` position for given `x`. - If `x` is outside the data point's `x` range, return None. - - Parameters - ---------- - x : float - position for which to calculate y. - - Returns - ------- - y : float - position along the spline 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: float) -> float | None: - """ - Calc first derivative at given x. - If x is outside the input x, return None - - Parameters - ---------- - x : float - position for which to calculate the first derivative. - - 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: float) -> float | None: - """ - Calc second derivative at given x. - If x is outside the input x, return None - - Parameters - ---------- - x : float - position for which to calculate the second derivative. - - 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: float) -> int: - """ - Search data segment index. - - Parameters - ---------- - x : float - position for which to find the segment index. - - Returns - ------- - index : int - index of the segment. - """ - return bisect.bisect(self.x[:-1], x) - 1 - - def __calc_A(self, h: np.ndarray) -> np.ndarray: - """ - Calc matrix A for spline coefficient c. - - Parameters - ---------- - h : np.ndarray - difference of x coordinates. - - Returns - ------- - A : np.ndarray - matrix A. - """ - 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: np.ndarray, a: np.ndarray) -> np.ndarray: - """ - Calc matrix B for spline coefficient c. - - Parameters - ---------- - h : np.ndarray - difference of x coordinates. - a : np.ndarray - y coordinates for data points. - - Returns - ------- - B : np.ndarray - matrix B. - """ - 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 f1tenth_gym.envs.track.utils import nearest_point_on_trajectory class CubicSpline2D: @@ -236,20 +27,16 @@ class CubicSpline2D: cubic spline for y coordinates. """ - def __init__(self, x: np.ndarray, y: np.ndarray): - """ - Returns a 2D cubic spline object. - - Parameters - ---------- - x : list - x coordinates for data points. - y : list - y coordinates for data points. - """ - self.s = self.__calc_s(x, y) - self.sx = CubicSpline1D(x=self.s, y=x) - self.sy = CubicSpline1D(x=self.s, y=y) + 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.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") def __calc_s(self, x: np.ndarray, y: np.ndarray) -> np.ndarray: """ @@ -274,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[float | None, float | None]: + def calc_position(self, s: float) -> np.ndarray: """ Calc position at the given s. @@ -291,12 +78,9 @@ def calc_position(self, s: float) -> tuple[float | None, float | None]: y : float | None 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: float) -> float | None: + def calc_curvature(self, s: float) -> Optional[float]: """ Calc curvature at the given s. @@ -311,14 +95,12 @@ def calc_curvature(self, s: float) -> float | None: 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) - k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2) ** (3 / 2)) + 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 - def calc_yaw(self, s: float) -> float | None: + def calc_yaw(self, s: float) -> Optional[float]: """ Calc yaw angle at the given s. @@ -332,7 +114,110 @@ def calc_yaw(self, s: float) -> float | None: 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) + # Convert yaw to [0, 2pi] + yaw = yaw % (2 * math.pi) + return yaw + + def calc_arclength( + self, x: float, y: float, s_guess: float = 0.0 + ) -> tuple[float, float]: + """ + 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 + distance from the start point for given x, y. + ey : float + lateral deviation for given x, y. + """ + + def distance_to_spline(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, s_guess, full_output=True, disp=False) + closest_s = float(output[0][0]) + absolute_distance = output[1] + return closest_s, absolute_distance + + def calc_arclength_inaccurate(self, x: float, y: float) -> tuple[float, float]: + """ + 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 + 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 = float( + self.s[min_dist_segment] + + t * (self.s[min_dist_segment + 1] - self.s[min_dist_segment]) + ) + + return s, 0.0 + + def _calc_tangent(self, s: float) -> np.ndarray: + """ + 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. + """ + dx, dy = self.spline(s, 1) + tangent = np.array([dx, dy]) + return tangent + + 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. + + Returns + ------- + normal : float + normal vector for given s. + """ + dx, dy = self.spline(s, 1) + normal = np.array([-dy, dx]) + return normal diff --git a/f1tenth_gym/envs/track/track.py b/f1tenth_gym/envs/track/track.py index d0bd6585..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 @@ -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 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, + ) diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py new file mode 100644 index 00000000..d2d1fe50 --- /dev/null +++ b/gym/f110_gym/envs/track.py @@ -0,0 +1,306 @@ +import pathlib +import tarfile +from dataclasses import dataclass +from typing import Tuple +import tempfile + +import numpy as np +import requests +import yaml +from f110_gym.envs.cubic_spline import CubicSpline2D +from PIL import Image +from PIL.Image import Transpose +from yamldataclassconfig.config import YamlDataClassConfig + + +class Raceline: + n: int + + ss: np.ndarray # cumulative distance along the raceline + xs: np.ndarray # x-coordinates of the raceline + ys: np.ndarray # y-coordinates of the raceline + yaws: np.ndarray # yaw angle of the raceline + ks: np.ndarray # curvature of the raceline + vxs: np.ndarray # velocity along the raceline + axs: np.ndarray # acceleration along the raceline + + length: float + + def __init__( + self, + xs: np.ndarray, + ys: np.ndarray, + velxs: np.ndarray, + ss: np.ndarray = None, + psis: np.ndarray = None, + kappas: np.ndarray = None, + accxs: np.ndarray = None, + spline: CubicSpline2D = None, + ): + assert xs.shape == ys.shape == velxs.shape, "inconsistent shapes for x, y, vel" + + self.n = xs.shape[0] + self.ss = ss + self.xs = xs + self.ys = ys + self.yaws = psis + self.ks = kappas + self.vxs = velxs + self.axs = accxs + + # approximate track length by linear-interpolation of x,y waypoints + # note: we could use 'ss' but sometimes it is normalized to [0,1], so we recompute it here + self.length = float(np.sum(np.sqrt(np.diff(xs) ** 2 + np.diff(ys) ** 2))) + + # compute spline + self.spline = spline if spline is not None else CubicSpline2D(xs, ys) + + @staticmethod + def from_centerline_file( + filepath: pathlib.Path, delimiter: str = ",", fixed_speed: float = 1.0 + ): + assert filepath.exists(), f"input filepath does not exist ({filepath})" + waypoints = np.loadtxt(filepath, delimiter=delimiter) + assert waypoints.shape[1] == 4, "expected waypoints as [x, y, w_left, w_right]" + + # fit cubic spline to waypoints + xx, yy = waypoints[:, 0], waypoints[:, 1] + # close loop + xx = np.append(xx, xx[0]) + yy = np.append(yy, yy[0]) + spline = CubicSpline2D(xx, yy) + ds = 0.1 + + ss, xs, ys, yaws, ks = [], [], [], [], [] + + for i_s in np.arange(0, spline.s[-1], ds): + x, y = spline.calc_position(i_s) + yaw = spline.calc_yaw(i_s) + k = spline.calc_curvature(i_s) + + xs.append(x) + ys.append(y) + yaws.append(yaw) + ks.append(k) + ss.append(i_s) + + return Raceline( + ss=np.array(ss).astype(np.float32), + xs=np.array(xs).astype(np.float32), + ys=np.array(ys).astype(np.float32), + psis=np.array(yaws).astype(np.float32), + kappas=np.array(ks).astype(np.float32), + velxs=np.ones_like(ss).astype( + np.float32 + ), # centerline does not have a speed profile, keep it constant at 1.0 m/s + accxs=np.zeros_like(ss).astype(np.float32), # constant acceleration + ) + + @staticmethod + def from_raceline_file(filepath: pathlib.Path, delimiter: str = ";"): + assert filepath.exists(), f"input filepath does not exist ({filepath})" + waypoints = np.loadtxt(filepath, delimiter=delimiter).astype(np.float32) + assert ( + waypoints.shape[1] == 7 + ), "expected waypoints as [s, x, y, psi, k, vx, ax]" + return Raceline( + ss=waypoints[:, 0], + xs=waypoints[:, 1], + ys=waypoints[:, 2], + psis=waypoints[:, 3], + kappas=waypoints[:, 4], + velxs=waypoints[:, 5], + accxs=waypoints[:, 6], + ) + + +@dataclass +class TrackSpec(YamlDataClassConfig): + name: str + image: str + resolution: float + origin: Tuple[float, float, float] + negate: int + occupied_thresh: float + free_thresh: float + + +def find_track_dir(track_name): + # we assume there are no blank space in the track name. however, to take into account eventual blank spaces in + # the map dirpath, we loop over all possible maps and check if there is a matching with the current track + map_dir = pathlib.Path(__file__).parent.parent.parent.parent / "maps" + + if not (map_dir / track_name).exists(): + print("Downloading Files for: " + track_name) + tracks_url = "http://api.f1tenth.org/" + track_name + ".tar.xz" + tracks_r = requests.get(url=tracks_url, allow_redirects=True) + if tracks_r.status_code == 404: + raise FileNotFoundError(f"No maps exists for {track_name}.") + + tempdir = tempfile.gettempdir() + "/" + + with open(tempdir + track_name + ".tar.xz", "wb") as f: + f.write(tracks_r.content) + + # extract + print("Extracting Files for: " + track_name) + tracks_file = tarfile.open(tempdir + track_name + ".tar.xz") + tracks_file.extractall(map_dir) + tracks_file.close() + + for base_dir in [map_dir]: + if not base_dir.exists(): + continue + + for dir in base_dir.iterdir(): + if track_name == str(dir.stem).replace(" ", ""): + return dir + + raise FileNotFoundError(f"no mapdir matching {track_name} in {[map_dir]}") + + +@dataclass +class Track: + spec: TrackSpec + filepath: str + ext: str + occupancy_map: np.ndarray + centerline: Raceline + raceline: Raceline + + def __init__( + self, + spec: TrackSpec, + filepath: str, + ext: str, + occupancy_map: np.ndarray, + centerline: Raceline = None, + raceline: Raceline = None, + ): + self.spec = spec + self.filepath = filepath + self.ext = ext + self.occupancy_map = occupancy_map + self.centerline = centerline + self.raceline = raceline + + @staticmethod + def load_spec(track: str, filespec: str): + """ + Load track specification from yaml file. + + Args: + + """ + with open(filespec, "r") as yaml_stream: + map_metadata = yaml.safe_load(yaml_stream) + track_spec = TrackSpec(name=track, **map_metadata) + return track_spec + + @staticmethod + def from_track_name(track: str): + try: + track_dir = find_track_dir(track) + track_spec = Track.load_spec( + track=track, filespec=str(track_dir / f"{track_dir.stem}_map.yaml") + ) + + # load occupancy grid + map_filename = pathlib.Path(track_spec.image) + image = Image.open(track_dir / str(map_filename)).transpose( + Transpose.FLIP_TOP_BOTTOM + ) + occupancy_map = np.array(image).astype(np.float32) + occupancy_map[occupancy_map <= 128] = 0.0 + occupancy_map[occupancy_map > 128] = 255.0 + + # if exists, load centerline + if (track_dir / f"{track}_centerline.csv").exists(): + centerline = Raceline.from_centerline_file( + track_dir / f"{track}_centerline.csv" + ) + else: + centerline = None + + # if exists, load raceline + if (track_dir / f"{track}_raceline.csv").exists(): + raceline = Raceline.from_raceline_file( + track_dir / f"{track}_raceline.csv" + ) + else: + raceline = centerline + + return Track( + spec=track_spec, + filepath=str((track_dir / map_filename.stem).absolute()), + ext=map_filename.suffix, + occupancy_map=occupancy_map, + centerline=centerline, + raceline=raceline, + ) + except Exception as ex: + 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.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 + + +if __name__ == "__main__": + track = Track.from_track_name("Example") + print("[Result] map loaded successfully") diff --git a/tests/test_cubic_spline.py b/tests/test_cubic_spline.py new file mode 100644 index 00000000..a9b3ca94 --- /dev/null +++ b/tests/test_cubic_spline.py @@ -0,0 +1,79 @@ +import unittest + +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] + 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) + + 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) + + 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 + ) + ) + + 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 + ) diff --git a/tests/test_track.py b/tests/test_track.py index 98ff7bbb..97ac6e7c 100644 --- a/tests/test_track.py +++ b/tests/test_track.py @@ -138,3 +138,38 @@ 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) + + # 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)