diff --git a/examples/random_trackgen.py b/examples/random_trackgen.py index fb725652..6179492a 100644 --- a/examples/random_trackgen.py +++ b/examples/random_trackgen.py @@ -162,7 +162,7 @@ def create_track(): assert i1 != -1 assert i2 != -1 - track = track[i1: i2 - 1] + track = track[i1 : i2 - 1] first_beta = track[0][1] first_perp_x = math.cos(first_beta) first_perp_y = math.sin(first_beta) diff --git a/examples/video_recording.py b/examples/video_recording.py new file mode 100644 index 00000000..dc965005 --- /dev/null +++ b/examples/video_recording.py @@ -0,0 +1,78 @@ +import time +import gymnasium as gym +import gymnasium.wrappers +import numpy as np + +from waypoint_follow import PurePursuitPlanner + + +def main(): + work = { + "mass": 3.463388126201571, + "lf": 0.15597534362552312, + "tlad": 0.82461887897713965, + "vgain": 1, + } + + env = gym.make( + "f110_gym:f110-v0", + config={ + "map": "Spielberg", + "num_agents": 1, + "timestep": 0.01, + "integrator": "rk4", + "control_input": "speed", + "model": "st", + "observation_config": {"type": "kinematic_state"}, + "params": {"mu": 1.0}, + }, + render_mode="rgb_array", + ) + env = gymnasium.wrappers.RecordVideo(env, f"video_{time.time()}") + track = env.unwrapped.track + + planner = PurePursuitPlanner(track=track, wb=0.17145 + 0.15875) + + poses = np.array( + [ + [ + track.raceline.xs[0], + track.raceline.ys[0], + track.raceline.yaws[0], + ] + ] + ) + + obs, info = env.reset(options={"poses": poses}) + done = False + + laptime = 0.0 + start = time.time() + + frames = [env.render()] + while not done and laptime < 15.0: + action = env.action_space.sample() + for i, agent_id in enumerate(obs.keys()): + speed, steer = planner.plan( + obs[agent_id]["pose_x"], + obs[agent_id]["pose_y"], + obs[agent_id]["pose_theta"], + work["tlad"], + work["vgain"], + ) + action[i] = [steer, speed] + + obs, step_reward, done, truncated, info = env.step(action) + laptime += step_reward + + frame = env.render() + frames.append(frame) + + print("Sim elapsed time:", laptime, "Real elapsed time:", time.time() - start) + + # close env to trigger video saving + env.close() + + +if __name__ == "__main__": + main() diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py index ee4763c1..dde0469f 100644 --- a/examples/waypoint_follow.py +++ b/examples/waypoint_follow.py @@ -1,9 +1,10 @@ import time +from typing import Tuple import gymnasium as gym import numpy as np from numba import njit -from pyglet.gl import GL_POINTS + """ Planner Helpers @@ -183,6 +184,8 @@ def __init__(self, track, wb): self.max_reacquire = 20.0 self.drawn_waypoints = [] + self.lookahead_point = None + self.current_index = None def load_waypoints(self, conf): """ @@ -195,32 +198,42 @@ def load_waypoints(self, conf): def render_waypoints(self, e): """ - update waypoints being drawn by EnvRenderer + 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. + """ + if self.lookahead_point is not None: + points = self.lookahead_point[:2][None] # shape (1, 2) + e.render_points(points, color=(0, 0, 128), size=2) + + 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] + e.render_lines(points, color=(0, 128, 0), size=1) - scaled_points = 50.0 * points - - for i in range(points.shape[0]): - if len(self.drawn_waypoints) < points.shape[0]: - b = e.batch.add( - 1, - GL_POINTS, - None, - ("v3f/stream", [scaled_points[i, 0], scaled_points[i, 1], 0.0]), - ("c3B/stream", [183, 193, 222]), - ) - self.drawn_waypoints.append(b) - else: - self.drawn_waypoints[i].vertices = [ - scaled_points[i, 0], - scaled_points[i, 1], - 0.0, - ] - - def _get_current_waypoint(self, waypoints, lookahead_distance, position, theta): + def _get_current_waypoint( + self, waypoints, lookahead_distance, position, theta + ) -> Tuple[np.ndarray, int]: """ - gets the current waypoint to follow + Returns the current waypoint to follow given the current pose. + + Args: + waypoints: The waypoints to follow (Nx3 array) + lookahead_distance: The lookahead distance [m] + position: The current position (2D array) + theta: The current heading [rad] + + Returns: + waypoint: The current waypoint to follow (x, y, speed) + i: The index of the current waypoint """ wpts = waypoints[:, :2] lookahead_distance = np.float32(lookahead_distance) @@ -231,61 +244,48 @@ def _get_current_waypoint(self, waypoints, lookahead_distance, position, theta): position, lookahead_distance, wpts, t1, wrap=True ) if i2 is None: - return None + return None, None current_waypoint = np.empty((3,), dtype=np.float32) # x, y current_waypoint[0:2] = wpts[i2, :] # speed current_waypoint[2] = waypoints[i, -1] - return current_waypoint + return current_waypoint, i elif nearest_dist < self.max_reacquire: # NOTE: specify type or numba complains - return wpts[i, :] + return wpts[i, :], i else: - return None + return None, None def plan(self, pose_x, pose_y, pose_theta, lookahead_distance, vgain): """ gives actuation given observation """ position = np.array([pose_x, pose_y]) - lookahead_point = self._get_current_waypoint( + lookahead_point, i = self._get_current_waypoint( self.waypoints, lookahead_distance, position, pose_theta ) if lookahead_point is None: return 4.0, 0.0 + # for rendering + self.lookahead_point = lookahead_point + self.current_index = i + + # actuation speed, steering_angle = get_actuation( - pose_theta, lookahead_point, position, lookahead_distance, self.wheelbase + pose_theta, + self.lookahead_point, + position, + lookahead_distance, + self.wheelbase, ) speed = vgain * speed return speed, steering_angle -class FlippyPlanner: - """ - Planner designed to exploit integration methods and dynamics. - For testing only. To observe this error, use single track dynamics for all velocities >0.1 - """ - - def __init__(self, speed=1, flip_every=1, steer=2): - self.speed = speed - self.flip_every = flip_every - self.counter = 0 - self.steer = steer - - def render_waypoints(self, *args, **kwargs): - pass - - def plan(self, *args, **kwargs): - if self.counter % self.flip_every == 0: - self.counter = 0 - self.steer *= -1 - return self.speed, self.steer - - def main(): """ main entry point @@ -296,7 +296,7 @@ def main(): "lf": 0.15597534362552312, "tlad": 0.82461887897713965, "vgain": 1, - } # 0.90338203837889} + } env = gym.make( "f110_gym:f110-v0", @@ -312,40 +312,23 @@ def main(): }, render_mode="human", ) + track = env.unwrapped.track - planner = PurePursuitPlanner( - track=env.track, wb=0.17145 + 0.15875 - ) # FlippyPlanner(speed=0.2, flip_every=1, steer=10) - - def render_callback(env_renderer): - # custom extra drawing function - - e = env_renderer - - # update camera to follow car - x = e.cars[0].vertices[::2] - y = e.cars[0].vertices[1::2] - top, bottom, left, right = max(y), min(y), min(x), max(x) - e.score_label.x = left - e.score_label.y = top - 700 - e.left = left - 800 - e.right = right + 800 - e.top = top + 800 - e.bottom = bottom - 800 - - planner.render_waypoints(env_renderer) - - env.add_render_callback(render_callback) + planner = PurePursuitPlanner(track=track, wb=0.17145 + 0.15875) poses = np.array( [ [ - env.track.raceline.xs[0], - env.track.raceline.ys[0], - env.track.raceline.yaws[0], + track.raceline.xs[0], + track.raceline.ys[0], + track.raceline.yaws[0], ] ] ) + env.unwrapped.add_render_callback(planner.render_waypoints) + env.unwrapped.add_render_callback(planner.render_local_plan) + env.unwrapped.add_render_callback(planner.render_lookahead_point) + obs, info = env.reset(options={"poses": poses}) done = False env.render() @@ -354,17 +337,20 @@ def render_callback(env_renderer): start = time.time() while not done: - agent_id = env.agent_ids[0] - speed, steer = planner.plan( - obs[agent_id]["pose_x"], - obs[agent_id]["pose_y"], - obs[agent_id]["pose_theta"], - work["tlad"], - work["vgain"], - ) - obs, step_reward, done, truncated, info = env.step(np.array([[steer, speed]])) + action = env.action_space.sample() + for i, agent_id in enumerate(obs.keys()): + speed, steer = planner.plan( + obs[agent_id]["pose_x"], + obs[agent_id]["pose_y"], + obs[agent_id]["pose_theta"], + work["tlad"], + work["vgain"], + ) + action[i] = np.array([steer, speed]) + + obs, step_reward, done, truncated, info = env.step(action) laptime += step_reward - env.render() + frame = env.render() print("Sim elapsed time:", laptime, "Real elapsed time:", time.time() - start) diff --git a/gym/f110_gym/__init__.py b/gym/f110_gym/__init__.py index 20052799..a993eb05 100644 --- a/gym/f110_gym/__init__.py +++ b/gym/f110_gym/__init__.py @@ -1,6 +1,5 @@ import gymnasium as gym gym.register( - id="f110-v0", - entry_point="f110_gym.envs:F110Env", + id="f110-v0", entry_point="f110_gym.envs:F110Env", ) diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py index 016569f9..5e560186 100644 --- a/gym/f110_gym/envs/base_classes.py +++ b/gym/f110_gym/envs/base_classes.py @@ -407,6 +407,7 @@ def __init__( self.ego_idx = ego_idx self.params = params self.agent_poses = np.empty((self.num_agents, 3)) + self.agent_steerings = np.empty((self.num_agents,)) self.agents: list[RaceCar] = [] self.collisions = np.zeros((self.num_agents,)) self.collision_idx = -1 * np.ones((self.num_agents,)) @@ -504,6 +505,7 @@ def step(self, control_inputs): # update sim's information of agent poses self.agent_poses[i, :] = np.append(agent.state[0:2], agent.state[4]) + self.agent_steerings[i] = agent.state[2] # check collisions between all agents self.check_collision() diff --git a/gym/f110_gym/envs/cubic_spline.py b/gym/f110_gym/envs/cubic_spline.py index 8b1a0fab..5ed193d6 100644 --- a/gym/f110_gym/envs/cubic_spline.py +++ b/gym/f110_gym/envs/cubic_spline.py @@ -64,7 +64,7 @@ def calc_position(self, x): i = self.__search_index(x) dx = x - self.x[i] position = ( - self.a[i] + self.b[i] * dx + self.c[i] * dx**2.0 + self.d[i] * dx**3.0 + self.a[i] + self.b[i] * dx + self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 ) return position @@ -86,7 +86,7 @@ def calc_first_derivative(self, x): i = self.__search_index(x) dx = x - self.x[i] - dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx**2.0 + dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 return dy def calc_second_derivative(self, x): @@ -205,7 +205,7 @@ def calc_curvature(self, s): ddx = self.sx.calc_second_derivative(s) dy = self.sy.calc_first_derivative(s) ddy = self.sy.calc_second_derivative(s) - k = (ddy * dx - ddx * dy) / ((dx**2 + dy**2) ** (3 / 2)) + k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2) ** (3 / 2)) return k def calc_yaw(self, s): diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py index 7353fe3c..51e7c54f 100644 --- a/gym/f110_gym/envs/f110_env.py +++ b/gym/f110_gym/envs/f110_env.py @@ -24,30 +24,24 @@ Author: Hongrui Zheng """ -import time - # gym imports import gymnasium as gym -# others -import numpy as np -# gl -import pyglet + +from f110_gym.envs.action import CarActionEnum, from_single_to_multi_action_space from f110_gym.envs.integrator import IntegratorType -from f110_gym.envs.action import (CarActionEnum, - from_single_to_multi_action_space) +from f110_gym.envs.rendering import make_renderer + +from f110_gym.envs.track import Track + # base classes -from f110_gym.envs.base_classes import DynamicModel, Simulator +from f110_gym.envs.base_classes import Simulator, DynamicModel from f110_gym.envs.observation import observation_factory -from f110_gym.envs.track import Track + from f110_gym.envs.utils import deep_update -pyglet.options["debug_gl"] = False -# rendering -VIDEO_W = 600 -VIDEO_H = 400 -WINDOW_W = 1000 -WINDOW_H = 800 +# others +import numpy as np class F110Env(gym.Env): @@ -91,11 +85,6 @@ class F110Env(gym.Env): # NOTE: change matadata with default rendering-modes, add definition of render_fps metadata = {"render_modes": ["human", "human_fast", "rgb_array"], "render_fps": 100} - # rendering - renderer = None - current_obs = None - render_callbacks = [] - def __init__(self, config: dict = None, render_mode=None, **kwargs): super().__init__() @@ -173,9 +162,20 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs): ) # stateful observations for rendering + # add choice of colors (same, random, ...) self.render_obs = None self.render_mode = render_mode + # match render_fps to integration timestep + self.metadata["render_fps"] = int(1.0 / self.timestep) + if self.render_mode == "human_fast": + self.metadata["render_fps"] *= 10 # boost fps by 10x + self.renderer, self.render_spec = make_renderer( + params=self.params, track=self.track, agent_ids=self.agent_ids, + render_mode=render_mode, render_fps=self.metadata["render_fps"] + ) + + @classmethod def default_config(cls) -> dict: """ @@ -264,7 +264,7 @@ def _check_done(self): temp_y[idx2] = -right_t - temp_y[idx2] temp_y[np.invert(np.logical_or(idx1, idx2))] = 0 - dist2 = delta_pt[0, :] ** 2 + temp_y**2 + dist2 = delta_pt[0, :] ** 2 + temp_y ** 2 closes = dist2 <= 0.1 for i in range(self.num_agents): if closes[i] and not self.near_starts[i]: @@ -310,24 +310,26 @@ def step(self, action): # observation obs = self.observation_type.observe() + # times + reward = self.timestep + self.current_time = self.current_time + self.timestep + + # update data member + self._update_state() + # rendering observation - F110Env.current_obs = obs self.render_obs = { "ego_idx": self.sim.ego_idx, "poses_x": self.sim.agent_poses[:, 0], "poses_y": self.sim.agent_poses[:, 1], "poses_theta": self.sim.agent_poses[:, 2], + "steering_angles": self.sim.agent_steerings, "lap_times": self.lap_times, "lap_counts": self.lap_counts, + "collisions": self.sim.collisions, + "sim_time": self.current_time, } - # times - reward = self.timestep - self.current_time = self.current_time + self.timestep - - # update data member - self._update_state() - # check done done, toggle_list = self._check_done() truncated = False @@ -429,7 +431,7 @@ def add_render_callback(self, callback_func): callback_func (function (EnvRenderer) -> None): custom function to called during render() """ - F110Env.render_callbacks.append(callback_func) + self.renderer.add_renderer_callback(callback_func) def render(self, mode="human"): """ @@ -447,46 +449,14 @@ def render(self, mode="human"): if self.render_mode not in self.metadata["render_modes"]: return - if self.render_mode in ["human", "human_fast"]: - self.render_frame(mode=self.render_mode) - elif self.render_mode == "rgb_array": - # NOTE: this is extremely slow and should be changed to use pygame - import PIL - from PIL.Image import Transpose - - self.render_frame(mode="human_fast") - image_data = ( - pyglet.image.get_buffer_manager().get_color_buffer().get_image_data() - ) - fmt = "RGB" - pitch = image_data.width * len(fmt) - pil_image = PIL.Image.frombytes( - fmt, - (image_data.width, image_data.height), - image_data.get_data(fmt, pitch), - ) - pil_image = pil_image.transpose(Transpose.FLIP_TOP_BOTTOM) - return np.array(pil_image) - else: - raise NotImplementedError(f"mode {self.render_mode} not implemented") - - def render_frame(self, mode): - if F110Env.renderer is None: - # first call, initialize everything - from f110_gym.envs.rendering import EnvRenderer - - F110Env.renderer = EnvRenderer(WINDOW_W, WINDOW_H) - F110Env.renderer.update_map(track=self.track) - - F110Env.renderer.update_obs(self.render_obs) - - for render_callback in F110Env.render_callbacks: - render_callback(F110Env.renderer) - - F110Env.renderer.dispatch_events() - F110Env.renderer.on_draw() - F110Env.renderer.flip() - if mode == "human": - time.sleep(0.005) - elif mode == "human_fast": - pass + + self.renderer.update(state=self.render_obs) + return self.renderer.render() + + def close(self): + """ + Ensure renderer is closed upon deletion + """ + if self.renderer is not None: + self.renderer.close() + super().close() diff --git a/gym/f110_gym/envs/f110_env_backup.py b/gym/f110_gym/envs/f110_env_backup.py index 4e60cdfc..e0acc38d 100644 --- a/gym/f110_gym/envs/f110_env_backup.py +++ b/gym/f110_gym/envs/f110_env_backup.py @@ -261,7 +261,7 @@ def _check_done(self): temp_y[idx2] = -right_t - temp_y[idx2] temp_y[np.invert(np.logical_or(idx1, idx2))] = 0 - dist2 = delta_pt[0, :] ** 2 + temp_y**2 + dist2 = delta_pt[0, :] ** 2 + temp_y ** 2 closes = dist2 <= 0.1 for i in range(self.num_agents): if closes[i] and not self.near_starts[i]: @@ -289,7 +289,7 @@ def _check_done(self): temp_y = -right_t - delta_pt[1] else: temp_y = 0 - dist2 = delta_pt[0] ** 2 + temp_y**2 + dist2 = delta_pt[0] ** 2 + temp_y ** 2 close = dist2 <= 0.1 # close = dist_to_start <= self.start_thresh if close and not self.near_start: diff --git a/gym/f110_gym/envs/rendering.py b/gym/f110_gym/envs/rendering.py deleted file mode 100644 index 9d863f43..00000000 --- a/gym/f110_gym/envs/rendering.py +++ /dev/null @@ -1,357 +0,0 @@ -# MIT License - -# Copyright (c) 2020 Joseph Auckley, Matthew O'Kelly, Aman Sinha, Hongrui Zheng - -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: - -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. - -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - -# flake8: noqa - -""" -Rendering engine for f1tenth gym env based on pyglet and OpenGL -Author: Hongrui Zheng -""" - -# other -import numpy as np -# opengl stuff -import pyglet -# helpers -from f110_gym.envs.collision_models import get_vertices -from f110_gym.envs.track import Track -from pyglet.gl import * - -# zooming constants -ZOOM_IN_FACTOR = 1.2 -ZOOM_OUT_FACTOR = 1 / ZOOM_IN_FACTOR - -# vehicle shape constants -CAR_LENGTH = 0.58 -CAR_WIDTH = 0.31 - - -class EnvRenderer(pyglet.window.Window): - """ - A window class inherited from pyglet.window.Window, handles the camera/projection interaction, resizing window, and rendering the environment - """ - - def __init__(self, width, height, *args, **kwargs): - """ - Class constructor - - Args: - width (int): width of the window - height (int): height of the window - - Returns: - None - """ - conf = Config(sample_buffers=1, samples=4, depth_size=16, double_buffer=True) - super().__init__( - width, height, config=conf, resizable=True, vsync=False, *args, **kwargs - ) - - # gl init - glClearColor(9 / 255, 32 / 255, 87 / 255, 1.0) - - # initialize camera values - self.left = -width / 2 - self.right = width / 2 - self.bottom = -height / 2 - self.top = height / 2 - self.zoom_level = 1.2 - self.zoomed_width = width - self.zoomed_height = height - - # current batch that keeps track of all graphics - self.batch = pyglet.graphics.Batch() - - # current env map - self.map_points = None - - # current env agent poses, (num_agents, 3), columns are (x, y, theta) - self.poses = None - - # current env agent vertices, (num_agents, 4, 2), 2nd and 3rd dimensions are the 4 corners in 2D - self.vertices = None - - # current score label - self.score_label = pyglet.text.Label( - "Lap Time: {laptime:.2f}, Ego Lap Count: {count:.0f}".format( - laptime=0.0, count=0.0 - ), - font_size=36, - x=0, - y=-800, - anchor_x="center", - anchor_y="center", - # width=0.01, - # height=0.01, - color=(255, 255, 255, 255), - batch=self.batch, - ) - - self.fps_display = pyglet.window.FPSDisplay(self) - - def update_map(self, track: Track): - """ - Update the map being drawn by the renderer. Converts image to a list of 3D points representing each obstacle pixel in the map. - - Args: - map_path (str): absolute path to the map without extensions - map_ext (str): extension for the map image file - - Returns: - None - """ - # occupancy map - map_img = track.occupancy_map - map_height = map_img.shape[0] - map_width = map_img.shape[1] - - # map spec - map_resolution = track.spec.resolution - origin = track.spec.origin - origin_x, origin_y, _ = origin - - # convert map pixels to coordinates - range_x = np.arange(map_width) - range_y = np.arange(map_height) - map_x, map_y = np.meshgrid(range_x, range_y) - map_x = (map_x * map_resolution + origin_x).flatten() - map_y = (map_y * map_resolution + origin_y).flatten() - map_z = np.zeros(map_y.shape) - map_coords = np.vstack((map_x, map_y, map_z)) - - # mask and only leave the obstacle points - map_mask = map_img == 0.0 - map_mask_flat = map_mask.flatten() - map_points = 50.0 * map_coords[:, map_mask_flat].T - for i in range(map_points.shape[0]): - self.batch.add( - 1, - GL_POINTS, - None, - ("v3f/stream", [map_points[i, 0], map_points[i, 1], map_points[i, 2]]), - ("c3B/stream", [183, 193, 222]), - ) - self.map_points = map_points - - def on_resize(self, width, height): - """ - Callback function on window resize, overrides inherited method, and updates camera values on top of the inherited on_resize() method. - - Potential improvements on current behavior: zoom/pan resets on window resize. - - Args: - width (int): new width of window - height (int): new height of window - - Returns: - None - """ - - # call overrided function - super().on_resize(width, height) - - # update camera value - (width, height) = self.get_size() - self.left = -self.zoom_level * width / 2 - self.right = self.zoom_level * width / 2 - self.bottom = -self.zoom_level * height / 2 - self.top = self.zoom_level * height / 2 - self.zoomed_width = self.zoom_level * width - self.zoomed_height = self.zoom_level * height - - def on_mouse_drag(self, x, y, dx, dy, buttons, modifiers): - """ - Callback function on mouse drag, overrides inherited method. - - Args: - x (int): Distance in pixels from the left edge of the window. - y (int): Distance in pixels from the bottom edge of the window. - dx (int): Relative X position from the previous mouse position. - dy (int): Relative Y position from the previous mouse position. - buttons (int): Bitwise combination of the mouse buttons currently pressed. - modifiers (int): Bitwise combination of any keyboard modifiers currently active. - - Returns: - None - """ - - # pan camera - self.left -= dx * self.zoom_level - self.right -= dx * self.zoom_level - self.bottom -= dy * self.zoom_level - self.top -= dy * self.zoom_level - - def on_mouse_scroll(self, x, y, dx, dy): - """ - Callback function on mouse scroll, overrides inherited method. - - Args: - x (int): Distance in pixels from the left edge of the window. - y (int): Distance in pixels from the bottom edge of the window. - scroll_x (float): Amount of movement on the horizontal axis. - scroll_y (float): Amount of movement on the vertical axis. - - Returns: - None - """ - - # Get scale factor - f = ZOOM_IN_FACTOR if dy > 0 else ZOOM_OUT_FACTOR if dy < 0 else 1 - - # If zoom_level is in the proper range - if 0.01 < self.zoom_level * f < 10: - self.zoom_level *= f - - (width, height) = self.get_size() - - mouse_x = x / width - mouse_y = y / height - - mouse_x_in_world = self.left + mouse_x * self.zoomed_width - mouse_y_in_world = self.bottom + mouse_y * self.zoomed_height - - self.zoomed_width *= f - self.zoomed_height *= f - - self.left = mouse_x_in_world - mouse_x * self.zoomed_width - self.right = mouse_x_in_world + (1 - mouse_x) * self.zoomed_width - self.bottom = mouse_y_in_world - mouse_y * self.zoomed_height - self.top = mouse_y_in_world + (1 - mouse_y) * self.zoomed_height - - def on_close(self): - """ - Callback function when the 'x' is clicked on the window, overrides inherited method. Also throws exception to end the python program when in a loop. - - Args: - None - - Returns: - None - - Raises: - Exception: with a message that indicates the rendering window was closed - """ - - super().on_close() - raise Exception("Rendering window was closed.") - - def on_draw(self): - """ - Function when the pyglet is drawing. The function draws the batch created that includes the map points, the agent polygons, and the information text, and the fps display. - - Args: - None - - Returns: - None - """ - - # if map and poses doesn't exist, raise exception - if self.map_points is None: - raise Exception("Map not set for renderer.") - if self.poses is None: - raise Exception("Agent poses not updated for renderer.") - - # Initialize Projection matrix - glMatrixMode(GL_PROJECTION) - glLoadIdentity() - - # Initialize Modelview matrix - glMatrixMode(GL_MODELVIEW) - glLoadIdentity() - # Save the default modelview matrix - glPushMatrix() - - # Clear window with ClearColor - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) - - # Set orthographic projection matrix - glOrtho(self.left, self.right, self.bottom, self.top, 1, -1) - - # Draw all batches - self.batch.draw() - self.fps_display.draw() - # Remove default modelview matrix - glPopMatrix() - - def update_obs(self, obs): - """ - Updates the renderer with the latest observation from the gym environment, including the agent poses, and the information text. - - Args: - obs (dict): observation dict from the gym env - - Returns: - None - """ - - self.ego_idx = obs["ego_idx"] - poses_x = obs["poses_x"] - poses_y = obs["poses_y"] - poses_theta = obs["poses_theta"] - - num_agents = len(poses_x) - if self.poses is None: - self.cars = [] - for i in range(num_agents): - if i == self.ego_idx: - vertices_np = get_vertices( - np.array([0.0, 0.0, 0.0]), CAR_LENGTH, CAR_WIDTH - ) - vertices = list(vertices_np.flatten()) - car = self.batch.add( - 4, - GL_QUADS, - None, - ("v2f", vertices), - ( - "c3B", - [172, 97, 185, 172, 97, 185, 172, 97, 185, 172, 97, 185], - ), - ) - self.cars.append(car) - else: - vertices_np = get_vertices( - np.array([0.0, 0.0, 0.0]), CAR_LENGTH, CAR_WIDTH - ) - vertices = list(vertices_np.flatten()) - car = self.batch.add( - 4, - GL_QUADS, - None, - ("v2f", vertices), - ("c3B", [99, 52, 94, 99, 52, 94, 99, 52, 94, 99, 52, 94]), - ) - self.cars.append(car) - - poses = np.stack((poses_x, poses_y, poses_theta)).T - for j in range(poses.shape[0]): - vertices_np = 50.0 * get_vertices(poses[j, :], CAR_LENGTH, CAR_WIDTH) - vertices = list(vertices_np.flatten()) - self.cars[j].vertices = vertices - self.poses = poses - - self.score_label.text = ( - "Lap Time: {laptime:.2f}, Ego Lap Count: {count:.0f}".format( - laptime=obs["lap_times"][0], count=obs["lap_counts"][obs["ego_idx"]] - ) - ) diff --git a/gym/f110_gym/envs/rendering/__init__.py b/gym/f110_gym/envs/rendering/__init__.py new file mode 100644 index 00000000..cbc3ba3b --- /dev/null +++ b/gym/f110_gym/envs/rendering/__init__.py @@ -0,0 +1,32 @@ +from __future__ import annotations +import pathlib +from typing import List, Tuple, Any + +from f110_gym.envs.rendering.renderer import RenderSpec, EnvRenderer +from f110_gym.envs.track import Track + + +def make_renderer( + params: dict[str, Any], + track: Track, + agent_ids: list[str], + render_mode: str = None, + render_fps: int = 100, +) -> Tuple[EnvRenderer, RenderSpec]: + from f110_gym.envs.rendering.rendering_pygame import PygameEnvRenderer + + cfg_file = pathlib.Path(__file__).parent.absolute() / "rendering.yaml" + render_spec = RenderSpec.from_yaml(cfg_file) + + if render_mode in ["human", "rgb_array", "human_fast"]: + renderer = PygameEnvRenderer( + params=params, + track=track, + agent_ids=agent_ids, + render_spec=render_spec, + render_mode=render_mode, + render_fps=render_fps, + ) + else: + renderer = None + return renderer, render_spec diff --git a/gym/f110_gym/envs/rendering/objects.py b/gym/f110_gym/envs/rendering/objects.py new file mode 100644 index 00000000..002a1c71 --- /dev/null +++ b/gym/f110_gym/envs/rendering/objects.py @@ -0,0 +1,190 @@ +from __future__ import annotations +import cv2 +import numpy as np +import pygame + +from f110_gym.envs.collision_models import get_vertices +from f110_gym.envs.rendering import RenderSpec + + +class TextObject: + def __init__( + self, + window_shape: tuple[int, int] = (1000, 1000), + relative_font_size: int = 32, + font_name: str = "Arial", + position: str | tuple = "bottom_right", + ): + 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 + ) -> 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 isinstance(position, tuple): + return position + + if isinstance(position, str): + position = position.lower() + if position == "bottom_right": + display_width, display_height = ( + display.get_width(), + display.get_height(), + ) + text_width, text_height = self.text.get_width(), self.text.get_height() + bottom_right = ( + display_width - text_width, + display_height - text_height, + ) + return bottom_right + elif position == "bottom_left": + display_height = display.get_height() + text_height = self.text.get_height() + bottom_left = (0, display_height - text_height) + return bottom_left + elif position == "bottom_center": + display_width, display_height = ( + display.get_width(), + display.get_height(), + ) + text_width, text_height = self.text.get_width(), self.text.get_height() + bottom_center = ( + (display_width - text_width) / 2, + display_height - text_height, + ) + return bottom_center + elif position == "top_right": + display_width = display.get_width() + text_width = self.text.get_width() + top_right = (display_width - text_width, 0) + return top_right + elif position == "top_left": + top_left = (0, 0) + return top_left + elif position == "top_center": + display_width = display.get_width() + text_width = self.text.get_width() + top_center = ((display_width - text_width) / 2, 0) + return top_center + else: + raise NotImplementedError(f"Position {position} not implemented.") + + def render(self, text: str, display: pygame.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) + + +class Map: + """ + Class to display the track map according to the desired zoom level. + """ + + def __init__(self, map_img: np.ndarray, zoom_level: float): + orig_width, orig_height = map_img.shape + scaled_width = int(orig_width * zoom_level) + scaled_height = int(orig_height * zoom_level) + map_img = cv2.resize( + map_img, dsize=(scaled_width, scaled_height), interpolation=cv2.INTER_AREA + ) + + # convert shape from (W, H) to (W, H, 3) + track_map = np.stack([map_img, map_img, map_img], axis=-1) + + # rotate and flip to match the track orientation + track_map = np.rot90(track_map, k=1) # rotate clockwise + track_map = np.flip(track_map, axis=0) # flip vertically + + self.track_map = track_map + self.map_surface = pygame.surfarray.make_surface(self.track_map) + + def render(self, display: pygame.Surface): + display.blit(self.map_surface, (0, 0)) + + +class Car: + """ + Class to display the car. + """ + + def __init__( + self, + render_spec: RenderSpec, + map_origin: tuple[float, float], + resolution: float, + ppu: float, + car_length: float, + car_width: float, + color: list[int] | None = None, + wheel_size: float = 0.2, + ): + self.car_length = car_length + self.car_width = car_width + self.wheel_size = wheel_size + self.car_tickness = render_spec.car_tickness + self.show_wheels = render_spec.show_wheels + + self.origin = map_origin + self.resolution = resolution + self.ppu = ppu + + self.color = color or (0, 0, 0) + self.pose = None + self.steering = None + self.rect = None + + def update(self, state: dict[str, np.ndarray], idx: int): + self.pose = ( + state["poses_x"][idx], + state["poses_y"][idx], + state["poses_theta"][idx], + ) + self.color = (255, 0, 0) if state["collisions"][idx] > 0 else self.color + self.steering = self.pose[2] + state["steering_angles"][idx] + + def render(self, display: pygame.Surface): + vertices = get_vertices(self.pose, self.car_length, self.car_width) + vertices[:, 0] = (vertices[:, 0] - self.origin[0]) / ( + self.resolution * self.ppu + ) + vertices[:, 1] = (vertices[:, 1] - self.origin[1]) / ( + self.resolution * self.ppu + ) + + self.rect = pygame.draw.polygon(display, self.color, vertices) + + pygame.draw.lines(display, (0, 0, 0), True, vertices, self.car_tickness) + + # draw two lines in proximity of the front wheels + # to indicate the steering angle + if self.show_wheels: + # percentage along the car length to draw the wheels segments + lam = 0.15 + + # find point at perc between front and back vertices + front_left = (vertices[0] * lam + vertices[3] * (1 - lam)).astype(int) + front_right = (vertices[1] * lam + vertices[2] * (1 - lam)).astype(int) + arrow_length = self.wheel_size / self.resolution + + for mid_point in [front_left, front_right]: + end_point = mid_point + 0.5 * arrow_length * np.array( + [np.cos(self.steering), np.sin(self.steering)] + ) + base_point = mid_point - 0.5 * arrow_length * np.array( + [np.cos(self.steering), np.sin(self.steering)] + ) + + pygame.draw.line( + display, + (0, 0, 0), + base_point.astype(int), + end_point.astype(int), + self.car_tickness + 1, + ) diff --git a/gym/f110_gym/envs/rendering/renderer.py b/gym/f110_gym/envs/rendering/renderer.py new file mode 100644 index 00000000..d20a1b93 --- /dev/null +++ b/gym/f110_gym/envs/rendering/renderer.py @@ -0,0 +1,85 @@ +from __future__ import annotations +import pathlib +from abc import abstractmethod +from dataclasses import dataclass +from typing import Union, Any + +import yaml + + +@dataclass +class RenderSpec: + window_size: int + zoom_in_factor: float + focus_on: str + car_tickness: int + show_wheels: bool + show_info: bool = True + vehicle_palette: list[str] = None + + def __init__( + self, + window_size: int = 800, + focus_on: str = None, + zoom_in_factor: float = 1.5, + car_tickness: int = 1, + show_wheels: bool = False, + show_info: bool = True, + vehicle_palette: list[str] = None, + ): + self.window_size = window_size + self.focus_on = focus_on + self.zoom_in_factor = zoom_in_factor + self.car_tickness = car_tickness + self.show_wheels = show_wheels + self.show_info = show_info + self.vehicle_palette = vehicle_palette or ["#984ea3"] + + @staticmethod + def from_yaml(yaml_file: Union[str, pathlib.Path]): + with open(yaml_file, "r") as yaml_stream: + try: + config = yaml.safe_load(yaml_stream) + except yaml.YAMLError as ex: + print(ex) + return RenderSpec(**config) + + +class EnvRenderer: + render_callbacks = [] + + @abstractmethod + def update(self, state): + """ + Update the state to be rendered. + This is called at every rendering call. + """ + raise NotImplementedError() + + def add_renderer_callback(self, callback_fn: callable): + """ + Add a callback function to be called at every rendering call. + This is called at the end of `update`. + """ + self.render_callbacks.append(callback_fn) + + @abstractmethod + def render_map(self): + """ + Render the current state in a frame. + """ + raise NotImplementedError() + + @abstractmethod + def render(self): + """ + Render the current state in a frame. + """ + raise NotImplementedError() + + @abstractmethod + def close(self): + """ + Close the rendering window. + """ + raise NotImplementedError() diff --git a/gym/f110_gym/envs/rendering/rendering.yaml b/gym/f110_gym/envs/rendering/rendering.yaml new file mode 100644 index 00000000..78e3ed69 --- /dev/null +++ b/gym/f110_gym/envs/rendering/rendering.yaml @@ -0,0 +1,15 @@ +window_size: 800 # [px] width and height of the window +focus_on: agent_0 # [str] agent id to focus on (e.g., agent_0) or null for map view +zoom_in_factor: 1.5 # [float] zoom in factor. 1.0 is no zoom, >1.0 is zoom in, <1.0 is zoom out +show_wheels: True # [bool] it toggles the visualization of the vehicle wheels +car_tickness: 1 # [px] thickness of the car +show_info: True # [bool] it toggles the visualization of the instruction text +vehicle_palette: # [list] cyclic list of colors for the vehicles + - "#984ea3" + - "#e41a1c" + - "#ff7f00" + - "#a65628" + - "#f781bf" + - "#888888" + - "#a6cee3" + - "#b2df8a" diff --git a/gym/f110_gym/envs/rendering/rendering_pygame.py b/gym/f110_gym/envs/rendering/rendering_pygame.py new file mode 100644 index 00000000..eb0ccb85 --- /dev/null +++ b/gym/f110_gym/envs/rendering/rendering_pygame.py @@ -0,0 +1,356 @@ +from __future__ import annotations + +import logging +import math +import pathlib +from typing import Union, List, Tuple, Any + +import cv2 +import numpy as np +import pygame +import yaml +from PIL import Image, ImageColor + +from f110_gym.envs.rendering.objects import ( + Map, + Car, + TextObject, +) +from f110_gym.envs.track import Track +from f110_gym.envs.rendering.renderer import EnvRenderer, RenderSpec + +# one-line instructions visualized at the top of the screen (if show_info=True) +INSTRUCTION_TEXT = "Mouse click (L/M/R): Change POV - 'S' key: On/Off" + + +class PygameEnvRenderer(EnvRenderer): + def __init__( + self, + params: dict[str, Any], + track: Track, + agent_ids: list[str], + render_spec: RenderSpec, + render_mode: str, + render_fps: int, + ): + super().__init__() + self.params = params # simulation params + self.agent_ids = agent_ids # list of agent ids + + self.cars = None + self.window = None + self.canvas = None + + self.render_spec = render_spec + self.render_mode = render_mode + self.render_fps = render_fps + + colors_rgb = [ + [rgb for rgb in ImageColor.getcolor(c, "RGB")] + for c in render_spec.vehicle_palette + ] + self.car_colors = [ + colors_rgb[i % len(colors_rgb)] for i in range(len(self.agent_ids)) + ] + + width, height = render_spec.window_size, render_spec.window_size + + pygame.init() + if self.render_mode in ["human", "human_fast"]: + pygame.display.init() + pygame.event.set_allowed([]) + self.window = pygame.display.set_mode((width, height)) + self.window.fill((255, 255, 255)) # white background + + self.canvas = pygame.Surface((width, height)) + + # load map metadata + map_filepath = pathlib.Path(track.filepath) + map_yaml = map_filepath.with_suffix(".yaml") + with open(map_yaml, "r") as yaml_stream: + try: + self.map_metadata = yaml.safe_load(yaml_stream) + except yaml.YAMLError as ex: + print(ex) + + # fps and time renderer + self.clock = pygame.time.Clock() + self.fps_renderer = TextObject( + window_shape=(width, height), position="bottom_left" + ) + self.time_renderer = TextObject( + window_shape=(width, height), position="bottom_right" + ) + self.bottom_info_renderer = TextObject( + window_shape=(width, height), position="bottom_center" + ) + self.top_info_renderer = TextObject( + window_shape=(width, height), position="top_center" + ) + + # load map image + original_img = map_filepath.parent / self.map_metadata["image"] + original_img = np.array( + Image.open(original_img).transpose(Image.FLIP_TOP_BOTTOM) + ).astype(np.float64) + + self.map_renderers = { + "map": Map(map_img=original_img, zoom_level=0.4), + "car": Map(map_img=original_img, zoom_level=render_spec.zoom_in_factor), + } + self.map_canvases = { + k: pygame.Surface((map_r.track_map.shape[0], map_r.track_map.shape[1])) + for k, map_r in self.map_renderers.items() + } + self.ppus = { + k: original_img.shape[0] / map_r.track_map.shape[0] + for k, map_r in self.map_renderers.items() + } + + # callbacks for custom visualization, called at every rendering step + self.callbacks = [] + + # event handling flags + self.draw_flag: bool = True + if render_spec.focus_on: + self.active_map_renderer = "car" + self.follow_agent_flag: bool = True + self.agent_to_follow: int = self.agent_ids.index(render_spec.focus_on) + else: + self.active_map_renderer = "map" + self.follow_agent_flag: bool = False + self.agent_to_follow: int = None + + def update(self, state): + """ + Update the simulation state to be rendered. + + Args: + state: simulation state as dictionary + """ + # initialize cars + if self.cars is None: + self.cars = [ + Car( + car_length=self.params["length"], + car_width=self.params["width"], + color=self.car_colors[ic], + render_spec=self.render_spec, + map_origin=self.map_metadata["origin"], + resolution=self.map_metadata["resolution"], + ppu=self.ppus[self.active_map_renderer], + ) + for ic in range(len(self.agent_ids)) + ] + + # update cars state and zoom level (updating points-per-unit) + for i in range(len(self.agent_ids)): + self.cars[i].update(state, i) + self.cars[i].ppu = self.ppus[self.active_map_renderer] + + # update time + self.sim_time = state["sim_time"] + + def add_renderer_callback(self, callback_fn: callable): + self.callbacks.append(callback_fn) + + def render(self): + self.event_handling() + + self.canvas.fill((255, 255, 255)) # white background + self.map_canvas = self.map_canvases[self.active_map_renderer] + self.map_canvas.fill((255, 255, 255)) # white background + + if self.draw_flag: + self.map_renderers[self.active_map_renderer].render(self.map_canvas) + + # draw cars + for i in range(len(self.agent_ids)): + self.cars[i].render(self.map_canvas) + + # call callbacks + for callback_fn in self.callbacks: + callback_fn(self) + + surface_mod_rect = self.map_canvas.get_rect() + screen_rect = self.canvas.get_rect() + + if self.follow_agent_flag: + origin = self.map_metadata["origin"] + resolution = ( + self.map_metadata["resolution"] + * self.ppus[self.active_map_renderer] + ) + ego_x, ego_y = self.cars[self.agent_to_follow].pose[:2] + cx = (ego_x - origin[0]) / resolution + cy = (ego_y - origin[1]) / resolution + else: + cx = self.map_canvas.get_width() / 2 + cy = self.map_canvas.get_height() / 2 + + surface_mod_rect.x = screen_rect.centerx - cx + surface_mod_rect.y = screen_rect.centery - cy + + self.canvas.blit(self.map_canvas, surface_mod_rect) + + agent_to_follow_id = ( + self.agent_ids[self.agent_to_follow] + if self.agent_to_follow is not None + else None + ) + self.bottom_info_renderer.render( + text=f"Focus on: {agent_to_follow_id}", display=self.canvas + ) + + if self.render_spec.show_info: + self.top_info_renderer.render(text=INSTRUCTION_TEXT, display=self.canvas) + self.time_renderer.render(text=f"{self.sim_time:.2f}", display=self.canvas) + + if self.render_mode in ["human", "human_fast"]: + assert self.window is not None + + self.fps_renderer.render( + text=f"FPS: {self.clock.get_fps():.2f}", display=self.canvas + ) + + self.window.blit(self.canvas, self.canvas.get_rect()) + + pygame.event.pump() + pygame.display.update() + + # We need to ensure that human-rendering occurs at the predefined framerate. + # The following line will automatically add a delay to keep the framerate stable. + self.clock.tick(self.render_fps) + else: # rgb_array + frame = np.transpose( + np.array(pygame.surfarray.pixels3d(self.canvas)), axes=(1, 0, 2) + ) + if frame.shape[0] > 2000: + frame = cv2.resize( + frame, dsize=(2000, 2000), interpolation=cv2.INTER_AREA + ) + return frame + + def event_handling(self): + """ + Handle interaction events to change point-of-view. + + Events: + - Left mouse button: follow next agent (according to agent_ids order) + - Right mouse button: follow previous agent + - Middle mouse button: change to map view + - S key: enable/disable rendering + """ + for event in pygame.event.get(): + if event.type == pygame.MOUSEBUTTONDOWN and event.button == 1: + logging.debug("Pressed left button -> Follow Next agent") + + self.follow_agent_flag = True + if self.agent_to_follow is None: + self.agent_to_follow = 0 + else: + self.agent_to_follow = (self.agent_to_follow + 1) % len( + self.agent_ids + ) + + self.active_map_renderer = "car" + + elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 3: + logging.debug("Pressed right button -> Follow Previous agent") + + self.follow_agent_flag = True + if self.agent_to_follow is None: + self.agent_to_follow = 0 + else: + self.agent_to_follow = (self.agent_to_follow - 1) % len( + self.agent_ids + ) + + self.active_map_renderer = "car" + + elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 2: + logging.debug("Pressed middle button -> Change to Map View") + + self.follow_agent_flag = False + self.agent_to_follow = None + + self.active_map_renderer = "map" + + elif event.type == pygame.KEYDOWN and event.key == pygame.K_s: + logging.debug("Pressed S key -> Enable/disable rendering") + self.draw_flag = not (self.draw_flag) + + def render_points( + self, + points: Union[List, np.ndarray], + color: Tuple[int, int, int] = (0, 0, 255), + size: int = 1, + ): + """ + Render a sequence of xy points on screen. + + Args: + points: sequence of xy points (N, 2) + color: rgb color of the points + size: size of the points in pixels + """ + origin = self.map_metadata["origin"] + ppu = self.ppus[self.active_map_renderer] + resolution = self.map_metadata["resolution"] * ppu + points = ((points - origin[:2]) / resolution).astype(int) + size = math.ceil(size / ppu) + + for point in points: + pygame.draw.circle(self.map_canvas, color, point, size) + + def render_lines( + self, + points: Union[List, np.ndarray], + color: Tuple[int, int, int] = (0, 0, 255), + size: int = 1, + ): + """ + Render a sequence of lines segments. + + Args: + points: sequence of xy points (N, 2) + color: rgb color of the points + size: size of the points in pixels + """ + origin = self.map_metadata["origin"] + ppu = self.ppus[self.active_map_renderer] + resolution = self.map_metadata["resolution"] * ppu + points = ((points - origin[:2]) / resolution).astype(int) + size = math.ceil(size / ppu) + + pygame.draw.lines( + self.map_canvas, color, closed=False, points=points, width=size + ) + + def render_closed_lines( + self, + points: Union[List, np.ndarray], + color: Tuple[int, int, int] = (0, 0, 255), + size: int = 1, + ): + """ + Render a sequence of lines segments. + + Args: + points: sequence of xy points (N, 2) + color: rgb color of the points + size: size of the points in pixels + """ + origin = self.map_metadata["origin"] + ppu = self.ppus[self.active_map_renderer] + resolution = self.map_metadata["resolution"] * ppu + points = ((points - origin[:2]) / resolution).astype(int) + size = math.ceil(size / ppu) + + pygame.draw.lines( + self.map_canvas, color, closed=True, points=points, width=size + ) + + def close(self): + if self.render_mode == "human" or self.render_mode == "human_fast": + pygame.quit() diff --git a/gym/f110_gym/test/benchmark_renderer.py b/gym/f110_gym/test/benchmark_renderer.py new file mode 100644 index 00000000..90a1634b --- /dev/null +++ b/gym/f110_gym/test/benchmark_renderer.py @@ -0,0 +1,218 @@ +import numpy as np + +from f110_gym.envs import F110Env +from f110_gym.envs.utils import deep_update + + +def pretty_print(dict: dict, col_width=15): + keys = list(dict.keys()) + columns = ["key"] + [str(k) for k in dict[keys[0]]] + + # opening line + for _ in columns: + print("|" + "-" * col_width, end="") + print("|") + # header + for col in columns: + padding = max(0, col_width - len(col)) + print("|" + col[:col_width] + " " * padding, end="") + print("|") + # separator line + for _ in columns: + print("|" + "-" * col_width, end="") + print("|") + + # table + for key in keys: + padding = max(0, col_width - len(str(key))) + print("|" + str(key)[:col_width] + " " * padding, end="") + for col in columns[1:]: + padding = max(0, col_width - len(str(dict[key][col]))) + print("|" + str(dict[key][col])[:col_width] + " " * padding, end="") + print("|") + + # footer + for col in columns: + print("|" + "-" * col_width, end="") + print("|") + + +class BenchmarkRenderer: + @staticmethod + def _make_env(config={}, render_mode=None) -> F110Env: + import gymnasium as gym + import f110_gym + + base_config = { + "map": "Spielberg", + "num_agents": 1, + "timestep": 0.01, + "integrator": "rk4", + "control_input": "speed", + "model": "st", + "observation_config": {"type": "kinematic_state"}, + "params": {"mu": 1.0}, + } + config = deep_update(base_config, config) + + env = gym.make("f110_gym:f110-v0", config=config, render_mode=render_mode,) + + return env + + def benchmark_single_agent_rendering(self): + import time + + sim_time = 15.0 # seconds + results = {} + + for render_mode in [None, "human", "human_fast", "rgb_array", "rgb_array_list"]: + env = self._make_env(render_mode=render_mode) + env.reset() + frame = env.render() + + print( + f"Running simulation of {sim_time}s for render mode: {render_mode}..." + ) + + max_steps = int(sim_time / env.timestep) + t0 = time.time() + for _ in range(max_steps): + action = env.action_space.sample() + env.step(action) + frame = env.render() + tf = time.time() + env.close() + + results[render_mode] = { + "sim_time": sim_time, + "elapsed_time": tf - t0, + "fps": max_steps / (tf - t0), + } + + pretty_print(results) + + def benchmark_n_agents_human_rendering(self): + """ + This is meant to benchmark the human rendering mode, for increasing nr of agents. + """ + import time + + sim_time = 15.0 # seconds + render_mode = "human" + + results = {} + + for num_agents in [1, 2, 3, 4, 5, 10]: + env = self._make_env( + config={"num_agents": num_agents}, render_mode=render_mode + ) + env.reset() + frame = env.render() + + print( + f"Running simulation of {num_agents} agents for render mode: {render_mode}..." + ) + + max_steps = int(sim_time / env.timestep) + t0 = time.time() + for _ in range(max_steps): + action = env.action_space.sample() + env.step(action) + frame = env.render() + tf = time.time() + env.close() + + results[num_agents] = { + "sim_time": sim_time, + "elapsed_time": tf - t0, + "fps": max_steps / (tf - t0), + } + + pretty_print(results) + + def benchmark_callbacks_human_rendering(self): + import time + + sim_time = 15.0 # seconds + render_mode = "human" + + results = {} + + class GoStraightPlanner: + def __init__(self, env, agent_id: str = "agent_0"): + self.waypoints = np.stack( + [env.track.raceline.xs, env.track.raceline.ys] + ).T + self.pos = None + self.agent_id = agent_id + + def plan(self, obs): + state = obs[self.agent_id] + self.pos = np.array([state["pose_x"], state["pose_y"]]) + return np.array([0.0, 2.5]) + + def render_waypoints(self, e): + e.render_closed_lines(points=self.waypoints, size=1) + + def render_position(self, e): + if self.pos is not None: + points = self.pos[None] + e.render_points(points, size=1) + + for render_config in [[False, False], [True, False], [True, True]]: + env = self._make_env(render_mode=render_mode) + planner = GoStraightPlanner(env) + + show_path, show_point = render_config + config_str = f"show_path={show_path}, show_point={show_point}" + + if show_path: + env.add_render_callback(callback_func=planner.render_waypoints) + + if show_point: + env.add_render_callback(callback_func=planner.render_position) + + rnd_idx = np.random.randint(0, len(env.track.raceline.xs)) + obs, _ = env.reset( + options={ + "poses": np.array( + [ + [ + env.track.raceline.xs[rnd_idx], + env.track.raceline.ys[rnd_idx], + env.track.raceline.yaws[rnd_idx], + ] + ] + ) + } + ) + frame = env.render() + + print( + f"Running simulation of {config_str} for render mode: {render_mode}..." + ) + + max_steps = int(sim_time / env.timestep) + t0 = time.time() + for _ in range(max_steps): + action = planner.plan(obs=obs) + obs, _, _, _, _ = env.step(np.array([action])) + frame = env.render() + tf = time.time() + env.close() + + results[config_str] = { + "sim_time": sim_time, + "elapsed_time": tf - t0, + "fps": max_steps / (tf - t0), + } + + pretty_print(results) + + +if __name__ == "__main__": + benchmark = BenchmarkRenderer() + + benchmark.benchmark_single_agent_rendering() + benchmark.benchmark_n_agents_human_rendering() + benchmark.benchmark_callbacks_human_rendering() diff --git a/gym/f110_gym/test/test_renderer.py b/gym/f110_gym/test/test_renderer.py new file mode 100644 index 00000000..f1645608 --- /dev/null +++ b/gym/f110_gym/test/test_renderer.py @@ -0,0 +1,89 @@ +import unittest + +import numpy as np + +from f110_gym.envs import F110Env +from f110_gym.envs.utils import deep_update + + +class TestRenderer(unittest.TestCase): + @staticmethod + def _make_env(config={}, render_mode=None) -> F110Env: + import gymnasium as gym + import f110_gym + + base_config = { + "map": "Spielberg", + "num_agents": 1, + "timestep": 0.01, + "integrator": "rk4", + "control_input": "speed", + "model": "st", + "observation_config": {"type": "kinematic_state"}, + "params": {"mu": 1.0}, + } + config = deep_update(base_config, config) + + env = gym.make("f110_gym:f110-v0", config=config, render_mode=render_mode,) + + return env + + def test_human_render(self): + env = self._make_env(render_mode="human") + env.reset() + for _ in range(100): + action = env.action_space.sample() + env.step(action) + env.render() + env.close() + + self.assertTrue(True, "Human render test failed") + + def test_rgb_array_render(self): + env = self._make_env(render_mode="rgb_array") + env.reset() + for _ in range(100): + action = env.action_space.sample() + env.step(action) + frame = env.render() + + self.assertTrue(isinstance(frame, np.ndarray), "Frame is not a numpy array") + self.assertTrue(len(frame.shape) == 3, "Frame is not a 3D array") + self.assertTrue(frame.shape[2] == 3, "Frame does not have 3 channels") + + env.close() + + self.assertTrue(True, "rgb_array render test failed") + + def test_rgb_array_list(self): + env = self._make_env(render_mode="rgb_array_list") + env.reset() + + steps = 100 + for _ in range(steps): + action = env.action_space.sample() + env.step(action) + + frame_list = env.render() + + self.assertTrue( + isinstance(frame_list, list), "the returned object is not a list of frames" + ) + self.assertTrue( + len(frame_list) == steps + 1, + "the returned list does not have the correct number of frames", + ) + self.assertTrue( + all([isinstance(frame, np.ndarray) for frame in frame_list]), + "not all frames are numpy arrays", + ) + self.assertTrue( + all([len(frame.shape) == 3 for frame in frame_list]), + "not all frames are 3D arrays", + ) + self.assertTrue( + all([frame.shape[2] == 3 for frame in frame_list]), + "not all frames have 3 channels", + ) + + env.close() diff --git a/gym/f110_gym/test/test_scan_sim.py b/gym/f110_gym/test/test_scan_sim.py new file mode 100644 index 00000000..e69de29b diff --git a/setup.py b/setup.py index 8d7de462..778b4d5e 100644 --- a/setup.py +++ b/setup.py @@ -14,8 +14,8 @@ "scipy>=1.7.3", "numba>=0.55.2", "pyyaml>=5.3.1", - "pyglet<1.5", - "pyopengl", + "pygame", + "opencv-python", "yamldataclassconfig", "requests", "shapely", diff --git a/tests/test_f110_env.py b/tests/test_f110_env.py index c4ba7fa7..faccb97e 100644 --- a/tests/test_f110_env.py +++ b/tests/test_f110_env.py @@ -18,10 +18,7 @@ def _make_env(self, config={}): } conf = deep_update(conf, config) - env = gym.make( - "f110_gym:f110-v0", - config=conf, - ) + env = gym.make("f110_gym:f110-v0", config=conf,) return env def test_gymnasium_api(self):