def test_step():
v = ControlledVehicle(road=None, position=[0, 0], speed=20, heading=0)
for _ in range(2 * FPS):
v.step(dt=1 / FPS)
assert v.position[0] == pytest.approx(40)
assert v.position[1] == pytest.approx(0)
assert v.speed == pytest.approx(20)
assert v.heading == pytest.approx(0)
def is_conflict_possible(v1: ControlledVehicle, v2: ControlledVehicle, horizon: int = 3, step: float = 0.25) -> bool:
times = np.arange(step, horizon, step)
positions_1, headings_1 = v1.predict_trajectory_constant_speed(times)
positions_2, headings_2 = v2.predict_trajectory_constant_speed(times)
for position_1, heading_1, position_2, heading_2 in zip(positions_1, headings_1, positions_2, headings_2):
# Fast spherical pre-check
if np.linalg.norm(position_2 - position_1) > v1.LENGTH:
continue
# Accurate rectangular check
if utils.rotated_rectangles_intersect((position_1, 1.5*v1.LENGTH, 0.9*v1.WIDTH, heading_1),
(position_2, 1.5*v2.LENGTH, 0.9*v2.WIDTH, heading_2)):
return True
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 test_network():
# Diamond
net = RoadNetwork()
net.add_lane(0, 1, StraightLane([0, 0], [10, 0]))
net.add_lane(1, 2, StraightLane([10, 0], [5, 5]))
net.add_lane(2, 0, StraightLane([5, 5], [0, 0]))
net.add_lane(1, 3, StraightLane([10, 0], [5, -5]))
net.add_lane(3, 0, StraightLane([5, -5], [0, 0]))
print(net.graph)
# Road
road = Road(network=net)
v = ControlledVehicle(road, [5, 0], heading=0, target_velocity=2)
road.vehicles.append(v)
assert v.lane_index == (0, 1, 0)
# Lane changes
dt = 1 / 15
lane_index = v.target_lane_index
lane_changes = 0
for _ in range(int(20 / dt)):
road.act()
road.step(dt)
if lane_index != v.target_lane_index:
lane_index = v.target_lane_index
lane_changes += 1
assert lane_changes >= 3
def test_speed_control():
road = Road(RoadNetwork.straight_road_network(1))
v = ControlledVehicle(road=road, position=road.network.get_lane(("0", "1", 0)).position(0, 0), speed=20, heading=0)
v.act('FASTER')
for _ in range(int(3 * v.TAU_ACC * FPS)):
v.act()
v.step(dt=1/FPS)
assert v.speed == pytest.approx(20 + v.DELTA_SPEED, abs=0.5)
assert v.position[1] == pytest.approx(0)
assert v.lane_index[2] == 0
def test_lane_change():
road = Road(RoadNetwork.straight_road_network(2))
v = ControlledVehicle(road=road, position=road.network.get_lane(("0", "1", 0)).position(0, 0), speed=20, heading=0)
v.act('LANE_RIGHT')
for _ in range(3 * FPS):
v.act()
v.step(dt=1/FPS)
assert v.speed == pytest.approx(20)
assert v.position[1] == pytest.approx(StraightLane.DEFAULT_WIDTH, abs=StraightLane.DEFAULT_WIDTH/4)
assert v.lane_index[2] == 1
def acceleration_features(self, ego_vehicle: ControlledVehicle,
front_vehicle: Vehicle = None,
rear_vehicle: Vehicle = None) -> np.ndarray:
vt, dv, dp = 0, 0, 0
if ego_vehicle:
vt = ego_vehicle.target_speed - ego_vehicle.speed
d_safe = self.DISTANCE_WANTED + np.maximum(ego_vehicle.speed, 0) * self.TIME_WANTED
if front_vehicle:
d = ego_vehicle.lane_distance_to(front_vehicle)
dv = min(front_vehicle.speed - ego_vehicle.speed, 0)
dp = min(d - d_safe, 0)
return np.array([vt, dv, dp])
def test_velocity_control():
road = Road(RoadNetwork.straight_road_network(1))
v = ControlledVehicle(road=road,
position=road.network.get_lane(
(0, 1, 0)).position(0, 0),
velocity=20,
heading=0)
v.act('FASTER')
for _ in range(int(3 * v.TAU_A * FPS)):
v.act()
v.step(dt=1 / FPS)
assert v.velocity == pytest.approx(20 + v.DELTA_VELOCITY, abs=0.5)
assert v.position[1] == pytest.approx(0)
assert v.lane_index[2] == 0