Enhancements to parking environment

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()

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:

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:
  """

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,

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
 

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)