Merge pull request #140 from f1tenth/fix_frenet_conversion_and_add_fr…

…enet_obs Fix frenet conversion and add frenet obs
f1tenth · Sep 17, 2024 · 5a301bd · 5a301bd
2 parents a7ebea5 + 11ca8de
commit 5a301bd
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 3 deletions.
diff --git a/f1tenth_gym/envs/observation.py b/f1tenth_gym/envs/observation.py
@@ -223,11 +223,13 @@ def observe(self):
             lap_count = self.env.lap_counts[i]
 
             x, y, theta = agent.state[xi], agent.state[yi], agent.state[yawi]
-            vx, vy = agent.state[vxi], 0.0
+            vlong = agent.state[vxi]
             delta = agent.state[deltai]
             beta = (
                 0.0 if len(agent.state) < 7 else agent.state[slipi]
             )  # set 0.0 when KST Model
+            vx = vlong * np.cos(beta)
+            vy = vlong * np.sin(beta)
             angvel = (
                 0.0 if len(agent.state) < 7 else agent.state[yaw_ratei]
             )  # set 0.0 when KST Model
@@ -284,5 +286,17 @@ def observation_factory(env, type: str | None, **kwargs) -> Observation:
             "beta",
         ]
         return FeaturesObservation(env, features=features)
+    elif type == "frenet_dynamic_state":
+        features = [
+            "pose_x",
+            "pose_y",
+            "delta",
+            "linear_vel_x",
+            "linear_vel_y",
+            "pose_theta",
+            "ang_vel_z",
+            "beta",
+        ]
+        return FeaturesObservation(env, features=features)
     else:
         raise ValueError(f"Invalid observation type {type}.")
diff --git a/f1tenth_gym/envs/track/cubic_spline.py b/f1tenth_gym/envs/track/cubic_spline.py
@@ -182,7 +182,7 @@ def calc_arclength_inaccurate(self, x: float, y: float) -> tuple[float, float]:
             + t * (self.s[min_dist_segment + 1] - self.s[min_dist_segment])
         )
 
-        return s, 0.0
+        return s, ey
 
     def _calc_tangent(self, s: float) -> np.ndarray:
         """

diff --git a/f1tenth_gym/envs/track/track.py b/f1tenth_gym/envs/track/track.py
@@ -276,7 +276,7 @@ def cartesian_to_frenet(self, x, y, phi, s_guess=0):
             ey: lateral deviation
             ephi: heading deviation
         """
-        s, ey = self.centerline.spline.calc_arclength(x, y, s_guess)
+        s, ey = self.centerline.spline.calc_arclength_inaccurate(x, y)
         if s > self.centerline.spline.s[-1]:
             # Wrap around
             s = s - self.centerline.spline.s[-1]