Add support for Track from raceline file

Author: AhmadAmine998

f1tenth_gym/envs/track/raceline.py

@@ -133,9 +133,9 @@ def from_centerline_file(
         )
 
     @staticmethod
-    def from_raceline_file(filepath: pathlib.Path, delimiter: str = ";", track_scale: Optional[float] = 1.0) -> Raceline:
+    def from_raceline_file(filepath: pathlib.Path, delimiter: str = ";", skip_rows: int = 3, track_scale: Optional[float] = 1.0) -> Raceline:
         """
-        Load raceline from a raceline file.
+        Load raceline from a raceline file of the format [s, x, y, psi, k, vx, ax].
 
         Parameters
         ----------
@@ -151,22 +151,19 @@ def from_raceline_file(filepath: pathlib.Path, delimiter: str = ";", track_scale
         Raceline
             raceline object
         """
+        if type(filepath) is str:
+            filepath = pathlib.Path(filepath)
+
         assert filepath.exists(), f"input filepath does not exist ({filepath})"
-        waypoints = np.loadtxt(filepath, delimiter=delimiter).astype(np.float32)
+        waypoints = np.loadtxt(filepath, delimiter=delimiter, skiprows=
+        skip_rows).astype(np.float32)
 
         if track_scale != 1.0:
             # scale x-y waypoints and recalculate s, psi, and k
             waypoints[:, 1] *= track_scale
             waypoints[:, 2] *= track_scale
             spline = CubicSpline2D(x=waypoints[:, 1], y=waypoints[:, 2])    
-            ss, yaws, ks = [], [], []
-            for (x, y) in zip(waypoints[:, 1], waypoints[:, 2]):
-                i_s, _  = spline.calc_arclength(x, y)
-                yaw = spline.calc_yaw(i_s)
-                k = spline.calc_curvature(i_s)
-                yaws.append(yaw)
-                ks.append(k)
-                ss.append(i_s)
+            ss, yaws, ks = spline.ss, spline.psis, spline.ks
             waypoints[:, 0] = ss
             waypoints[:, 3] = yaws
             waypoints[:, 4] = ks

f1tenth_gym/envs/track/track.py

@@ -256,22 +256,7 @@ def from_refline(x: np.ndarray, y: np.ndarray, velx: np.ndarray):
         margin_perc = 0.1
 
         spline = CubicSpline2D(x=x, y=y)
-        ss, xs, ys, yaws, ks, vxs = [], [], [], [], [], []
-        for i_s in np.arange(0, spline.s[-1], ds):
-            xi, yi = spline.calc_position(i_s)
-            yaw = spline.calc_yaw(i_s)
-            k = spline.calc_curvature(i_s)
-
-            # find closest waypoint
-            closest = np.argmin(np.hypot(x - xi, y - yi))
-            v = velx[closest]
-
-            xs.append(xi)
-            ys.append(yi)
-            yaws.append(yaw)
-            ks.append(k)
-            ss.append(i_s)
-            vxs.append(v)
+        ss, xs, ys, yaws, ks, vxs = spline.ss, spline.xs, spline.ys, spline.psis, spline.ks, velx
 
         refline = Raceline(
             ss=np.array(ss).astype(np.float32),
@@ -316,6 +301,67 @@ def from_refline(x: np.ndarray, y: np.ndarray, velx: np.ndarray):
             raceline=refline,
             centerline=refline,
         )
+    
+    def from_raceline_file(filepath: pathlib.Path, delimiter: str = ";", skip_rows: int = 3, track_scale: float = 1.0) -> Track:
+        """
+        Creates a Track object from a raceline file of the format [s, x, y, psi, k, vx, ax].
+        
+        Args:
+            filepath (pathlib.Path): path to the raceline file
+            delimiter (str, optional): delimiter used in the file. Defaults to ";".
+            skip_rows (int, optional): number of rows to skip. Defaults to 3.
+            track_scale (float, optional): scale of the track. Defaults to 1.0.
+        
+        Returns:
+            Track: track object
+        """
+        raceline = Raceline.from_raceline_file(filepath, delimiter, skip_rows, track_scale)
+        xs = raceline.xs
+        ys = raceline.ys
+        resolution = 0.05
+        margin_perc = 0.1
+
+        min_x, max_x = np.min(xs), np.max(xs)
+        min_y, max_y = np.min(ys), np.max(ys)
+        x_range = max_x - min_x
+        y_range = max_y - min_y
+        occupancy_map = 255.0 * np.ones(
+            (
+                int((1 + 2 * margin_perc) * x_range / resolution),
+                int((1 + 2 * margin_perc) * y_range / resolution),
+            ),
+            dtype=np.float32,
+        )
+        # origin is the bottom left corner
+        origin = (min_x - margin_perc * x_range, min_y - margin_perc * y_range, 0.0)
+
+        track_spec = TrackSpec(
+            name=None,
+            image=None,
+            resolution=resolution,
+            origin=origin,
+            negate=False,
+            occupied_thresh=0.65,
+            free_thresh=0.196,
+        )
+
+        track_spec = TrackSpec(
+            name=None,
+            image=None,
+            resolution=0.05,
+            origin=(0.0, 0.0, 0.0),
+            negate=False,
+            occupied_thresh=0.65,
+            free_thresh=0.196,
+        )
+        return Track(
+            spec=track_spec,
+            filepath=None,
+            ext=None,
+            occupancy_map=occupancy_map,
+            raceline=raceline,
+            centerline=raceline,
+        )
 
     def save_raceline(self, outdir: pathlib.Path):
         """