Merge pull request #116 from f1tenth/115-create-map-with-no-walls-and…

…-utility-for-adding-any-reference-line

refactoring track and rendering

examples/run_in_empty_track.py

@@ -0,0 +1,59 @@
+import numpy as np
+
+from waypoint_follow import PurePursuitPlanner
+from f110_gym.envs.track import Track
+import gymnasium as gym
+
+
+def main():
+    """
+    Demonstrate the creation of an empty map with a custom reference line.
+    This is useful for testing and debugging control algorithms on standard maneuvers.
+    """
+    # create sinusoidal reference line with custom velocity profile
+    xs = np.linspace(0, 100, 200)
+    ys = np.sin(xs / 2.0) * 5.0
+    velxs = 4.0 * (1 + (np.abs(np.cos(xs / 2.0))))
+
+    # create track from custom reference line
+    track = Track.from_refline(x=xs, y=ys, velx=velxs)
+
+    # env and planner
+    env = gym.make(
+        "f110_gym:f110-v0",
+        config={
+            "map": track,
+            "num_agents": 1,
+            "observation_config": {"type": "kinematic_state"},
+        },
+        render_mode="human",
+    )
+    planner = PurePursuitPlanner(track=track, wb=0.17145 + 0.15875)
+
+    # rendering callbacks
+    env.add_render_callback(track.raceline.render_waypoints)
+    env.add_render_callback(planner.render_lookahead_point)
+
+    # simulation
+    obs, info = env.reset()
+    done = False
+    env.render()
+
+    while not done:
+        speed, steer = planner.plan(
+            obs["agent_0"]["pose_x"],
+            obs["agent_0"]["pose_y"],
+            obs["agent_0"]["pose_theta"],
+            lookahead_distance=0.8,
+            vgain=1.0,
+        )
+        action = np.array([[steer, speed]])
+        obs, timestep, terminated, truncated, infos = env.step(action)
+        done = terminated or truncated
+        env.render()
+
+    env.close()
+
+
+if __name__ == "__main__":
+    main()

examples/waypoint_follow.py

@@ -196,13 +196,6 @@ def load_waypoints(self, conf):
             conf.wpt_path, delimiter=conf.wpt_delim, skiprows=conf.wpt_rowskip
         ).astype(np.float32)
 
-    def render_waypoints(self, e):
-        """
-        Callback to render waypoints.
-        """
-        points = self.waypoints[:, :2]
-        e.render_closed_lines(points, color=(128, 0, 0), size=1)
-
     def render_lookahead_point(self, e):
         """
         Callback to render the lookahead point.
@@ -318,7 +311,7 @@ def main():
 
     planner = PurePursuitPlanner(track=track, wb=0.17145 + 0.15875)
 
-    env.unwrapped.add_render_callback(planner.render_waypoints)
+    env.unwrapped.add_render_callback(track.raceline.render_waypoints)
     env.unwrapped.add_render_callback(planner.render_local_plan)
     env.unwrapped.add_render_callback(planner.render_lookahead_point)
 

gym/f110_gym/envs/action.py

@@ -1,4 +1,3 @@
-from __future__ import annotations
 from abc import abstractmethod
 from enum import Enum
 from typing import Any, Dict, Tuple

gym/f110_gym/envs/base_classes.py

@@ -33,6 +33,7 @@
 from f110_gym.envs.collision_models import collision_multiple, get_vertices
 from f110_gym.envs.integrator import EulerIntegrator, IntegratorType
 from f110_gym.envs.laser_models import ScanSimulator2D, check_ttc_jit, ray_cast
+from f110_gym.envs.track import Track
 
 
 class RaceCar(object):
@@ -177,14 +178,14 @@ def update_params(self, params):
         """
         self.params = params
 
-    def set_map(self, map_name: str):
+    def set_map(self, map: str | Track):
         """
         Sets the map for scan simulator
 
         Args:
-            map_name (str): name of the map
+            map (str | Track): name of the map, or Track object
         """
