Source code for highway_env.vehicle.controller

import copy
from typing import List, Optional, Tuple, Union

import numpy as np

from highway_env import utils
from highway_env.road.road import LaneIndex, Road, Route
from highway_env.utils import Vector
from highway_env.vehicle.kinematics import Vehicle


[docs] class ControlledVehicle(Vehicle): """ A vehicle piloted by two low-level controller, allowing high-level actions such as cruise control and lane changes. - The longitudinal controller is a speed controller; - The lateral controller is a heading controller cascaded with a lateral position controller. """ target_speed: float """ Desired velocity.""" """Characteristic time""" TAU_ACC = 0.6 # [s] TAU_HEADING = 0.2 # [s] TAU_LATERAL = 0.6 # [s] TAU_PURSUIT = 0.5 * TAU_HEADING # [s] KP_A = 1 / TAU_ACC KP_HEADING = 1 / TAU_HEADING KP_LATERAL = 1 / TAU_LATERAL # [1/s] MAX_STEERING_ANGLE = np.pi / 3 # [rad] DELTA_SPEED = 5 # [m/s] def __init__( self, road: Road, position: Vector, heading: float = 0, speed: float = 0, target_lane_index: LaneIndex = None, target_speed: float = None, route: Route = None, ): super().__init__(road, position, heading, speed) self.target_lane_index = target_lane_index or self.lane_index self.target_speed = target_speed or self.speed self.route = route
[docs] @classmethod def create_from(cls, vehicle: "ControlledVehicle") -> "ControlledVehicle": """ Create a new vehicle from an existing one. The vehicle dynamics and target dynamics are copied, other properties are default. :param vehicle: a vehicle :return: a new vehicle at the same dynamical state """ v = cls( vehicle.road, vehicle.position, heading=vehicle.heading, speed=vehicle.speed, target_lane_index=vehicle.target_lane_index, target_speed=vehicle.target_speed, route=vehicle.route, ) return v
[docs] def plan_route_to(self, destination: str) -> "ControlledVehicle": """ Plan a route to a destination in the road network :param destination: a node in the road network """ try: path = self.road.network.shortest_path(self.lane_index[1], destination) except KeyError: path = [] if path: self.route = [self.lane_index] + [ (path[i], path[i + 1], None) for i in range(len(path) - 1) ] else: self.route = [self.lane_index] return self
[docs] def act(self, action: Union[dict, str] = None) -> None: """ Perform a high-level action to change the desired lane or speed. - If a high-level action is provided, update the target speed and lane; - then, perform longitudinal and lateral control. :param action: a high-level action """ self.follow_road() if action == "FASTER": self.target_speed += self.DELTA_SPEED elif action == "SLOWER": self.target_speed -= self.DELTA_SPEED elif action == "LANE_RIGHT": _from, _to, _id = self.target_lane_index target_lane_index = ( _from, _to, np.clip(_id + 1, 0, len(self.road.network.graph[_from][_to]) - 1), ) if self.road.network.get_lane(target_lane_index).is_reachable_from( self.position ): self.target_lane_index = target_lane_index elif action == "LANE_LEFT": _from, _to, _id = self.target_lane_index target_lane_index = ( _from, _to, np.clip(_id - 1, 0, len(self.road.network.graph[_from][_to]) - 1), ) if self.road.network.get_lane(target_lane_index).is_reachable_from( self.position ): self.target_lane_index = target_lane_index action = { "steering": self.steering_control(self.target_lane_index), "acceleration": self.speed_control(self.target_speed), } action["steering"] = np.clip( action["steering"], -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE ) super().act(action)
[docs] def follow_road(self) -> None: """At the end of a lane, automatically switch to a next one.""" if self.road.network.get_lane(self.target_lane_index).after_end(self.position): self.target_lane_index = self.road.network.next_lane( self.target_lane_index, route=self.route, position=self.position, np_random=self.road.np_random, )
[docs] def steering_control(self, target_lane_index: LaneIndex) -> float: """ Steer the vehicle to follow the center of an given lane. 1. Lateral position is controlled by a proportional controller yielding a lateral speed command 2. Lateral speed command is converted to a heading reference 3. Heading is controlled by a proportional controller yielding a heading rate command 4. Heading rate command is converted to a steering angle :param target_lane_index: index of the lane to follow :return: a steering wheel angle command [rad] """ target_lane = self.road.network.get_lane(target_lane_index) lane_coords = target_lane.local_coordinates(self.position) lane_next_coords = lane_coords[0] + self.speed * self.TAU_PURSUIT lane_future_heading = target_lane.heading_at(lane_next_coords) # Lateral position control lateral_speed_command = -self.KP_LATERAL * lane_coords[1] # Lateral speed to heading heading_command = np.arcsin( np.clip(lateral_speed_command / utils.not_zero(self.speed), -1, 1) ) heading_ref = lane_future_heading + np.clip( heading_command, -np.pi / 4, np.pi / 4 ) # Heading control heading_rate_command = self.KP_HEADING * utils.wrap_to_pi( heading_ref - self.heading ) # Heading rate to steering angle slip_angle = np.arcsin( np.clip( self.LENGTH / 2 / utils.not_zero(self.speed) * heading_rate_command, -1, 1, ) ) steering_angle = np.arctan(2 * np.tan(slip_angle)) steering_angle = np.clip( steering_angle, -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE ) return float(steering_angle)
[docs] def speed_control(self, target_speed: float) -> float: """ Control the speed of the vehicle. Using a simple proportional controller. :param target_speed: the desired speed :return: an acceleration command [m/s2] """ return self.KP_A * (target_speed - self.speed)
[docs] def get_routes_at_intersection(self) -> List[Route]: """Get the list of routes that can be followed at the next intersection.""" if not self.route: return [] for index in range(min(len(self.route), 3)): try: next_destinations = self.road.network.graph[self.route[index][1]] except KeyError: continue if len(next_destinations) >= 2: break else: return [self.route] next_destinations_from = list(next_destinations.keys()) routes = [ self.route[0 : index + 1] + [(self.route[index][1], destination, self.route[index][2])] for destination in next_destinations_from ] return routes
[docs] def set_route_at_intersection(self, _to: int) -> None: """ Set the road to be followed at the next intersection. Erase current planned route. :param _to: index of the road to follow at next intersection, in the road network """ routes = self.get_routes_at_intersection() if routes: if _to == "random": _to = self.road.np_random.integers(len(routes)) self.route = routes[_to % len(routes)]
[docs] def predict_trajectory_constant_speed( self, times: np.ndarray ) -> Tuple[List[np.ndarray], List[float]]: """ Predict the future positions of the vehicle along its planned route, under constant speed :param times: timesteps of prediction :return: positions, headings """ coordinates = self.lane.local_coordinates(self.position) route = self.route or [self.lane_index] pos_heads = [ self.road.network.position_heading_along_route( route, coordinates[0] + self.speed * t, 0, self.lane_index ) for t in times ] return tuple(zip(*pos_heads))
[docs] class MDPVehicle(ControlledVehicle): """A controlled vehicle with a specified discrete range of allowed target speeds.""" DEFAULT_TARGET_SPEEDS = np.linspace(20, 30, 3) def __init__( self, road: Road, position: List[float], heading: float = 0, speed: float = 0, target_lane_index: Optional[LaneIndex] = None, target_speed: Optional[float] = None, target_speeds: Optional[Vector] = None, route: Optional[Route] = None, ) -> None: """ Initializes an MDPVehicle :param road: the road on which the vehicle is driving :param position: its position :param heading: its heading angle :param speed: its speed :param target_lane_index: the index of the lane it is following :param target_speed: the speed it is tracking :param target_speeds: the discrete list of speeds the vehicle is able to track, through faster/slower actions :param route: the planned route of the vehicle, to handle intersections """ super().__init__( road, position, heading, speed, target_lane_index, target_speed, route ) self.target_speeds = ( np.array(target_speeds) if target_speeds is not None else self.DEFAULT_TARGET_SPEEDS ) self.speed_index = self.speed_to_index(self.target_speed) self.target_speed = self.index_to_speed(self.speed_index)
[docs] def act(self, action: Union[dict, str] = None) -> None: """ Perform a high-level action. - If the action is a speed change, choose speed from the allowed discrete range. - Else, forward action to the ControlledVehicle handler. :param action: a high-level action """ if action == "FASTER": self.speed_index = self.speed_to_index(self.speed) + 1 elif action == "SLOWER": self.speed_index = self.speed_to_index(self.speed) - 1 else: super().act(action) return self.speed_index = int( np.clip(self.speed_index, 0, self.target_speeds.size - 1) ) self.target_speed = self.index_to_speed(self.speed_index) super().act()
[docs] def index_to_speed(self, index: int) -> float: """ Convert an index among allowed speeds to its corresponding speed :param index: the speed index [] :return: the corresponding speed [m/s] """ return self.target_speeds[index]
[docs] def speed_to_index(self, speed: float) -> int: """ Find the index of the closest speed allowed to a given speed. Assumes a uniform list of target speeds to avoid searching for the closest target speed :param speed: an input speed [m/s] :return: the index of the closest speed allowed [] """ x = (speed - self.target_speeds[0]) / ( self.target_speeds[-1] - self.target_speeds[0] ) return np.int64( np.clip( np.round(x * (self.target_speeds.size - 1)), 0, self.target_speeds.size - 1, ) )
[docs] @classmethod def speed_to_index_default(cls, speed: float) -> int: """ Find the index of the closest speed allowed to a given speed. Assumes a uniform list of target speeds to avoid searching for the closest target speed :param speed: an input speed [m/s] :return: the index of the closest speed allowed [] """ x = (speed - cls.DEFAULT_TARGET_SPEEDS[0]) / ( cls.DEFAULT_TARGET_SPEEDS[-1] - cls.DEFAULT_TARGET_SPEEDS[0] ) return np.int64( np.clip( np.round(x * (cls.DEFAULT_TARGET_SPEEDS.size - 1)), 0, cls.DEFAULT_TARGET_SPEEDS.size - 1, ) )
@classmethod def get_speed_index(cls, vehicle: Vehicle) -> int: return getattr( vehicle, "speed_index", cls.speed_to_index_default(vehicle.speed) )
[docs] def predict_trajectory( self, actions: List, action_duration: float, trajectory_timestep: float, dt: float, ) -> List[ControlledVehicle]: """ Predict the future trajectory of the vehicle given a sequence of actions. :param actions: a sequence of future actions. :param action_duration: the duration of each action. :param trajectory_timestep: the duration between each save of the vehicle state. :param dt: the timestep of the simulation :return: the sequence of future states """ states = [] v = copy.deepcopy(self) t = 0 for action in actions: v.act(action) # High-level decision for _ in range(int(action_duration / dt)): t += 1 v.act() # Low-level control action v.step(dt) if (t % int(trajectory_timestep / dt)) == 0: states.append(copy.deepcopy(v)) return states