Merge pull request #112 from f1tenth/109-v100-waypoint-follow-example…

…-results-in-frequent-crashes

examples/waypoint_follow.py

@@ -310,7 +310,7 @@ def main():
             "model": "st",
             "observation_config": {"type": "kinematic_state"},
             "params": {"mu": 1.0},
-            "reset_config": {"type": "random_static"},
+            "reset_config": {"type": "rl_random_static"},
         },
         render_mode="human",
     )

gym/f110_gym/envs/f110_env.py

@@ -226,8 +226,8 @@ def default_config(cls) -> dict:
             "integrator": "rk4",
             "model": "st",
             "control_input": ["speed", "steering_angle"],
-            "observation_config": {"type": "original"},
-            "reset_config": {"type": "grid_static"},
+            "observation_config": {"type": None},
+            "reset_config": {"type": None},
         }
 
     def configure(self, config: dict) -> None:

gym/f110_gym/envs/observation.py

@@ -1,6 +1,4 @@
-"""
-Author: Luigi Berducci
-"""
+from __future__ import annotations
 from abc import abstractmethod
 from typing import List
 
@@ -265,7 +263,9 @@ def observe(self):
         return obs
 
 
-def observation_factory(env, type: str, **kwargs) -> Observation:
+def observation_factory(env, type: str | None, **kwargs) -> Observation:
+    type = type or "original"
+
     if type == "original":
         return OriginalObservation(env)
     elif type == "features":

gym/f110_gym/envs/reset/__init__.py

@@ -1,20 +1,21 @@
+from __future__ import annotations
 from f110_gym.envs.reset.masked_reset import GridResetFn, AllTrackResetFn
 from f110_gym.envs.reset.reset_fn import ResetFn
 from f110_gym.envs.track import Track
 
 
-def make_reset_fn(type: str, track: Track, num_agents: int, **kwargs) -> ResetFn:
-    if type == "grid_static":
-        return GridResetFn(track=track, num_agents=num_agents, shuffle=False, **kwargs)
-    elif type == "grid_random":
-        return GridResetFn(track=track, num_agents=num_agents, shuffle=True, **kwargs)
-    elif type == "random_static":
-        return AllTrackResetFn(
-            track=track, num_agents=num_agents, shuffle=False, **kwargs
-        )
-    elif type == "random_random":
-        return AllTrackResetFn(
-            track=track, num_agents=num_agents, shuffle=True, **kwargs
-        )
-    else:
-        raise ValueError(f"invalid reset type {type}")
+def make_reset_fn(type: str | None, track: Track, num_agents: int, **kwargs) -> ResetFn:
+    type = type or "rl_grid_static"
+
+    try:
+        refline_token, reset_token, shuffle_token = type.split("_")
+
+        refline = {"cl": track.centerline, "rl": track.raceline}[refline_token]
+        reset_fn = {"grid": GridResetFn, "random": AllTrackResetFn}[reset_token]
+        shuffle = {"static": False, "random": True}[shuffle_token]
+        options = {"cl": {"move_laterally": True}, "rl": {"move_laterally": False}}[refline_token]
+
+    except Exception as ex:
+        raise ValueError(f"Invalid reset function type: {type}. Expected format: <refline>_<resetfn>_<shuffle>") from ex
+
+    return reset_fn(reference_line=refline, num_agents=num_agents, shuffle=shuffle, **options, **kwargs)

gym/f110_gym/envs/reset/masked_reset.py

@@ -4,7 +4,7 @@
 
 from f110_gym.envs.reset.reset_fn import ResetFn
 from f110_gym.envs.reset.utils import sample_around_waypoint
-from f110_gym.envs.track import Track
+from f110_gym.envs.track import Track, Raceline
 
 
 class MaskedResetFn(ResetFn):
@@ -14,23 +14,24 @@ def get_mask(self) -> np.ndarray:
 
     def __init__(
         self,
-        track: Track,
+        reference_line: Raceline,
         num_agents: int,
         move_laterally: bool,
         min_dist: float,
         max_dist: float,
     ):
-        self.track = track
+        self.reference_line = reference_line
         self.n_agents = num_agents
         self.min_dist = min_dist
         self.max_dist = max_dist
         self.move_laterally = move_laterally
         self.mask = self.get_mask()