-        RaceCar.scan_simulator.set_map(map_name)
+        RaceCar.scan_simulator.set_map(map)
 
     def reset(self, pose):
         """
@@ -430,18 +431,18 @@ def __init__(
         num_beams = self.agents[0].scan_simulator.num_beams
         self.agent_scans = np.empty((self.num_agents, num_beams))
 
-    def set_map(self, map_name):
+    def set_map(self, map: str | Track):
         """
         Sets the map of the environment and sets the map for scan simulator of each agent
 
         Args:
-            map_name (str): name of the map
+            map (str | Track): name of the map, or Track object
 
         Returns:
             None
         """
         for agent in self.agents:
-            agent.set_map(map_name)
+            agent.set_map(map)
 
     def update_params(self, params, agent_idx=-1):
         """

gym/f110_gym/envs/f110_env.py

@@ -95,7 +95,7 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs):
         self.configure(config)
 
         self.seed = self.config["seed"]
-        self.map_name = self.config["map"]
+        self.map = self.config["map"]
         self.params = self.config["params"]
         self.num_agents = self.config["num_agents"]
         self.timestep = self.config["timestep"]
@@ -143,10 +143,14 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs):
             model=self.model,
             action_type=self.action_type,
         )
-        self.sim.set_map(self.map_name)
-        self.track = Track.from_track_name(
-            self.map_name
-        )  # load track in gym env for convenience
+        self.sim.set_map(self.map)
+
+        if isinstance(self.map, Track):
+            self.track = self.map
+        else:
+            self.track = Track.from_track_name(
+                self.map
+            )  # load track in gym env for convenience
 
         # observations
         self.agent_ids = [f"agent_{i}" for i in range(self.num_agents)]

gym/f110_gym/envs/integrator.py

@@ -1,4 +1,3 @@
-import warnings
 from abc import abstractmethod
 from enum import Enum
 

gym/f110_gym/envs/laser_models.py

@@ -25,7 +25,7 @@
 Prototype of Utility functions and classes for simulating 2D LIDAR scans
 Author: Hongrui Zheng
 """
-
+from __future__ import annotations
 import unittest
 
 import numpy as np
@@ -458,20 +458,20 @@ def __init__(self, num_beams, fov, eps=0.0001, theta_dis=2000, max_range=30.0):
         self.sines = np.sin(theta_arr)
         self.cosines = np.cos(theta_arr)
 
-    def set_map(self, map_name: str):
+    def set_map(self, map: str | Track):
         """
         Set the bitmap of the scan simulator by path
 
             Args:
