Source code for highway_env.envs.intersection_env

from __future__ import annotations

import numpy as np

from highway_env import utils
from highway_env.envs.common.abstract import AbstractEnv
from highway_env.road.lane import AbstractLane, CircularLane, LineType, StraightLane
from highway_env.road.regulation import RegulatedRoad
from highway_env.road.road import RoadNetwork
from highway_env.vehicle.kinematics import Vehicle


[docs] class IntersectionEnv(AbstractEnv): ACTIONS: dict[int, str] = {0: "SLOWER", 1: "IDLE", 2: "FASTER"} ACTIONS_INDEXES = {v: k for k, v in ACTIONS.items()}
[docs] @classmethod def default_config(cls) -> dict: config = super().default_config() config.update( { "observation": { "type": "Kinematics", "vehicles_count": 15, "features": ["presence", "x", "y", "vx", "vy", "cos_h", "sin_h"], "features_range": { "x": [-100, 100], "y": [-100, 100], "vx": [-20, 20], "vy": [-20, 20], }, "absolute": True, "flatten": False, "observe_intentions": False, }, "action": { "type": "DiscreteMetaAction", "longitudinal": True, "lateral": False, "target_speeds": [0, 4.5, 9], }, "duration": 13, # [s] "destination": "o1", "controlled_vehicles": 1, "initial_vehicle_count": 10, "spawn_probability": 0.6, "screen_width": 600, "screen_height": 600, "centering_position": [0.5, 0.6], "scaling": 5.5 * 1.3, "collision_reward": -5, "high_speed_reward": 1, "arrived_reward": 1, "reward_speed_range": [7.0, 9.0], "normalize_reward": False, "offroad_terminal": False, } ) return config
def _reward(self, action: int) -> float: """Aggregated reward, for cooperative agents.""" return sum( self._agent_reward(action, vehicle) for vehicle in self.controlled_vehicles ) / len(self.controlled_vehicles) def _rewards(self, action: int) -> dict[str, float]: """Multi-objective rewards, for cooperative agents.""" agents_rewards = [ self._agent_rewards(action, vehicle) for vehicle in self.controlled_vehicles ] return { name: sum(agent_rewards[name] for agent_rewards in agents_rewards) / len(agents_rewards) for name in agents_rewards[0].keys() } def _agent_reward(self, action: int, vehicle: Vehicle) -> float: """Per-agent reward signal.""" rewards = self._agent_rewards(action, vehicle) reward = sum( self.config.get(name, 0) * reward for name, reward in rewards.items() ) reward = self.config["arrived_reward"] if rewards["arrived_reward"] else reward reward *= rewards["on_road_reward"] if self.config["normalize_reward"]: reward = utils.lmap( reward, [self.config["collision_reward"], self.config["arrived_reward"]], [0, 1], ) return reward def _agent_rewards(self, action: int, vehicle: Vehicle) -> dict[str, float]: """Per-agent per-objective reward signal.""" scaled_speed = utils.lmap( vehicle.speed, self.config["reward_speed_range"], [0, 1] ) return { "collision_reward": vehicle.crashed, "high_speed_reward": np.clip(scaled_speed, 0, 1), "arrived_reward": self.has_arrived(vehicle), "on_road_reward": vehicle.on_road, } def _is_terminated(self) -> bool: return ( any(vehicle.crashed for vehicle in self.controlled_vehicles) or all(self.has_arrived(vehicle) for vehicle in self.controlled_vehicles) or (self.config["offroad_terminal"] and not self.vehicle.on_road) ) def _agent_is_terminal(self, vehicle: Vehicle) -> bool: """The episode is over when a collision occurs or when the access ramp has been passed.""" return vehicle.crashed or self.has_arrived(vehicle) def _is_truncated(self) -> bool: """The episode is truncated if the time limit is reached.""" return self.time >= self.config["duration"] def _info(self, obs: np.ndarray, action: int) -> dict: info = super()._info(obs, action) info["agents_rewards"] = tuple( self._agent_reward(action, vehicle) for vehicle in self.controlled_vehicles ) info["agents_terminated"] = tuple( self._agent_is_terminal(vehicle) for vehicle in self.controlled_vehicles ) return info def _reset(self) -> None: self._make_road() self._make_vehicles(self.config["initial_vehicle_count"])
[docs] def step(self, action: int) -> tuple[np.ndarray, float, bool, bool, dict]: obs, reward, terminated, truncated, info = super().step(action) self._clear_vehicles() self._spawn_vehicle(spawn_probability=self.config["spawn_probability"]) return obs, reward, terminated, truncated, info
def _make_road(self) -> None: """ Make an 4-way intersection. The horizontal road has the right of way. More precisely, the levels of priority are: - 3 for horizontal straight lanes and right-turns - 1 for vertical straight lanes and right-turns - 2 for horizontal left-turns - 0 for vertical left-turns The code for nodes in the road network is: (o:outer | i:inner + [r:right, l:left]) + (0:south | 1:west | 2:north | 3:east) :return: the intersection road """ lane_width = AbstractLane.DEFAULT_WIDTH right_turn_radius = lane_width + 5 # [m} left_turn_radius = right_turn_radius + lane_width # [m} outer_distance = right_turn_radius + lane_width / 2 access_length = 50 + 50 # [m] net = RoadNetwork() n, c, s = LineType.NONE, LineType.CONTINUOUS, LineType.STRIPED for corner in range(4): angle = np.radians(90 * corner) is_horizontal = corner % 2 priority = 3 if is_horizontal else 1 rotation = np.array( [[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]] ) # Incoming start = rotation @ np.array( [lane_width / 2, access_length + outer_distance] ) end = rotation @ np.array([lane_width / 2, outer_distance]) net.add_lane( "o" + str(corner), "ir" + str(corner), StraightLane( start, end, line_types=[s, c], priority=priority, speed_limit=10 ), ) # Right turn r_center = rotation @ (np.array([outer_distance, outer_distance])) net.add_lane( "ir" + str(corner), "il" + str((corner - 1) % 4), CircularLane( r_center, right_turn_radius, angle + np.radians(180), angle + np.radians(270), line_types=[n, c], priority=priority, speed_limit=10, ), ) # Left turn l_center = rotation @ ( np.array( [ -left_turn_radius + lane_width / 2, left_turn_radius - lane_width / 2, ] ) ) net.add_lane( "ir" + str(corner), "il" + str((corner + 1) % 4), CircularLane( l_center, left_turn_radius, angle + np.radians(0), angle + np.radians(-90), clockwise=False, line_types=[n, n], priority=priority - 1, speed_limit=10, ), ) # Straight start = rotation @ np.array([lane_width / 2, outer_distance]) end = rotation @ np.array([lane_width / 2, -outer_distance]) net.add_lane( "ir" + str(corner), "il" + str((corner + 2) % 4), StraightLane( start, end, line_types=[s, n], priority=priority, speed_limit=10 ), ) # Exit start = rotation @ np.flip( [lane_width / 2, access_length + outer_distance], axis=0 ) end = rotation @ np.flip([lane_width / 2, outer_distance], axis=0) net.add_lane( "il" + str((corner - 1) % 4), "o" + str((corner - 1) % 4), StraightLane( end, start, line_types=[n, c], priority=priority, speed_limit=10 ), ) road = RegulatedRoad( network=net, np_random=self.np_random, record_history=self.config["show_trajectories"], ) self.road = road def _make_vehicles(self, n_vehicles: int = 10) -> None: """ Populate a road with several vehicles on the highway and on the merging lane :return: the ego-vehicle """ # Configure vehicles vehicle_type = utils.class_from_path(self.config["other_vehicles_type"]) vehicle_type.DISTANCE_WANTED = 7 # Low jam distance vehicle_type.COMFORT_ACC_MAX = 6 vehicle_type.COMFORT_ACC_MIN = -3 # Random vehicles simulation_steps = 3 for t in range(n_vehicles - 1): self._spawn_vehicle(np.linspace(0, 80, n_vehicles)[t]) for _ in range(simulation_steps): [ ( self.road.act(), self.road.step(1 / self.config["simulation_frequency"]), ) for _ in range(self.config["simulation_frequency"]) ] # Challenger vehicle self._spawn_vehicle( 60, spawn_probability=1, go_straight=True, position_deviation=0.1, speed_deviation=0, ) # Controlled vehicles self.controlled_vehicles = [] for ego_id in range(0, self.config["controlled_vehicles"]): ego_lane = self.road.network.get_lane( (f"o{ego_id % 4}", f"ir{ego_id % 4}", 0) ) destination = self.config["destination"] or "o" + str( self.np_random.integers(1, 4) ) ego_vehicle = self.action_type.vehicle_class( self.road, ego_lane.position(60 + 5 * self.np_random.normal(1), 0), speed=ego_lane.speed_limit, heading=ego_lane.heading_at(60), ) try: ego_vehicle.plan_route_to(destination) ego_vehicle.speed_index = ego_vehicle.speed_to_index( ego_lane.speed_limit ) ego_vehicle.target_speed = ego_vehicle.index_to_speed( ego_vehicle.speed_index ) except AttributeError: pass self.road.vehicles.append(ego_vehicle) self.controlled_vehicles.append(ego_vehicle) for v in self.road.vehicles: # Prevent early collisions if ( v is not ego_vehicle and np.linalg.norm(v.position - ego_vehicle.position) < 20 ): self.road.vehicles.remove(v) def _spawn_vehicle( self, longitudinal: float = 0, position_deviation: float = 1.0, speed_deviation: float = 1.0, spawn_probability: float = 0.6, go_straight: bool = False, ) -> None: if self.np_random.uniform() > spawn_probability: return route = self.np_random.choice(range(4), size=2, replace=False) route[1] = (route[0] + 2) % 4 if go_straight else route[1] vehicle_type = utils.class_from_path(self.config["other_vehicles_type"]) vehicle = vehicle_type.make_on_lane( self.road, ("o" + str(route[0]), "ir" + str(route[0]), 0), longitudinal=( longitudinal + 5 + self.np_random.normal() * position_deviation ), speed=8 + self.np_random.normal() * speed_deviation, ) for v in self.road.vehicles: if np.linalg.norm(v.position - vehicle.position) < 15: return vehicle.plan_route_to("o" + str(route[1])) vehicle.randomize_behavior() self.road.vehicles.append(vehicle) return vehicle def _clear_vehicles(self) -> None: is_leaving = ( lambda vehicle: "il" in vehicle.lane_index[0] and "o" in vehicle.lane_index[1] and vehicle.lane.local_coordinates(vehicle.position)[0] >= vehicle.lane.length - 4 * vehicle.LENGTH ) self.road.vehicles = [ vehicle for vehicle in self.road.vehicles if vehicle in self.controlled_vehicles or not (is_leaving(vehicle) or vehicle.route is None) ] def has_arrived(self, vehicle: Vehicle, exit_distance: float = 25) -> bool: return ( "il" in vehicle.lane_index[0] and "o" in vehicle.lane_index[1] and vehicle.lane.local_coordinates(vehicle.position)[0] >= exit_distance )
class MultiAgentIntersectionEnv(IntersectionEnv): @classmethod def default_config(cls) -> dict: config = super().default_config() config.update( { "action": { "type": "MultiAgentAction", "action_config": { "type": "DiscreteMetaAction", "lateral": False, "longitudinal": True, }, }, "observation": { "type": "MultiAgentObservation", "observation_config": {"type": "Kinematics"}, }, "controlled_vehicles": 2, } ) return config class ContinuousIntersectionEnv(IntersectionEnv): @classmethod def default_config(cls) -> dict: config = super().default_config() config.update( { "observation": { "type": "Kinematics", "vehicles_count": 5, "features": [ "presence", "x", "y", "vx", "vy", "long_off", "lat_off", "ang_off", ], }, "action": { "type": "ContinuousAction", "steering_range": [-np.pi / 3, np.pi / 3], "longitudinal": True, "lateral": True, "dynamical": True, }, } ) return config