+        self.reference_line = reference_line
 
     def sample(self) -> np.ndarray:
         waypoint_id = np.random.choice(np.where(self.mask)[0])
         poses = sample_around_waypoint(
-            track=self.track,
+            reference_line=self.reference_line,
             waypoint_id=waypoint_id,
             n_agents=self.n_agents,
             min_dist=self.min_dist,
@@ -43,9 +44,10 @@ def sample(self) -> np.ndarray:
 class GridResetFn(MaskedResetFn):
     def __init__(
         self,
-        track: Track,
+        reference_line: Raceline,
         num_agents: int,
         move_laterally: bool = True,
+        use_centerline: bool = True,
         shuffle: bool = True,
         start_width: float = 1.0,
         min_dist: float = 1.5,
@@ -55,7 +57,7 @@ def __init__(
         self.shuffle = shuffle
 
         super().__init__(
-            track=track,
+            reference_line=reference_line,
             num_agents=num_agents,
             move_laterally=move_laterally,
             min_dist=min_dist,
@@ -64,10 +66,10 @@ def __init__(
 
     def get_mask(self) -> np.ndarray:
         # approximate the nr waypoints in the starting line
-        step_size = self.track.centerline.length / self.track.centerline.n
+        step_size = self.reference_line.length / self.reference_line.n
         n_wps = int(self.start_width / step_size)
 
-        mask = np.zeros(self.track.centerline.n)
+        mask = np.zeros(self.reference_line.n)
         mask[:n_wps] = 1
         return mask.astype(bool)
 
@@ -83,15 +85,15 @@ def sample(self) -> np.ndarray:
 class AllTrackResetFn(MaskedResetFn):
     def __init__(
         self,
-        track: Track,
+        reference_line: Raceline,
         num_agents: int,
         move_laterally: bool = True,
         shuffle: bool = True,
         min_dist: float = 1.5,
         max_dist: float = 2.5,
     ):
         super().__init__(
-            track=track,
+            reference_line=reference_line,
             num_agents=num_agents,
             move_laterally=move_laterally,
             min_dist=min_dist,
@@ -100,7 +102,7 @@ def __init__(
         self.shuffle = shuffle
 
     def get_mask(self) -> np.ndarray:
-        return np.ones(self.track.centerline.n).astype(bool)
+        return np.ones(self.reference_line.n).astype(bool)
 
     def sample(self) -> np.ndarray:
         poses = super().sample()

gym/f110_gym/envs/reset/utils.py

@@ -2,11 +2,11 @@
 
 import numpy as np
 
-from f110_gym.envs.track import Track
+from f110_gym.envs.track import Track, Raceline
 
 
 def sample_around_waypoint(
-    track: Track,
+    reference_line: Raceline,
     waypoint_id: int,
     n_agents: int,
     min_dist: float,
@@ -26,22 +26,22 @@ def sample_around_waypoint(
         - move_laterally: if True, the agents are sampled on the left/right of the track centerline
     """
     current_wp_id = waypoint_id
-    n_waypoints = track.centerline.n
+    n_waypoints = reference_line.n
 
     poses = []
     rnd_sign = (
-        np.random.choice([-1, 1]) if move_laterally else 1
+        np.random.choice([-1.0, 1.0]) if move_laterally else 0.0
     )  # random sign to sample lateral position (left/right)
     for i in range(n_agents):
         # compute pose from current wp_id
         wp = [
-            track.centerline.xs[current_wp_id],
-            track.centerline.ys[current_wp_id],
+            reference_line.xs[current_wp_id],
+            reference_line.ys[current_wp_id],
         ]
         next_wp_id = (current_wp_id + 1) % n_waypoints
         next_wp = [
-            track.centerline.xs[next_wp_id],
-            track.centerline.ys[next_wp_id],
+            reference_line.xs[next_wp_id],
+            reference_line.ys[next_wp_id],
         ]
         theta = np.arctan2(next_wp[1] - wp[1], next_wp[0] - wp[0])
 
@@ -65,8 +65,8 @@ def sample_around_waypoint(
             if pnt_id > n_waypoints - 1:
                 pnt_id = 0
             # increment distance
-            x_diff = track.centerline.xs[pnt_id] - track.centerline.xs[pnt_id - 1]
-            y_diff = track.centerline.ys[pnt_id] - track.centerline.ys[pnt_id - 1]
+            x_diff = reference_line.xs[pnt_id] - reference_line.xs[pnt_id - 1]
+            y_diff = reference_line.ys[pnt_id] - reference_line.ys[pnt_id - 1]
             dist = dist + np.linalg.norm(
                 [y_diff, x_diff]
             )  # approx distance by summing linear segments