def steering_control(self, target_lane_index):
"""
Steer the vehicle to follow the center of an given lane.
1. Lateral position is controlled by a proportional controller yielding a lateral velocity command
2. Lateral velocity 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.velocity * self.PURSUIT_TAU
lane_future_heading = target_lane.heading_at(lane_next_coords)
# Lateral position control
lateral_velocity_command = -self.KP_LATERAL * lane_coords[1]
# Lateral velocity to heading
heading_command = np.arcsin(
np.clip(lateral_velocity_command / utils.not_zero(self.velocity),
-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
steering_angle = np.arctan(
self.LENGTH / utils.not_zero(self.velocity) * heading_rate_command)
steering_angle = np.clip(steering_angle, -self.MAX_STEERING_ANGLE,
self.MAX_STEERING_ANGLE)
return steering_angle
def acceleration(self,
ego_vehicle: ControlledVehicle,
front_vehicle: Vehicle = None,
rear_vehicle: Vehicle = None) -> float:
"""
Compute an acceleration command with the Intelligent Driver Model.
The acceleration is chosen so as to:
- reach a target speed;
- maintain a minimum safety distance (and safety time) w.r.t the front vehicle.
:param ego_vehicle: the vehicle whose desired acceleration is to be computed. It does not have to be an
IDM vehicle, which is why this method is a class method. This allows an IDM vehicle to
reason about other vehicles behaviors even though they may not IDMs.
:param front_vehicle: the vehicle preceding the ego-vehicle
:param rear_vehicle: the vehicle following the ego-vehicle
:return: the acceleration command for the ego-vehicle [m/s2]
"""
if not ego_vehicle or not isinstance(ego_vehicle, Vehicle):
return 0
ego_target_speed = utils.not_zero(
getattr(ego_vehicle, "target_speed", 0))
acceleration = self.COMFORT_ACC_MAX * (1 - np.power(
max(ego_vehicle.speed, 0) / ego_target_speed, self.DELTA))
if front_vehicle:
d = ego_vehicle.lane_distance_to(front_vehicle)
acceleration -= self.COMFORT_ACC_MAX * \
np.power(self.desired_gap(ego_vehicle, front_vehicle) / utils.not_zero(d), 2)
return acceleration
def acceleration(cls, ego_vehicle, front_vehicle=None, rear_vehicle=None):
"""
Compute an acceleration command with the Intelligent Driver Model.
The acceleration is chosen so as to:
- reach a target velocity;
- maintain a minimum safety distance (and safety time) w.r.t the front vehicle.
:param ego_vehicle: the vehicle whose desired acceleration is to be computed. It does not have to be an
IDM vehicle, which is why this method is a class method. This allows an IDM vehicle to
reason about other vehicles behaviors even though they may not IDMs.
:param front_vehicle: the vehicle preceding the ego-vehicle
:param rear_vehicle: the vehicle following the ego-vehicle
:return: the acceleration command for the ego-vehicle [m/s2]
"""
if not ego_vehicle:
return 0
acceleration = cls.COMFORT_ACC_MAX * (1 - np.power(
ego_vehicle.velocity / utils.not_zero(ego_vehicle.target_velocity),
cls.DELTA))
if front_vehicle:
d = ego_vehicle.lane_distance_to(front_vehicle)
acceleration -= cls.COMFORT_ACC_MAX * \
np.power(cls.desired_gap(ego_vehicle, front_vehicle) / utils.not_zero(d), 2)
return acceleration
def acceleration(self, ego_vehicle, front_vehicle=None, rear_vehicle=None):
"""
Compute an acceleration command with the Intelligent Driver Model.
The acceleration is chosen so as to:
- reach a target velocity;
- maintain a minimum safety distance (and safety time) w.r.t the front vehicle.
:param ego_vehicle: the vehicle whose desired acceleration is to be computed. It does not have to be an
IDM vehicle, which is why this method is a class method. This allows an IDM vehicle to
reason about other vehicles behaviors even though they may not IDMs.
:param front_vehicle: the vehicle preceding the ego-vehicle
:param rear_vehicle: the vehicle following the ego-vehicle
:return: the acceleration command for the ego-vehicle [m/s2]
"""
if not ego_vehicle:
return 0
ego_target_velocity = utils.not_zero(
getattr(ego_vehicle, "target_velocity", 0))
acceleration = self.COMFORT_ACC_MAX * (1 - np.power(
max(ego_vehicle.velocity, 0) / ego_target_velocity, self.DELTA))
if front_vehicle:
d = ego_vehicle.lane_distance_to(front_vehicle)
acceleration -= self.COMFORT_ACC_MAX * \
np.power(self.desired_gap(ego_vehicle, front_vehicle) / utils.not_zero(d), 2)
# print("distance to front vehicle", d, ego_vehicle.LENGTH)
# if ((front_vehicle.lane == self.lane and d < 1.5*ego_vehicle.LENGTH)):
# # print(ego_vehicle.lane, front_vehicle.lane)
# acceleration = -self.ACC_MAX
# self.target_velocity = 15
# print("target_velocity", self.target_velocity)
return acceleration
def steering_features(self, target_lane_index):
"""
A collection of features used to follow a lane
:param target_lane_index: index of the lane to follow
:return: a array of features
"""
lane = self.road.network.get_lane(target_lane_index)
lane_coords = lane.local_coordinates(self.position)
lane_next_coords = lane_coords[0] + self.velocity * self.PURSUIT_TAU
lane_future_heading = lane.heading_at(lane_next_coords)
features = np.array([utils.wrap_to_pi(lane_future_heading - self.heading) *
self.LENGTH / utils.not_zero(self.velocity),
-lane_coords[1] * self.LENGTH / (utils.not_zero(self.velocity) ** 2)])
return features
def compute_ttc_grid(env, time_quantization, horizon, considered_lanes="all"):
"""
For each ego-velocity and lane, compute the predicted time-to-collision to each vehicle within the lane and
store the results in an occupancy grid.
"""
road_lanes = env.road.network.all_side_lanes(env.vehicle.lane_index)
grid = np.zeros((env.vehicle.SPEED_COUNT, len(road_lanes), int(horizon / time_quantization)))
for velocity_index in range(grid.shape[0]):
ego_velocity = env.vehicle.index_to_speed(velocity_index)
for other in env.road.vehicles:
if (other is env.vehicle) or (ego_velocity == other.velocity):
continue
margin = other.LENGTH / 2 + env.vehicle.LENGTH / 2
collision_points = [(0, 1), (-margin, 0.5), (margin, 0.5)]
for m, cost in collision_points:
distance = env.vehicle.lane_distance_to(other) + m
other_projected_velocity = other.velocity
time_to_collision = distance / utils.not_zero(ego_velocity - other_projected_velocity)
if time_to_collision < 0:
continue
if env.road.network.is_connected_road(env.vehicle.lane_index, other.lane_index,
route=env.vehicle.route, depth=3):
# Same road, or connected road with same number of lanes
if len(env.road.network.all_side_lanes(other.lane_index)) == len(env.road.network.all_side_lanes(env.vehicle.lane_index)):
lane = [other.lane_index[2]]
# Different road of different number of lanes: uncertainty on future lane, use all
else:
lane = range(grid.shape[1])
# Quantize time-to-collision to both upper and lower values
for time in [int(time_to_collision / time_quantization),
int(np.ceil(time_to_collision / time_quantization))]:
if 0 <= time < grid.shape[2]:
# TODO: check lane overflow (e.g. vehicle with higher lane id than current road capacity)
grid[velocity_index, lane, time] = np.maximum(grid[velocity_index, lane, time], cost)
return grid
def compute_ttc_grid(env, time_quantization, horizon):
"""
For each ego-velocity and lane, compute the predicted time-to-collision to each vehicle within the lane and
store the results in an occupancy grid.
"""
grid = np.zeros((env.vehicle.SPEED_COUNT, len(env.road.lanes),
int(horizon / time_quantization)))
for velocity_index in range(grid.shape[0]):
ego_velocity = env.vehicle.index_to_speed(velocity_index)
for other in env.road.vehicles:
if (other is env.vehicle) or (ego_velocity == other.velocity):
continue
margin = other.LENGTH / 2 + env.vehicle.LENGTH / 2
collision_points = [(0, 1), (-margin, 0.5), (margin, 0.5)]
for m, cost in collision_points:
distance = env.vehicle.lane_distance_to(other) + m
time_to_collision = distance / utils.not_zero(ego_velocity -
other.velocity)
if time_to_collision < 0:
continue
# Quantize time-to-collision to both upper and lower values
lane = other.lane_index
for time in [
int(time_to_collision / time_quantization),
int(np.ceil(time_to_collision / time_quantization))
]:
if 0 <= lane < grid.shape[1] and 0 <= time < grid.shape[2]:
grid[velocity_index, lane,
time] = max(grid[velocity_index, lane, time],
cost)
return grid
def compute_ttc_grid(env: 'AbstractEnv',
time_quantization: float,
horizon: float,
vehicle: Optional[Vehicle] = None) -> np.ndarray:
"""
Compute the grid of predicted time-to-collision to each vehicle within the lane
For each ego-speed and lane.
:param env: environment
:param time_quantization: time step of a grid cell
:param horizon: time horizon of the grid
:param vehicle: the observer vehicle
:return: the time-co-collision grid, with axes SPEED x LANES x TIME
"""
vehicle = vehicle or env.vehicle
road_lanes = env.road.network.all_side_lanes(env.vehicle.lane_index)
grid = np.zeros((vehicle.SPEED_COUNT, len(road_lanes),
int(horizon / time_quantization)))
for speed_index in range(grid.shape[0]):
ego_speed = vehicle.index_to_speed(speed_index)
for other in env.road.vehicles:
if (other is vehicle) or (ego_speed == other.speed):
continue
margin = other.LENGTH / 2 + vehicle.LENGTH / 2
collision_points = [(0, 1), (-margin, 0.5), (margin, 0.5)]
for m, cost in collision_points:
distance = vehicle.lane_distance_to(other) + m
other_projected_speed = other.speed * np.dot(
other.direction, vehicle.direction)
time_to_collision = distance / utils.not_zero(
ego_speed - other_projected_speed)
if time_to_collision < 0:
continue
if env.road.network.is_connected_road(vehicle.lane_index,
other.lane_index,
route=vehicle.route,
depth=3):
# Same road, or connected road with same number of lanes
if len(env.road.network.all_side_lanes(
other.lane_index)) == len(
env.road.network.all_side_lanes(
vehicle.lane_index)):
lane = [other.lane_index[2]]
# Different road of different number of lanes: uncertainty on future lane, use all
else:
lane = range(grid.shape[1])
# Quantize time-to-collision to both upper and lower values
for time in [
int(time_to_collision / time_quantization),
int(np.ceil(time_to_collision / time_quantization))
]:
if 0 <= time < grid.shape[2]:
# TODO: check lane overflow (e.g. vehicle with higher lane id than current road capacity)
grid[speed_index, lane, time] = np.maximum(
grid[speed_index, lane, time], cost)
return grid
