def _create_road(self):
"""
Create a road composed of straight adjacent lanes.
"""
self.road = Road(network=RoadNetwork.straight_road_network(
self.config["lanes_count"]),
np_random=self.np_random)
def _create_road(self) -> None:
# Circle lanes: (s)outh/(e)ast/(n)orth/(w)est (e)ntry/e(x)it.
center = [0, 0] # [m]
radius = self.config["derby_radius"] # [m]
alpha = 90 # [deg]
net = RoadNetwork()
radii = [radius]
n, c, s = LineType.NONE, LineType.CONTINUOUS, LineType.CONTINUOUS
line = [[c, s], [n, c]]
for lane in [0]:
net.add_lane(
"se", "ee",
CircularLane(center,
radii[lane],
np.deg2rad(-90 - alpha),
np.deg2rad(alpha + 5),
clockwise=True,
line_types=line[lane]))
net.add_lane(
"ee", "se",
CircularLane(center,
radii[lane],
np.deg2rad(alpha - 5),
np.deg2rad(92 + alpha),
clockwise=True,
line_types=line[lane]))
road = Road(network=net,
np_random=self.np_random,
record_history=self.config["show_trajectories"])
self.road = road
def _make_road(self) -> None:
"""
Make a road composed of a straight highway and a merging lane.
:return: the road
"""
net = RoadNetwork()
# Highway lanes
ends = [150, 80, 80, 150] # Before, converging, merge, after
c, s, n = LineType.CONTINUOUS_LINE, LineType.STRIPED, LineType.NONE
y = [0, StraightLane.DEFAULT_WIDTH]
line_type = [[c, s], [n, c]]
line_type_merge = [[c, s], [n, s]]
for i in range(2):
net.add_lane("a", "b", StraightLane([0, y[i]], [sum(ends[:2]), y[i]], line_types=line_type[i]))
net.add_lane("b", "c", StraightLane([sum(ends[:2]), y[i]], [sum(ends[:3]), y[i]], line_types=line_type_merge[i]))
net.add_lane("c", "d", StraightLane([sum(ends[:3]), y[i]], [sum(ends), y[i]], line_types=line_type[i]))
# Merging lane
amplitude = 3.25
ljk = StraightLane([0, 6.5 + 4 + 4], [ends[0], 6.5 + 4 + 4], line_types=[c, c], forbidden=True)
lkb = SineLane(ljk.position(ends[0], -amplitude), ljk.position(sum(ends[:2]), -amplitude),
amplitude, 2 * np.pi / (2*ends[1]), np.pi / 2, line_types=[c, c], forbidden=True)
lbc = StraightLane(lkb.position(ends[1], 0), lkb.position(ends[1], 0) + [ends[2], 0],
line_types=[n, c], forbidden=True)
net.add_lane("j", "k", ljk)
net.add_lane("k", "b", lkb)
net.add_lane("b", "c", lbc)
road = Road(network=net, np_random=self.np_random, record_history=self.config["show_trajectories"])
road.objects.append(Obstacle(road, lbc.position(ends[2], 0)))
self.road = road
def _create_road(self, spots=15):
"""
Create a road composed of straight adjacent lanes.
"""
net = RoadNetwork()
width = 4.0
lt = (LineType.CONTINUOUS, LineType.CONTINUOUS)
x_offset = 0
y_offset = 12
length = 8
# Parking spots
for k in range(spots):
x = (k - 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, speed_limit=5))
net.add_lane("b", "c", StraightLane([x, -y_offset], [x, -y_offset - length],
width=width, line_types=lt, speed_limit=5))
self.spots = spots
self.vehicle_starting = [x, y_offset + (length / 2)]
self.num_middle_lanes = 0
self.x_range = (int(spots / 2) + 1) * width
# Generate the middle lane for the busy parking lot
for y in np.arange(-y_offset + width, y_offset, width):
net.add_lane("d", "e", StraightLane([-self.x_range, y], [self.x_range, y],
width=width,
line_types=(LineType.STRIPED, LineType.STRIPED),
speed_limit=5))
self.num_middle_lanes += 1
self.road = Road(network=net,
np_random=self.np_random,
record_history=self.config["show_trajectories"])
def _make_road(self) -> None:
net = RoadNetwork()
radius = self.config["radius"] # [m]
# radius = np.random.choice([50, 500]) # [m]
# radius = np.random.choice([50, 200, 500, 1000]) # [m]
center = [0, StraightLane.DEFAULT_WIDTH + radius] # [m]
alpha = 0 # [deg]
radii = [
radius, radius + StraightLane.DEFAULT_WIDTH,
radius + 2 * StraightLane.DEFAULT_WIDTH
]
n, c, s = LineType.NONE, LineType.CONTINUOUS, LineType.STRIPED
line = [[c, s], [n, s], [n, c]]
for lane in [0, 1, 2]:
net.add_lane(
"a",
"b",
# CircularLane(center, radii[lane], np.deg2rad(90 - alpha), np.deg2rad(-90+alpha),
CircularLane(center,
radii[lane],
np.deg2rad(90 - alpha),
np.deg2rad(-360 + alpha),
clockwise=False,
line_types=line[lane]))
road = Road(network=net,
np_random=self.np_random,
record_history=self.config["show_trajectories"])
self.road = road
self.road.record_history = True
def _make_road(self):
net = RoadNetwork()
lane = SineLane([0, 0], [500, 0],
amplitude=5,
pulsation=2 * np.pi / 100,
phase=0,
width=10,
line_types=[LineType.STRIPED, LineType.STRIPED])
net.add_lane("a", "b", lane)
other_lane = StraightLane(
[50, 50], [115, 15],
line_types=[LineType.STRIPED, LineType.STRIPED],
width=10)
net.add_lane("c", "d", other_lane)
self.lanes = [other_lane, lane]
self.lane = self.lanes.pop(0)
net.add_lane(
"d", "a",
StraightLane([115, 15],
[115 + 20, 15 + 20 * (15 - 50) / (115 - 50)],
line_types=[LineType.NONE, LineType.STRIPED],
width=10))
road = Road(network=net,
np_random=self.np_random,
record_history=self.config["show_trajectories"])
self.road = road
def _create_road(self, spots=15):
"""
Create a road composed of straight adjacent lanes.
"""
net = RoadNetwork()
width = 4.0
lt = (LineType.CONTINUOUS, LineType.CONTINUOUS)
x_offset = 0
y_offset = 10
length = 8
for k in range(spots):
x = (k - 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))
self.road = Road(network=net,
np_random=self.np_random,
record_history=self.config["show_trajectories"])
def main():
env = gym.make('highway-v0')
road = Road.create_random_road(lanes_count=2,
lane_width=4.0,
vehicles_count=0,
vehicles_type=IDMVehicle)
vehicle = MDPVehicle.create_random(road)
road.vehicles.append(vehicle)
env.road = road
env.vehicle = vehicle
agent = MCTSAgent(env, iterations=100, temperature=20 *
5) # compare step by subtree and step by prior
sim = Simulation(env, agent)
t = 0
while not sim.done:
sim.step()
t += 1
if t == 10:
print('Added obstacle')
env.road.vehicles.append(
Obstacle(
road,
[env.vehicle.position[0] + 50., env.vehicle.position[1]]))
sim.close()
def _make_road(self, length=800):
"""
Make a road composed of a two-way road.
:return: the road
"""
net = RoadNetwork()
# Lanes
net.add_lane(
"a", "b",
StraightLane(
[0, 0], [length, 0],
line_types=[LineType.CONTINUOUS_LINE, LineType.STRIPED]))
net.add_lane(
"a", "b",
StraightLane([0, StraightLane.DEFAULT_WIDTH],
[length, StraightLane.DEFAULT_WIDTH],
line_types=[LineType.NONE, LineType.CONTINUOUS_LINE]))
net.add_lane(
"b", "a",
StraightLane([length, 0], [0, 0],
line_types=[LineType.NONE, LineType.NONE]))
road = Road(network=net,
np_random=self.np_random,
record_history=self.config["show_trajectories"])
self.road = road
def make_roads(self):
net = RoadNetwork()
n, c, s = LineType.NONE, LineType.CONTINUOUS, LineType.STRIPED
"""
e:entrance
before_inter:a node before inter_node
inter: stripe lines --> solid lines
"""
net.add_lane("se", "inters", StraightLane(np.array([0, 0]), np.array([0, -70]), line_types=[c, s]))
net.add_lane("se", "inters", StraightLane(np.array([4, 0]), np.array([4, -70]), line_types=[s, s]))
net.add_lane("se", "inters", StraightLane(np.array([8, 0]), np.array([8, -70]), line_types=[s, c]))
# net.add_lane("before_inters", "inters", StraightLane(np.array([0, -60]), np.array([0, -70]), line_types=[c, s]))
# net.add_lane("before_inters", "inters", StraightLane(np.array([4, -60]), np.array([4, -70]), line_types=[s, s]))
# net.add_lane("before_inters", "inters", StraightLane(np.array([8, -60]), np.array([8, -70]), line_types=[s, c]))
net.add_lane("inters", "sel",
StraightLane(np.array([0, -70]), np.array([0, -100]), line_types=[c, c], forbidden=True))
net.add_lane("inters", "sem",
StraightLane(np.array([4, -70]), np.array([4, -100]), line_types=[c, c], forbidden=True))
net.add_lane("inters", "ser",
StraightLane(np.array([8, -70]), np.array([8, -100]), line_types=[c, c], forbidden=True))
net.add_lane("nxl", "inter_n", StraightLane(np.array([0, -130]), np.array([0, -150]), line_types=[c, s]))
net.add_lane("nxm", "inter_n", StraightLane(np.array([4, -130]), np.array([4, -150]), line_types=[s, s]))
net.add_lane("nxr", "inter_n", StraightLane(np.array([8, -130]), np.array([8, -150]), line_types=[s, c]))
net.add_lane("interw", "wel", StraightLane(np.array([-30, -110]), np.array([-6, -110]), line_types=[c, c]))
net.add_lane("interw", "wer", StraightLane(np.array([-30, -106]), np.array([-6, -106]), line_types=[c, c]))
net.add_lane("wxl", "inter_w", StraightLane(np.array([-6, -118]), np.array([-30, -118]), line_types=[c, s]))
net.add_lane("wxr", "inter_w", StraightLane(np.array([-6, -122]), np.array([-30, -122]), line_types=[s, c]))
net.add_lane("intere", "eel", StraightLane(np.array([40, -118]), np.array([14, -118]), line_types=[c, c]))
net.add_lane("intere", "eer", StraightLane(np.array([40, -122]), np.array([14, -122]), line_types=[c, c]))
net.add_lane("exl", "inter_e", StraightLane(np.array([14, -110]), np.array([40, -110]), line_types=[c, s]))
net.add_lane("exr", "inter_e", StraightLane(np.array([14, -106]), np.array([40, -106]), line_types=[s, c]))
net.add_lane("sel", "wxl", StraightLane(np.array([0, -100]), np.array([-6, -118]), line_types=[n, n]))
net.add_lane("sem", "nxm", StraightLane(np.array([4, -100]), np.array([4, -130]), line_types=[n, n]))
net.add_lane("ser", "exr", StraightLane(np.array([8, -100]), np.array([14, -106]), line_types=[n, n]))
road = Road(network=net, np_random=self.np_random)
green_time = 5
red_time = 8
green_flash_time = 2
yellow_time = 1
"""
southwest crossroad traffic lights
"""
self.traffic_lights = [
RedLight(road, [0, -100], red_time, green_time, green_flash_time, yellow_time, 1),
RedLight(road, [4, -100], red_time, green_time, green_flash_time, yellow_time, 1),
RedLight(road, [8, -100], 0, 10, 0, 0, 1),
]
self.road = road
self.road.vehicles = self.traffic_lights
def _create_road(self):
"""
Create a road composed of straight adjacent lanes.
"""
self.road = Road(network=RoadNetwork.straight_road_network(
self.config["lanes_count"]),
np_random=self.np_random,
record_history=self.config["show_trajectories"])
def test_front():
r = Road(RoadNetwork.straight_road_network(1))
v1 = Vehicle(road=r, position=[0, 0], velocity=20)
v2 = Vehicle(road=r, position=[10, 0], velocity=10)
r.vehicles.extend([v1, v2])
assert v1.lane_distance_to(v2) == pytest.approx(10)
assert v2.lane_distance_to(v1) == pytest.approx(-10)
def test_partial():
road = Road(RoadNetwork.straight_road_network())
v = IntervalVehicle(road, position=[0, 0], velocity=20, heading=0)
for _ in range(2 * FPS):
v.step(dt=1/FPS, mode="partial")
assert v.interval.position[0, 0] <= v.position[0] <= v.interval.position[1, 0]
assert v.interval.position[0, 1] <= v.position[1] <= v.interval.position[1, 1]
assert v.interval.heading[0] <= v.heading <= v.interval.heading[1]
def __init__(self, circuit_config, np_random):
self._circuit_config = circuit_config
self._road_network = RoadNetwork()
self._road = Road(network=self._road_network,
np_random=np_random,
record_history=True)
self._route = list()
self._circuit_length = 0
self._create_circuit()
def _make_road(self):
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 = 40 # [m]
# Corners: 0:south, 1:west, 2:north, 3:east. i:inner, o:outer, r:right, l:left
net = RoadNetwork()
n, c, s = LineType.NONE, LineType.CONTINUOUS, LineType.STRIPED
for corner in range(4):
angle = rad(90 * corner)
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]))
# 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 + rad(180),
angle + rad(270),
line_types=[n, c]))
# 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 + rad(0),
angle + rad(-90),
clockwise=False,
line_types=[n, n]))
# 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]))
# 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]))
road = Road(network=net, np_random=self.np_random)
self.road = road
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 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_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 _make_road(self):
"""
Make a road composed of a straight highway and a merging lane.
:return: the road
"""
net = RoadNetwork()
c, s, n = LineType.CONTINUOUS_LINE, LineType.STRIPED, LineType.NONE
y = [0, StraightLane.DEFAULT_WIDTH, 2 * StraightLane.DEFAULT_WIDTH]
line_type = [[c, s], [n, s], [n, c]]
for i in range(3):
net.add_lane("a", "b", StraightLane([0, y[i]], [self.HIGHWAY_LENGTH, y[i]], line_types=line_type[i]))
road = Road(network=net, np_random=self.np_random)
self.road = road
def make_road(self):
net = RoadNetwork()
# pre-processing
c, s, n = LineType.CONTINUOUS_LINE, LineType.STRIPED, LineType.NONE
W = StraightLane.DEFAULT_WIDTH
# Highway lanes
ends = [150, 10, 80]
ptr_LB, ptr_UB = 0, 0
ptr_LB, ptr_UB = ptr_UB, ptr_UB + ends[0]
net.add_lane("a", "b",
StraightLane([ptr_LB, W], [ptr_UB, W],
line_types=[c, s])) # Lane 1
net.add_lane("a", "b",
StraightLane([ptr_LB, 2 * W], [ptr_UB, 2 * W],
line_types=[s, c])) # Lane 2
ptr_LB, ptr_UB = ptr_UB, ptr_UB + ends[1]
net.add_lane("b", "c",
StraightLane([ptr_LB, W], [ptr_UB, W],
line_types=[c, s])) # Lane 1
net.add_lane("b", "c",
StraightLane([ptr_LB, 2 * W], [ptr_UB, 2 * W],
line_types=[s, c])) # Lane 2
ptr_LB, ptr_UB = ptr_UB, ptr_UB + ends[2]
net.add_lane("c", "d",
StraightLane([ptr_LB, W], [ptr_UB, W],
line_types=[c, s])) # Lane 1
net.add_lane("c", "d",
StraightLane([ptr_LB, 2 * W], [ptr_UB, 2 * W],
line_types=[s, c])) # Lane 2
# Crossing Lane
ends = [2, 2 * W, 2]
ptr_LB, ptr_UB = 0, 0
ptr_LB, ptr_UB = ptr_UB, ptr_UB + ends[0]
lij = PedestrianLane([150, ptr_LB], [150, ptr_UB], width=10)
ptr_LB, ptr_UB = ptr_UB, ptr_UB + ends[1]
ljk = PedestrianLane([150, ptr_LB], [150, ptr_UB], width=10)
ptr_LB, ptr_UB = ptr_UB, ptr_UB + ends[2]
lkl = PedestrianLane([150, ptr_LB], [150, ptr_UB], width=10)
net.add_lane("i", "j", lij)
net.add_lane("j", "k", ljk)
net.add_lane("k", "l", lkl)
road = Road(network=net)
# road.vehicles.append(Obstacle(road, [150-10, 2*W]))
road.vehicles.append(Occlusion(road, [150 - 10, 2 * W], [2 * W, W]))
self.road = road
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
def _create_road(self):
road = Road.create_random_road(
lanes_count=self.config["lanes_count"],
vehicles_count=self.config["vehicles_count"],
vehicles_type=utils.class_from_path(
self.config["other_vehicles_type"]),
np_random=self.np_random)
vehicle = MDPVehicle.create_random(
road,
25,
spacing=self.config["initial_spacing"],
prepend=True,
np_random=self.np_random)
road.vehicles.append(vehicle)
return road, vehicle
def make_straight(self):
lm10 = StraightLane(np.array([0, 0]),
0,
4.0, [LineType.CONTINUOUS_LINE, LineType.STRIPED],
bounds=[0, 500])
l1 = LanesConcatenation([lm10])
lm20 = StraightLane(l1.position(0, 4),
0,
4.0, [LineType.STRIPED, LineType.STRIPED],
bounds=[0, 500])
l2 = LanesConcatenation([lm20])
# lm30 = StraightLane(l2.position(0,4), 0, 4.0, [LineType.STRIPED, LineType.STRIPED],bounds=[0,100])
# lm31 = StraightLane(lm30.position(0,0), 0, 4.0, [LineType.STRIPED, LineType.STRIPED],bounds=[0,500])
# l3 = LanesConcatenation([lm30,lm31])
lm30 = StraightLane(l2.position(0, 4),
0,
4.0, [LineType.STRIPED, LineType.STRIPED],
bounds=[0, 500])
l3 = LanesConcatenation([lm30])
amplitude = 4.5
lm40 = StraightLane(l3.position(0, 4),
0,
4.0, [LineType.STRIPED, LineType.CONTINUOUS_LINE],
bounds=[200, 400])
lm41 = SineLane(lm40.position(400, amplitude),
0,
4.0,
-amplitude,
2 * np.pi / (2 * 50),
np.pi / 2, [LineType.CONTINUOUS, LineType.CONTINUOUS],
bounds=[0, 50],
forbidden=True)
lm42 = StraightLane(
lm41.position(50, 0),
0,
4.0, [LineType.CONTINUOUS_LINE, LineType.CONTINUOUS_LINE],
bounds=[0, 50],
forbidden=True)
l4 = LanesConcatenation([lm40, lm41, lm42])
road = Road([l1, l2, l3, l4])
# road = Road([ l3])
# road = Road([lm0,lm2])
# todo !!!!!!!!!!! how to do with Obstacle in lane.vehicles
obstacle = Obstacle(road, lm40.position(0, 0))
road.vehicles.append(obstacle)
road.lanes[3].vehicles.append(obstacle)
self.road = road
def main():
road = Road.create_random_road(lanes_count=2,
lane_width=4.0,
vehicles_count=1,
vehicles_type=LinearVehicle)
v = road.vehicles[0]
v.enable_lane_change = False
le = LinearEstimator(v)
sim = Simulation(road, ego_vehicle_type=ControlledVehicle)
while not sim.done:
sim.handle_events()
sim.act()
le.update(sim.dt)
sim.step()
sim.display()
sim.quit()
def make_road(self):
# Circle lanes: (s)outh/(e)ast/(n)orth/(w)est (e)ntry/e(x)it.
center = [0, 0] # [m]
radius = 30 # [m]
alpha = 20 # [deg]
net = RoadNetwork()
radii = [radius, radius+4]
n, c, s = LineType.NONE, LineType.CONTINUOUS, LineType.STRIPED
line = [[c, s], [n, c]]
for lane in [0, 1]:
net.add_lane("se", "ex", CircularLane(center, radii[lane], rad(90-alpha), rad(alpha), line_types=line[lane]))
net.add_lane("ex", "ee", CircularLane(center, radii[lane], rad(alpha), rad(-alpha), line_types=line[lane]))
net.add_lane("ee", "nx", CircularLane(center, radii[lane], rad(-alpha), rad(-90+alpha), line_types=line[lane]))
net.add_lane("nx", "ne", CircularLane(center, radii[lane], rad(-90+alpha), rad(-90-alpha), line_types=line[lane]))
net.add_lane("ne", "wx", CircularLane(center, radii[lane], rad(-90-alpha), rad(-180+alpha), line_types=line[lane]))
net.add_lane("wx", "we", CircularLane(center, radii[lane], rad(-180+alpha), rad(-180-alpha), line_types=line[lane]))
net.add_lane("we", "sx", CircularLane(center, radii[lane], rad(180-alpha), rad(90+alpha), line_types=line[lane]))
net.add_lane("sx", "se", CircularLane(center, radii[lane], rad(90+alpha), rad(90-alpha), line_types=line[lane]))
# Access lanes: (r)oad/(s)ine
access = 200 # [m]
dev = 120 # [m]
a = 5 # [m]
delta_st = 0.20*dev # [m]
delta_en = dev-delta_st
w = 2*np.pi/dev
net.add_lane("ser", "ses", StraightLane([2, access], [2, dev/2], line_types=[s, c]))
net.add_lane("ses", "se", SineLane([2+a, dev/2], [2+a, dev/2-delta_st], a, w, -np.pi/2, line_types=[c, c]))
net.add_lane("sx", "sxs", SineLane([-2-a, -dev/2+delta_en], [-2-a, dev/2], a, w, -np.pi/2+w*delta_en, line_types=[c, c]))
net.add_lane("sxs", "sxr", StraightLane([-2, dev / 2], [-2, access], line_types=[n, c]))
net.add_lane("eer", "ees", StraightLane([access, -2], [dev / 2, -2], line_types=[s, c]))
net.add_lane("ees", "ee", SineLane([dev / 2, -2-a], [dev / 2 - delta_st, -2-a], a, w, -np.pi / 2, line_types=[c, c]))
net.add_lane("ex", "exs", SineLane([-dev / 2 + delta_en, 2+a], [dev / 2, 2+a], a, w, -np.pi / 2 + w * delta_en, line_types=[c, c]))
net.add_lane("exs", "exr", StraightLane([dev / 2, 2], [access, 2], line_types=[n, c]))
net.add_lane("ner", "nes", StraightLane([-2, -access], [-2, -dev / 2], line_types=[s, c]))
net.add_lane("nes", "ne", SineLane([-2 - a, -dev / 2], [-2 - a, -dev / 2 + delta_st], a, w, -np.pi / 2, line_types=[c, c]))
net.add_lane("nx", "nxs", SineLane([2 + a, dev / 2 - delta_en], [2 + a, -dev / 2], a, w, -np.pi / 2 + w * delta_en, line_types=[c, c]))
net.add_lane("nxs", "nxr", StraightLane([2, -dev / 2], [2, -access], line_types=[n, c]))
road = Road(network=net)
self.road = road
def test_stop_before_obstacle(vehicle_type):
road = Road(RoadNetwork.straight_road_network(lanes=1))
vehicle = vehicle_type(road=road, position=[0, 0], speed=20, heading=0)
obstacle = Obstacle(road=road, position=[80, 0])
road.vehicles.append(vehicle)
road.objects.append(obstacle)
for _ in range(10 * FPS):
road.act()
road.step(dt=1/FPS)
assert not vehicle.crashed
assert vehicle.position[0] == pytest.approx(obstacle.position[0] - vehicle_type.DISTANCE_WANTED, abs=1)
assert vehicle.position[1] == pytest.approx(0)
assert vehicle.speed == pytest.approx(0, abs=1)
assert vehicle.heading == pytest.approx(0)
def _make_road(self, length=128):
"""
Making double lane road with counter-clockwise U-Turn.
:return: the road
"""
net = RoadNetwork()
# Defining upper starting lanes after the U-Turn.
# These Lanes are defined from x-coordinate 'length' to 0.
net.add_lane("c", "d", StraightLane([length, StraightLane.DEFAULT_WIDTH], [0, StraightLane.DEFAULT_WIDTH],
line_types=(LineType.CONTINUOUS_LINE, LineType.STRIPED)))
net.add_lane("c", "d", StraightLane([length, 0], [0, 0],
line_types=(LineType.NONE, LineType.CONTINUOUS_LINE)))
# Defining counter-clockwise circular U-Turn lanes.
center = [length, StraightLane.DEFAULT_WIDTH + 20] # [m]
radius = 20 # [m]
alpha = 0 # [deg]
radii = [radius, radius+StraightLane.DEFAULT_WIDTH]
n, c, s = LineType.NONE, LineType.CONTINUOUS, LineType.STRIPED
line = [[c, s], [n, c]]
for lane in [0, 1]:
net.add_lane("b", "c",
CircularLane(center, radii[lane], np.deg2rad(90 - alpha), np.deg2rad(-90+alpha),
clockwise=False, line_types=line[lane]))
offset = 2*radius
# Defining lower starting lanes before the U-Turn.
# These Lanes are defined from x-coordinate 0 to 'length'.
net.add_lane("a", "b", StraightLane([0, ((2 * StraightLane.DEFAULT_WIDTH + offset) - StraightLane.DEFAULT_WIDTH)],
[length, ((2 * StraightLane.DEFAULT_WIDTH + offset) - StraightLane.DEFAULT_WIDTH)],
line_types=(LineType.CONTINUOUS_LINE,
LineType.STRIPED)))
net.add_lane("a", "b", StraightLane([0, (2 * StraightLane.DEFAULT_WIDTH + offset)],
[length, (2 * StraightLane.DEFAULT_WIDTH + offset)],
line_types=(LineType.NONE,
LineType.CONTINUOUS_LINE)))
road = Road(network=net, np_random=self.np_random, record_history=self.config["show_trajectories"])
self.road = road
def test_collision():
# Collision between two vehicles
r = Road(RoadNetwork.straight_road_network(1))
v1 = Vehicle(road=r, position=[0, 0], speed=10)
v2 = Vehicle(road=r, position=[4, 0], speed=20)
v1.check_collision(v2)
assert v1.crashed and v2.crashed
# Collision between a vehicle and an obstacle
v3 = Vehicle(road=r, position=[20, 0], speed=10)
o = Obstacle(road=r, position=[23, 0])
v3.check_collision(o)
assert v3.crashed and o.hit
# Collision between a vehicle and a landmark
v4 = Vehicle(road=r, position=[40, 0], speed=10)
l = Landmark(road=r, position=[43, 0])
v4.check_collision(l)
assert v4.crashed is False
assert l.hit
def _create_road(self,
road_length=1000,
exit_position=400,
exit_length=100) -> None:
net = RoadNetwork.straight_road_network(self.config["lanes_count"],
start=0,
length=exit_position,
nodes_str=("0", "1"))
net = RoadNetwork.straight_road_network(self.config["lanes_count"] + 1,
start=exit_position,
length=exit_length,
nodes_str=("1", "2"),
net=net)
net = RoadNetwork.straight_road_network(
self.config["lanes_count"],
start=exit_position + exit_length,
length=road_length - exit_position - exit_length,
nodes_str=("2", "3"),
net=net)
for _from in net.graph:
for _to in net.graph[_from]:
for _id in range(len(net.graph[_from][_to])):
net.get_lane(
(_from, _to, _id)).speed_limit = 26 - 3.4 * _id
exit_position = np.array([
exit_position + exit_length,
self.config["lanes_count"] * CircularLane.DEFAULT_WIDTH
])
radius = 150
exit_center = exit_position + np.array([0, radius])
lane = CircularLane(center=exit_center,
radius=radius,
start_phase=3 * np.pi / 2,
end_phase=2 * np.pi,
forbidden=True)
net.add_lane("2", "exit", lane)
self.road = Road(network=net,
np_random=self.np_random,
record_history=self.config["show_trajectories"])
def generate_data(count):
# Vehicle.COLLISIONS_ENABLED = False
vehicle_type = LinearVehicle
road = Road.create_random_road(lanes_count=2,
lane_width=4.0,
vehicles_count=5,
vehicles_type=vehicle_type)
sim = Simulation(road, ego_vehicle_type=vehicle_type, displayed=True)
sim.RECORD_VIDEO = False
road.add_random_vehicles(5, vehicles_type=vehicle_type)
road.vehicles.append(Obstacle(road, np.array([50., 0])))
road.vehicles.append(Obstacle(road, np.array([130., 4.])))
for v in road.vehicles:
v.target_velocity = LinearVehicle.VELOCITY_WANTED
# v.enable_lane_change = False
for _ in range(count):
sim.handle_events()
sim.act()
sim.road.dump()
sim.step()
sim.display()
sim.quit()
return [v.get_log() for v in road.vehicles if not isinstance(v, Obstacle)]
