diff --git a/highway_env/envs/common/abstract.py b/highway_env/envs/common/abstract.py index f60ffba52..da29bb22e 100644 --- a/highway_env/envs/common/abstract.py +++ b/highway_env/envs/common/abstract.py @@ -106,7 +106,8 @@ def default_config(cls) -> dict: "render_agent": True, "offscreen_rendering": os.environ.get("OFFSCREEN_RENDERING", "0") == "1", "manual_control": False, - "real_time_rendering": False + "real_time_rendering": False, + "center_window": True } def configure(self, config: dict) -> None: @@ -284,7 +285,7 @@ def render(self) -> Optional[np.ndarray]: self.enable_auto_render = True - self.viewer.display() + self.viewer.display(self.config["center_window"]) if not self.viewer.offscreen: self.viewer.handle_events() diff --git a/highway_env/envs/common/graphics.py b/highway_env/envs/common/graphics.py index 7c4e34570..d5e99daab 100644 --- a/highway_env/envs/common/graphics.py +++ b/highway_env/envs/common/graphics.py @@ -94,12 +94,12 @@ def handle_events(self) -> None: if self.env.action_type: EventHandler.handle_event(self.env.action_type, event) - def display(self) -> None: + def display(self, center=True) -> None: """Display the road and vehicles on a pygame window.""" if not self.enabled: return - self.sim_surface.move_display_window_to(self.window_position()) + self.sim_surface.move_display_window_to(self.window_position() if center else [0.5, 0.5]) RoadGraphics.display(self.env.road, self.sim_surface) if self.vehicle_trajectory: diff --git a/highway_env/envs/parking_env.py b/highway_env/envs/parking_env.py index 54ba79032..61c7d506a 100644 --- a/highway_env/envs/parking_env.py +++ b/highway_env/envs/parking_env.py @@ -3,6 +3,7 @@ from gymnasium import Env import numpy as np +import random from highway_env.envs.common.abstract import AbstractEnv from highway_env.envs.common.observation import MultiAgentObservation, observation_factory @@ -11,6 +12,7 @@ from highway_env.vehicle.graphics import VehicleGraphics from highway_env.vehicle.kinematics import Vehicle from highway_env.vehicle.objects import Landmark, Obstacle +from highway_env.utils import are_polygons_intersecting class GoalEnv(Env): @@ -100,7 +102,15 @@ def default_config(cls) -> dict: "scaling": 7, "controlled_vehicles": 1, "vehicles_count": 0, - "add_walls": True + "add_walls": True, + "center_window": False, + "n_rows": 2, # number of parking rows + "y_offset": 10, # y distance between parallel rows + "font_size": 22, # display font size for ids ['None' to stop render] + "spots": 14, # spots in each row + "obstacles": False, + "random_start": False, + "prevent_early_collision": False }) return config @@ -131,60 +141,152 @@ def _create_road(self, spots: int = 14) -> None: :param spots: number of spots in the parking """ + spots = self.config["spots"] net = RoadNetwork() width = 4.0 lt = (LineType.CONTINUOUS, LineType.CONTINUOUS) x_offset = 0 - y_offset = 10 + y_offset = self.config["y_offset"] length = 8 - for k in range(spots): - x = (k + 1 - spots // 2) * (width + x_offset) - width / 2 - net.add_lane("a", "b", StraightLane([x, y_offset], [x, y_offset+length], width=width, line_types=lt)) - net.add_lane("b", "c", StraightLane([x, -y_offset], [x, -y_offset-length], width=width, line_types=lt)) + + row_ys = [] + for i in range(self.config["n_rows"]): + if i == 0: + row_ys.append([y_offset, y_offset+length]) + elif i == 1: + row_ys.append([-y_offset, -y_offset-length]) + else: + if i%2 == 0: + row_ys.append([row_ys[i-2][1]+2*y_offset, row_ys[i-2][1]+2*y_offset+length]) + else: + row_ys.append([row_ys[i-2][1]-2*y_offset, row_ys[i-2][1]-2*y_offset-length]) + + id = 0 + for row in range(0, self.config["n_rows"]): + for k in range(spots): + x = (k + 1 - spots // 2) * (width + x_offset) - width / 2 + net.add_lane( + chr(ord('a')+row), chr(ord('a')+row+1), + StraightLane([x, row_ys[row][0]], [x, row_ys[row][1]], + width=width, line_types=lt, identifier=id, + display_font_size=self.config['font_size'] + ) + ) + id += 1 self.road = Road(network=net, np_random=self.np_random, record_history=self.config["show_trajectories"]) + + # Store the allowed x,y coordinate ranges to spawn the agent + allowed_y_space = [[-y_offset+8, y_offset-8]] + if len(row_ys) > 2: + for i in range(2, len(row_ys)): + if i%2 == 0: + allowed_y_space.append([row_ys[i-2][1]+8, row_ys[i][0]-8]) + else: + allowed_y_space.append([row_ys[i][0]+8, row_ys[i-2][1]-8]) + + self.allowed_vehicle_space = { + 'x' : (-30, 30), + 'y' : allowed_y_space + } + + # Walls + x_end = abs((1 - spots // 2) * (width + x_offset) - width / 2) + + if len(row_ys) > 1: + wall_y_top = row_ys[-1][1] + 4 if row_ys[-1][1] > 0 else row_ys[-1][1] - 4 + wall_y_bottom = row_ys[-2][1] + 4 if row_ys[-2][1] > 0 else row_ys[-2][1] - 4 + else: + wall_y_bottom = row_ys[-1][1] + 4 + wall_y_top = -y_offset - 4 + + wall_x = x_end + 14 + + for y in [wall_y_top, wall_y_bottom]: + obstacle = Obstacle(self.road, [0, y]) + obstacle.LENGTH, obstacle.WIDTH = (2*wall_x, 1) + obstacle.diagonal = np.sqrt(obstacle.LENGTH**2 + obstacle.WIDTH**2) + self.road.objects.append(obstacle) + + wall_y = 0 + if self.config["n_rows"] > 1 and self.config["n_rows"]%2==1: wall_y = y_offset+4 + elif self.config["n_rows"] == 1: wall_y = y_offset-6 + + for x in [-wall_x, wall_x]: + obstacle = Obstacle( + self.road, + [x, wall_y], + heading=np.pi / 2 + ) + obstacle.LENGTH, obstacle.WIDTH = (abs(wall_y_top) + abs(wall_y_bottom), 1) + obstacle.diagonal = np.sqrt(obstacle.LENGTH**2 + obstacle.WIDTH**2) + self.road.objects.append(obstacle) + + if self.config['obstacles'] and self.config["n_rows"] > 3: + self._create_obstacles() + + def _create_obstacles(self): + """Create some random obstacles""" + obstacle = Obstacle(self.road, (18, -18 - self.config['y_offset']), 90) + obstacle.LENGTH, obstacle.WIDTH = 5, 5 + obstacle.diagonal = np.sqrt(obstacle.LENGTH**2 + obstacle.WIDTH**2) + obstacle.line_color = (187, 84, 49) + self.road.objects.append(obstacle) + + obstacle = Obstacle(self.road, (-18, 18 + self.config['y_offset']), 90) + obstacle.LENGTH, obstacle.WIDTH = 5, 5 + obstacle.diagonal = np.sqrt(obstacle.LENGTH**2 + obstacle.WIDTH**2) + obstacle.line_color = (187, 84, 49) + self.road.objects.append(obstacle) def _create_vehicles(self) -> None: """Create some new random vehicles of a given type, and add them on the road.""" # Controlled vehicles self.controlled_vehicles = [] for i in range(self.config["controlled_vehicles"]): - vehicle = self.action_type.vehicle_class(self.road, [i*20, 0], 2*np.pi*self.np_random.uniform(), 0) + if self.config["random_start"]: + while(True): + x = np.random.randint(self.allowed_vehicle_space['x'][0], self.allowed_vehicle_space['x'][1]) + y_sector = np.random.choice(range(len(self.allowed_vehicle_space['y']))) + y = np.random.randint(self.allowed_vehicle_space['y'][y_sector][0], self.allowed_vehicle_space['y'][y_sector][1]) + vehicle = self.action_type.vehicle_class(self.road, [x, y], 2*np.pi*self.np_random.uniform(), 0) + + intersect = False + for o in self.road.objects: + res, _, _ = are_polygons_intersecting(vehicle.polygon(), o.polygon(), vehicle.velocity, o.velocity) + intersect |= res + if not intersect: + break + else: + vehicle = self.action_type.vehicle_class(self.road, [0, 0], 2*np.pi*self.np_random.uniform(), 0) + vehicle.color = VehicleGraphics.EGO_COLOR self.road.vehicles.append(vehicle) self.controlled_vehicles.append(vehicle) # Goal - lane = self.np_random.choice(self.road.network.lanes_list()) - self.goal = Landmark(self.road, lane.position(lane.length/2, 0), heading=lane.heading) + goal_lane = self.np_random.choice(self.road.network.lanes_list()) + self.goal = Landmark(self.road, goal_lane.position(goal_lane.length/2, 0), heading=goal_lane.heading) self.road.objects.append(self.goal) # Other vehicles - for i in range(self.config["vehicles_count"]): - lane = ("a", "b", i) if self.np_random.uniform() >= 0.5 else ("b", "c", i) + free_lanes = self.road.network.lanes_list().copy() + free_lanes.remove(goal_lane) + random.Random(4).shuffle(free_lanes) + for _ in range(self.config["vehicles_count"] - 1): + lane = free_lanes.pop() v = Vehicle.make_on_lane(self.road, lane, 4, speed=0) self.road.vehicles.append(v) - for v in self.road.vehicles: # Prevent early collisions - if v is not self.vehicle and ( - np.linalg.norm(v.position - self.goal.position) < 20 or - np.linalg.norm(v.position - self.vehicle.position) < 20): - self.road.vehicles.remove(v) - # Walls - if self.config['add_walls']: - width, height = 70, 42 - for y in [-height / 2, height / 2]: - obstacle = Obstacle(self.road, [0, y]) - obstacle.LENGTH, obstacle.WIDTH = (width, 1) - obstacle.diagonal = np.sqrt(obstacle.LENGTH**2 + obstacle.WIDTH**2) - self.road.objects.append(obstacle) - for x in [-width / 2, width / 2]: - obstacle = Obstacle(self.road, [x, 0], heading=np.pi / 2) - obstacle.LENGTH, obstacle.WIDTH = (height, 1) - obstacle.diagonal = np.sqrt(obstacle.LENGTH**2 + obstacle.WIDTH**2) - self.road.objects.append(obstacle) + if self.config["prevent_early_collision"]: + for v in self.road.vehicles: # Prevent early collisions + if v is not self.vehicle and ( + np.linalg.norm(v.position - self.goal.position) < 20 or + np.linalg.norm(v.position - self.vehicle.position) < 20): + self.road.vehicles.remove(v) + def compute_reward(self, achieved_goal: np.ndarray, desired_goal: np.ndarray, info: dict, p: float = 0.5) -> float: """ diff --git a/highway_env/road/graphics.py b/highway_env/road/graphics.py index 23dd3766c..1bd6970c7 100644 --- a/highway_env/road/graphics.py +++ b/highway_env/road/graphics.py @@ -125,14 +125,23 @@ def display(cls, lane: AbstractLane, surface: WorldSurface) -> None: stripes_count = int(2 * (surface.get_height() + surface.get_width()) / (cls.STRIPE_SPACING * surface.scaling)) s_origin, _ = lane.local_coordinates(surface.origin) s0 = (int(s_origin) // cls.STRIPE_SPACING - stripes_count // 2) * cls.STRIPE_SPACING + + last_pixel = [] # Store the pixel coordinates of the last pixel of a lane for side in range(2): if lane.line_types[side] == LineType.STRIPED: cls.striped_line(lane, surface, stripes_count, s0, side) elif lane.line_types[side] == LineType.CONTINUOUS: - cls.continuous_curve(lane, surface, stripes_count, s0, side) + last_pixel.append(cls.continuous_curve(lane, surface, stripes_count, s0, side, return_last_pixel=True)) elif lane.line_types[side] == LineType.CONTINUOUS_LINE: cls.continuous_line(lane, surface, stripes_count, s0, side) + if lane.display_font_size != None: + x = (last_pixel[0][0] + last_pixel[1][0]) // 2 - 8 + y = last_pixel[0][1] + font1 = pygame.font.SysFont('arial', lane.display_font_size) + text_surface = font1.render(str(lane.identifier), True, (0, 0, 0)) + surface.blit(text_surface, (x,y)) + @classmethod def striped_line(cls, lane: AbstractLane, surface: WorldSurface, stripes_count: int, longitudinal: float, side: int) -> None: @@ -152,7 +161,7 @@ def striped_line(cls, lane: AbstractLane, surface: WorldSurface, stripes_count: @classmethod def continuous_curve(cls, lane: AbstractLane, surface: WorldSurface, stripes_count: int, - longitudinal: float, side: int) -> None: + longitudinal: float, side: int, return_last_pixel: bool = False) -> None: """ Draw a striped line on one side of a lane, on a surface. @@ -161,11 +170,12 @@ def continuous_curve(cls, lane: AbstractLane, surface: WorldSurface, stripes_cou :param stripes_count: the number of stripes to draw :param longitudinal: the longitudinal position of the first stripe [m] :param side: which side of the road to draw [0:left, 1:right] + :param return_last_pixel: returns the last pixel coordinate of line """ starts = longitudinal + np.arange(stripes_count) * cls.STRIPE_SPACING ends = longitudinal + np.arange(stripes_count) * cls.STRIPE_SPACING + cls.STRIPE_SPACING lats = [(side - 0.5) * lane.width_at(s) for s in starts] - cls.draw_stripes(lane, surface, starts, ends, lats) + return cls.draw_stripes(lane, surface, starts, ends, lats, return_last_pixel) @classmethod def continuous_line(cls, lane: AbstractLane, surface: WorldSurface, stripes_count: int, longitudinal: float, @@ -186,7 +196,8 @@ def continuous_line(cls, lane: AbstractLane, surface: WorldSurface, stripes_coun @classmethod def draw_stripes(cls, lane: AbstractLane, surface: WorldSurface, - starts: List[float], ends: List[float], lats: List[float]) -> None: + starts: List[float], ends: List[float], lats: List[float], + return_last_pixel: bool = False) -> None: """ Draw a set of stripes along a lane. @@ -195,6 +206,7 @@ def draw_stripes(cls, lane: AbstractLane, surface: WorldSurface, :param starts: a list of starting longitudinal positions for each stripe [m] :param ends: a list of ending longitudinal positions for each stripe [m] :param lats: a list of lateral positions for each stripe [m] + :param return_last_pixel: returns the last pixel coordinate of line """ starts = np.clip(starts, 0, lane.length) ends = np.clip(ends, 0, lane.length) @@ -204,6 +216,10 @@ def draw_stripes(cls, lane: AbstractLane, surface: WorldSurface, (surface.vec2pix(lane.position(starts[k], lats[k]))), (surface.vec2pix(lane.position(ends[k], lats[k]))), max(surface.pix(cls.STRIPE_WIDTH), 1)) + last_coordinate = (surface.vec2pix(lane.position(ends[k], lats[k]))) + + if return_last_pixel: + return last_coordinate @classmethod def draw_ground(cls, lane: AbstractLane, surface: WorldSurface, color: Tuple[float], width: float, diff --git a/highway_env/road/lane.py b/highway_env/road/lane.py index 87d302d33..922278289 100644 --- a/highway_env/road/lane.py +++ b/highway_env/road/lane.py @@ -152,7 +152,9 @@ def __init__(self, line_types: Tuple[LineType, LineType] = None, forbidden: bool = False, speed_limit: float = 20, - priority: int = 0) -> None: + priority: int = 0, + identifier: int = None, + display_font_size: int = 15) -> None: """ New straight lane. @@ -162,6 +164,8 @@ def __init__(self, :param line_types: the type of lines on both sides of the lane :param forbidden: is changing to this lane forbidden :param priority: priority level of the lane, for determining who has right of way + :param identifier: sequential id assigned to each lane + :param display_font_size: priority level of the lane, for determining who has right of way """ self.start = np.array(start) self.end = np.array(end) @@ -175,6 +179,9 @@ def __init__(self, self.priority = priority self.speed_limit = speed_limit + self.identifier = identifier + self.display_font_size = display_font_size # Font size to display the lane counter + def position(self, longitudinal: float, lateral: float) -> np.ndarray: return self.start + longitudinal * self.direction + lateral * self.direction_lateral diff --git a/highway_env/vehicle/objects.py b/highway_env/vehicle/objects.py index 1eba65201..bda417a8d 100644 --- a/highway_env/vehicle/objects.py +++ b/highway_env/vehicle/objects.py @@ -1,5 +1,5 @@ from abc import ABC -from typing import Sequence, Tuple, TYPE_CHECKING, Optional +from typing import Sequence, Tuple, TYPE_CHECKING, Optional, Union import numpy as np from highway_env import utils @@ -51,8 +51,11 @@ def __init__(self, road: 'Road', position: Sequence[float], heading: float = 0, self.hit = False self.impact = np.zeros(self.position.shape) + # Option to set line color of the object manually + self.line_color = None + @classmethod - def make_on_lane(cls, road: 'Road', lane_index: LaneIndex, longitudinal: float, speed: Optional[float] = None) \ + def make_on_lane(cls, road: 'Road', lane: Union['AbstractLane', LaneIndex], longitudinal: float, speed: Optional[float] = None) \ -> 'RoadObject': """ Create a vehicle on a given lane at a longitudinal position. @@ -63,7 +66,8 @@ def make_on_lane(cls, road: 'Road', lane_index: LaneIndex, longitudinal: float, :param speed: initial speed in [m/s] :return: a RoadObject at the specified position """ - lane = road.network.get_lane(lane_index) + if type(lane) is LaneIndex: + lane = road.network.get_lane(lane) if speed is None: speed = lane.speed_limit return cls(road, lane.position(longitudinal, 0), lane.heading_at(longitudinal), speed)