Fixed rendering of scaled track, works only for pyqt6 rendering

examples/waypoint_follow.py

@@ -5,6 +5,7 @@
 import numpy as np
 from numba import njit
 
+from f1tenth_gym.envs.f110_env import F110Env
 
 """
 Planner Helpers
@@ -184,11 +185,11 @@ def __init__(self, track, wb):
         self.max_reacquire = 20.0
 
         self.drawn_waypoints = []
-        self.lookahead_point = np.array([0, 0, 0], dtype=np.float32)
-        self.current_index = 0
+        self.lookahead_point = None
+        self.current_index = None
 
-        self.local_plan_render = None
         self.lookahead_point_render = None
+        self.local_plan_render = None
 
     def load_waypoints(self, conf):
         """
@@ -204,21 +205,23 @@ def render_lookahead_point(self, e):
         Callback to render the lookahead point.
         """
         if self.lookahead_point is not None:
-            points = self.lookahead_point[:2][None]  # shape (1, 2)
             if self.lookahead_point_render is None:
+                points = self.lookahead_point[:2][None]  # shape (1, 2)
                 self.lookahead_point_render = e.render_points(points, color=(0, 0, 128), size=2)
             else:
+                points = self.lookahead_point[:2][None]  # shape (1, 2)
                 self.lookahead_point_render.updateItems(points)
 
     def render_local_plan(self, e):
         """
         update waypoints being drawn by EnvRenderer
         """
         if self.current_index is not None:
-            points = self.waypoints[self.current_index : self.current_index + 10, :2]
             if self.local_plan_render is None:
+                points = self.waypoints[self.current_index : self.current_index + 10, :2]
                 self.local_plan_render = e.render_lines(points, color=(0, 128, 0), size=1)
             else:
+                points = self.waypoints[self.current_index : self.current_index + 10, :2]
                 self.local_plan_render.updateItems(points)
 
     def _get_current_waypoint(
@@ -296,11 +299,11 @@ def main():
     work = {
         "mass": 3.463388126201571,
         "lf": 0.15597534362552312,
-        "tlad": 0.82461887897713965,
+        "tlad": 0.82461887897713965*10,
         "vgain": 1,
     }
 
-    num_agents = 3
+    num_agents = 1
     env = gym.make(
         "f1tenth_gym:f1tenth-v0",
         config={
@@ -311,14 +314,15 @@ def main():
             "control_input": ["speed", "steering_angle"],
             "model": "st",
             "observation_config": {"type": "kinematic_state"},
-            "params": {"mu": 1.0},
+            "params": F110Env.fullscale_vehicle_params(),
             "reset_config": {"type": "rl_random_static"},
+            "scale": 10.0,
         },
         render_mode="human",
     )
     track = env.unwrapped.track
 
-    planner = PurePursuitPlanner(track=track, wb=0.17145 + 0.15875)
+    planner = PurePursuitPlanner(track=track, wb=(F110Env.fullscale_vehicle_params()["lf"] + F110Env.fullscale_vehicle_params()["lr"]))
 
     env.unwrapped.add_render_callback(track.raceline.render_waypoints)
     env.unwrapped.add_render_callback(planner.render_local_plan)

f1tenth_gym/envs/track/track.py

@@ -117,14 +117,19 @@ def from_track_name(track: str, track_scale: float = 1.0) -> Track:
             track_spec = Track.load_spec(
                 track=track, filespec=str(track_dir / f"{track_dir.stem}_map.yaml")
             )
+            track_spec.resolution = track_spec.resolution * track_scale
+            track_spec.origin = (
+                track_spec.origin[0] * track_scale,
+                track_spec.origin[1] * track_scale,
+                track_spec.origin[2],
+            )
 
             # load occupancy grid
             map_filename = pathlib.Path(track_spec.image)
             image = Image.open(track_dir / str(map_filename)).transpose(
                 Transpose.FLIP_TOP_BOTTOM
             )
-            image = image.resize(np.array(track_scale * np.array(image.size, dtype=np.int32), dtype=np.int32), Image.LANCZOS)
-
+
             occupancy_map = np.array(image).astype(np.float32)
             occupancy_map[occupancy_map <= 128] = 0.0
             occupancy_map[occupancy_map > 128] = 255.0