-                map_name (str): name of the racetrack in the map dir, e.g. "Levine"
+                map (str | Track): path to the map file, or Track object
 
             Returns:
                 flag (bool): if image reading and loading is successful
         """
-        if self.track and self.track.spec.name == map_name:
-            return True
-
-        self.track = Track.from_track_name(map_name)
+        if isinstance(map, str):
+            self.track = Track.from_track_name(map)
+        elif isinstance(map, Track):
+            self.track = map
 
         # load map image
         self.map_img = self.track.occupancy_map

gym/f110_gym/envs/rendering/__init__.py

@@ -1,6 +1,5 @@
-from __future__ import annotations
 import pathlib
-from typing import List, Tuple, Any
+from typing import Any, Optional
 
 from f110_gym.envs.rendering.renderer import RenderSpec, EnvRenderer
 from f110_gym.envs.track import Track
@@ -10,9 +9,25 @@ def make_renderer(
     params: dict[str, Any],
     track: Track,
     agent_ids: list[str],
-    render_mode: str = None,
-    render_fps: int = 100,
-) -> Tuple[EnvRenderer, RenderSpec]:
+    render_mode: Optional[str] = None,
+    render_fps: Optional[int] = 100,
+) -> tuple[EnvRenderer, RenderSpec]:
+    """
+    Return an instance of the renderer and the rendering specification.
+
+    Parameters
+    ----------
+    params : dict
+        dictionary of renderer parameters
+    track : Track
+        track object
+    agent_ids : list
+        list of agent ids to render
+    render_mode : str, optional
+        rendering mode, by default None
+    render_fps : int, optional
+        rendering frames per second, by default 100
+    """
     from f110_gym.envs.rendering.rendering_pygame import PygameEnvRenderer
 
     cfg_file = pathlib.Path(__file__).parent.absolute() / "rendering.yaml"

gym/f110_gym/envs/rendering/objects.py

@@ -8,30 +8,75 @@
 
 
 class TextObject:
+    """
+    Class to display text on the screen at a given position.
+
+    Attributes
+    ----------
+    font : pygame.font.Font
+        font object
+    position : str | tuple
+        position of the text on the screen
+    text : pygame.Surface
+        text surface to be displayed
+    """
+
     def __init__(
         self,
-        window_shape: tuple[int, int] = (1000, 1000),
+        window_shape: tuple[int, int],
+        position: str | tuple,
         relative_font_size: int = 32,
         font_name: str = "Arial",
-        position: str | tuple = "bottom_right",
-    ):
+    ) -> None:
+        """
+        Initialize text object.
+
+        Parameters
+        ----------
+        window_shape : tuple
+            shape of the window (width, height) in pixels
+        position : str | tuple
+            position of the text on the screen
+        relative_font_size : int, optional
+            font size relative to the window shape, by default 32
+        font_name : str, optional
+            font name, by default "Arial"
+        """
         font_size = int(relative_font_size * window_shape[0] / 1000)
         self.font = pygame.font.SysFont(font_name, font_size)
         self.position = position
 
         self.text = self.font.render("", True, (125, 125, 125))
 
     def _position_resolver(
-        self, position: str | tuple, display: pygame.Surface
+        self, position: str | tuple[int, int], display: pygame.Surface
     ) -> tuple[int, int]:
         """
         This function takes strings like "bottom center" and converts them into a location for the text to be displayed.
-        if position is tuple, then passthrough.
+        If position is tuple, then passthrough.
+
+        Parameters
+        ----------
+        position : str | tuple
+            position of the text on the screen
+        display : pygame.Surface
+            display surface
+
+        Returns
+        -------
+        tuple
+            position of the text on the screen
+
+        Raises
+        ------
+        ValueError
+            if position is not a tuple or a string
+        NotImplementedError
+            if position is a string but not implemented
         """
-        if isinstance(position, tuple):
-            return position
-
-        if isinstance(position, str):
+        if isinstance(position, tuple) and len(position) == 2:
+            return int(position[0]), int(position[1])
+        elif isinstance(position, str):
             position = position.lower()
             if position == "bottom_right":
                 display_width, display_height = (
@@ -56,7 +101,7 @@ def _position_resolver(
                 )
                 text_width, text_height = self.text.get_width(), self.text.get_height()
                 bottom_center = (
-                    (display_width - text_width) / 2,
+                    (display_width - text_width) // 2,
                     display_height - text_height,
                 )
                 return bottom_center
@@ -71,12 +116,26 @@ def _position_resolver(
             elif position == "top_center":
                 display_width = display.get_width()
                 text_width = self.text.get_width()
-                top_center = ((display_width - text_width) / 2, 0)
+                top_center = ((display_width - text_width) // 2, 0)
                 return top_center
             else:
                 raise NotImplementedError(f"Position {position} not implemented.")
+        else:
+            raise ValueError(
+                f"Position expected to be a tuple[int, int] or a string. Got {position}."
+            )
 
-    def render(self, text: str, display: pygame.Surface):
+    def render(self, text: str, display: pygame.Surface) -> None:
+        """
+        Render text on the screen.
+
+        Parameters
+        ----------
+        text : str
+            text to be displayed
+        display : pygame.Surface
+            display surface                    
+        """
         self.text = self.font.render(text, True, (125, 125, 125))
         position_tuple = self._position_resolver(self.position, display)
         display.blit(self.text, position_tuple)