def CreateAdverseRoad(
positive_road: "Road",
roadnet_to_get_road: "RoadNetwork", # mostly, block network
roadnet_to_check_cross: "RoadNetwork", # mostly, previous global network
ignore_start: str = None,
ignore_end: str = None,
center_line_type=LineType.
CONTINUOUS, # Identical to Block.CENTER_LINE_TYPE
side_lane_line_type=LineType.SIDE,
inner_lane_line_type=LineType.BROKEN,
center_line_color=LineColor.YELLOW,
ignore_intersection_checking=None) -> bool:
adverse_road = -positive_road
lanes = get_lanes_on_road(positive_road, roadnet_to_get_road)
reference_lane = lanes[-1]
num = len(lanes) * 2
width = reference_lane.width_at(0)
if isinstance(reference_lane, StraightLane):
start_point = reference_lane.position(lanes[-1].length,
-(num - 1) * width)
end_point = reference_lane.position(0, -(num - 1) * width)
symmetric_lane = StraightLane(start_point, end_point, width,
lanes[-1].line_types,
reference_lane.forbidden,
reference_lane.speed_limit,
reference_lane.priority)
elif isinstance(reference_lane, CircularLane):
start_phase = reference_lane.end_phase
end_phase = reference_lane.start_phase
clockwise = False if reference_lane.direction == 1 else True
if not clockwise:
radius = reference_lane.radius + (num - 1) * width
else:
radius = reference_lane.radius - (num - 1) * width
symmetric_lane = CircularLane(reference_lane.center, radius,
start_phase, end_phase, clockwise, width,
reference_lane.line_types,
reference_lane.forbidden,
reference_lane.speed_limit,
reference_lane.priority)
else:
raise ValueError("Creating other lanes is not supported")
success = CreateRoadFrom(
symmetric_lane,
int(num / 2),
adverse_road,
roadnet_to_get_road,
roadnet_to_check_cross,
ignore_start=ignore_start,
ignore_end=ignore_end,
side_lane_line_type=side_lane_line_type,
inner_lane_line_type=inner_lane_line_type,
center_line_type=center_line_type,
center_line_color=center_line_color,
ignore_intersection_checking=ignore_intersection_checking)
positive_road.get_lanes(roadnet_to_get_road)[0].line_color = [
center_line_color, LineColor.GREY
]
return success
def _get_merge_part(self, side_lane: StraightLane):
tool_lane = StraightLane(side_lane.end, side_lane.start, side_lane.width)
merge_part, _ = create_bend_straight(
tool_lane,
10,
self.lane_width / 2,
np.pi / 2,
True,
width=self.lane_width,
line_types=(LineType.CONTINUOUS, LineType.BROKEN)
)
return merge_part
def create_bend_straight(previous_lane: "StraightLane",
following_lane_length,
radius: float,
angle: float,
clockwise: bool = True,
width: float = AbstractLane.DEFAULT_WIDTH,
line_types: Tuple[LineType, LineType] = None,
forbidden: bool = False,
speed_limit: float = 20,
priority: int = 0):
bend_direction = 1 if clockwise else -1
center = previous_lane.position(previous_lane.length,
bend_direction * radius)
p_lateral = previous_lane.direction_lateral
x, y = p_lateral
start_phase = 0
if y == 0:
start_phase = 0 if x < 0 else -np.pi
elif x == 0:
start_phase = np.pi / 2 if y < 0 else -np.pi / 2
else:
base_angel = np.arctan(y / x)
if x < 0:
start_phase = base_angel
elif y < 0:
start_phase = np.pi + base_angel
elif y > 0:
start_phase = -np.pi + base_angel
end_phase = start_phase + angle
if not clockwise:
start_phase = start_phase - np.pi
end_phase = start_phase - angle
bend = CircularLane(center, radius, start_phase, end_phase, clockwise,
width, line_types, forbidden, speed_limit, priority)
length = 2 * radius * angle / 2
bend_end = bend.position(length, 0)
next_lane_heading = get_vertical_vector(bend_end - center)
nxt_dir = next_lane_heading[0] if not clockwise else next_lane_heading[1]
nxt_dir = np.asarray(nxt_dir)
following_lane_end = nxt_dir * following_lane_length + bend_end
following_lane = StraightLane(bend_end, following_lane_end, width,
line_types, forbidden, speed_limit, priority)
return bend, following_lane
def _try_plug_into_previous_block(self) -> bool:
acc_lane_len = self.get_config()[Parameter.length]
no_cross = True
# extend road and acc raod part, part 0
self.set_part_idx(0)
sin_angle = math.sin(np.deg2rad(self.ANGLE))
cos_angle = math.cos(np.deg2rad(self.ANGLE))
longitude_len = sin_angle * self.RADIUS * 2 + cos_angle * self.CONNECT_PART_LEN + self.RAMP_LEN
extend_lane = ExtendStraightLane(
self.positive_basic_lane, longitude_len + self.EXTRA_PART, [LineType.BROKEN, LineType.CONTINUOUS]
)
extend_road = Road(self.pre_block_socket.positive_road.end_node, self.add_road_node())
no_cross = CreateRoadFrom(
extend_lane,
self.positive_lane_num,
extend_road,
self.block_network,
self._global_network,
side_lane_line_type=LineType.CONTINUOUS,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
extend_road.get_lanes(self.block_network)[-1].line_types = [
LineType.BROKEN if self.positive_lane_num != 1 else LineType.CONTINUOUS, LineType.CONTINUOUS
]
no_cross = CreateAdverseRoad(
extend_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
_extend_road = -extend_road
left_lane_line = LineType.NONE if self.positive_lane_num == 1 else LineType.BROKEN
_extend_road.get_lanes(self.block_network)[-1].line_types = [left_lane_line, LineType.SIDE]
# main acc part
acc_side_lane = ExtendStraightLane(
extend_lane, acc_lane_len + self.lane_width, [extend_lane.line_types[0], LineType.SIDE]
)
acc_road = Road(extend_road.end_node, self.add_road_node())
no_cross = CreateRoadFrom(
acc_side_lane,
self.positive_lane_num,
acc_road,
self.block_network,
self._global_network,
side_lane_line_type=LineType.CONTINUOUS,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
no_cross = CreateAdverseRoad(
acc_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
left_line_type = LineType.CONTINUOUS if self.positive_lane_num == 1 else LineType.BROKEN
acc_road.get_lanes(self.block_network)[-1].line_types = [left_line_type, LineType.BROKEN]
# socket part
socket_side_lane = ExtendStraightLane(acc_side_lane, self.SOCKET_LEN, acc_side_lane.line_types)
socket_road = Road(acc_road.end_node, self.add_road_node())
no_cross = CreateRoadFrom(
socket_side_lane,
self.positive_lane_num,
socket_road,
self.block_network,
self._global_network,
side_lane_line_type=LineType.CONTINUOUS,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
no_cross = CreateAdverseRoad(
socket_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
self.add_sockets(PGBlock.create_socket_from_positive_road(socket_road))
# ramp part, part 1
self.set_part_idx(1)
lateral_dist = (1 - cos_angle) * self.RADIUS * 2 + sin_angle * self.CONNECT_PART_LEN
end_point = extend_lane.position(self.EXTRA_PART + self.RAMP_LEN, lateral_dist + self.lane_width)
start_point = extend_lane.position(self.EXTRA_PART, lateral_dist + self.lane_width)
straight_part = StraightLane(
start_point, end_point, self.lane_width, self.LANE_TYPE, speed_limit=self.SPEED_LIMIT
)
straight_road = Road(self.add_road_node(), self.add_road_node())
self.block_network.add_lane(straight_road.start_node, straight_road.end_node, straight_part)
no_cross = (
not check_lane_on_road(
self._global_network,
straight_part,
0.95,
ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
self.add_respawn_roads(straight_road)
# p1 road 0, 1
bend_1, connect_part = create_bend_straight(
straight_part,
self.CONNECT_PART_LEN,
self.RADIUS,
np.deg2rad(self.ANGLE),
False,
self.lane_width,
self.LANE_TYPE,
speed_limit=self.SPEED_LIMIT
)
bend_1_road = Road(straight_road.end_node, self.add_road_node())
connect_road = Road(bend_1_road.end_node, self.add_road_node())
self.block_network.add_lane(bend_1_road.start_node, bend_1_road.end_node, bend_1)
self.block_network.add_lane(connect_road.start_node, connect_road.end_node, connect_part)
no_cross = (
not check_lane_on_road(
self._global_network, bend_1, 0.95, ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
no_cross = (
not check_lane_on_road(
self._global_network,
connect_part,
0.95,
ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
# p1, road 2, 3
bend_2, acc_lane = create_bend_straight(
connect_part,
acc_lane_len,
self.RADIUS,
np.deg2rad(self.ANGLE),
True,
self.lane_width,
self.LANE_TYPE,
speed_limit=self.SPEED_LIMIT
)
acc_lane.line_types = [LineType.BROKEN, LineType.CONTINUOUS]
bend_2_road = Road(connect_road.end_node, self.road_node(0, 0)) # end at part1 road 0, extend road
self.block_network.add_lane(bend_2_road.start_node, bend_2_road.end_node, bend_2)
self.block_network.add_lane(acc_road.start_node, acc_road.end_node, acc_lane)
no_cross = (
not check_lane_on_road(
self._global_network, bend_2, 0.95, ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
no_cross = (
not check_lane_on_road(
self._global_network, acc_lane, 0.95, ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
# p1, road 4, small circular to decorate
merge_lane, _ = create_bend_straight(
acc_lane, 10, self.lane_width / 2, np.pi / 2, False, self.lane_width,
(LineType.BROKEN, LineType.CONTINUOUS)
)
self.block_network.add_lane(Decoration.start, Decoration.end, merge_lane)
return no_cross
def _get_deacc_lane(self, att_lane: StraightLane):
start = att_lane.position(self.lane_width, self.lane_width)
end = att_lane.position(att_lane.length, self.lane_width)
return StraightLane(start, end, self.lane_width, (LineType.BROKEN, LineType.CONTINUOUS))
def __init__(self,
global_network: RoadNetwork,
lane_width: float,
lane_num: int,
render_root_np: NodePath,
physics_world: PhysicsWorld,
length: float = 50,
ignore_intersection_checking=False):
place_holder = PGBlockSocket(Road(Decoration.start, Decoration.end),
Road(Decoration.start, Decoration.end))
super(FirstPGBlock, self).__init__(
0,
place_holder,
global_network,
random_seed=0,
ignore_intersection_checking=ignore_intersection_checking)
assert length > self.ENTRANCE_LENGTH, (length, self.ENTRANCE_LENGTH)
self._block_objects = []
basic_lane = StraightLane(
[0, lane_width * (lane_num - 1)],
[self.ENTRANCE_LENGTH, lane_width * (lane_num - 1)],
line_types=(LineType.BROKEN, LineType.SIDE),
width=lane_width)
ego_v_spawn_road = Road(self.NODE_1, self.NODE_2)
CreateRoadFrom(
basic_lane,
lane_num,
ego_v_spawn_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking)
CreateAdverseRoad(
ego_v_spawn_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking)
next_lane = ExtendStraightLane(basic_lane,
length - self.ENTRANCE_LENGTH,
[LineType.BROKEN, LineType.SIDE])
other_v_spawn_road = Road(self.NODE_2, self.NODE_3)
CreateRoadFrom(
next_lane,
lane_num,
other_v_spawn_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking)
CreateAdverseRoad(
other_v_spawn_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking)
self._create_in_world()
# global_network += self.block_network
global_network.add(self.block_network)
socket = self.create_socket_from_positive_road(other_v_spawn_road)
socket.set_index(self.name, 0)
self.add_sockets(socket)
self.attach_to_world(render_root_np, physics_world)
self._respawn_roads = [other_v_spawn_road]
def _create_circular_part(self, road: Road, part_idx: int,
radius_exit: float, radius_inner: float,
angle: float) -> (str, str, StraightLane, bool):
"""
Create a part of roundabout according to a straight road
"""
none_cross = True
self.set_part_idx(part_idx)
radius_big = (self.positive_lane_num * 2 -
1) * self.lane_width + radius_inner
# circular part 0
segment_start_node = road.end_node
segment_end_node = self.add_road_node()
segment_road = Road(segment_start_node, segment_end_node)
lanes = road.get_lanes(
self._global_network) if part_idx == 0 else road.get_lanes(
self.block_network)
right_lane = lanes[-1]
bend, straight = create_bend_straight(right_lane, 10, radius_exit,
np.deg2rad(angle), True,
self.lane_width,
(LineType.BROKEN, LineType.SIDE))
ignore_last_2_part_start = self.road_node((part_idx + 3) % 4, 0)
ignore_last_2_part_end = self.road_node((part_idx + 3) % 4, 0)
none_cross = CreateRoadFrom(
bend,
self.positive_lane_num,
segment_road,
self.block_network,
self._global_network,
ignore_start=ignore_last_2_part_start,
ignore_end=ignore_last_2_part_end,
ignore_intersection_checking=self.ignore_intersection_checking
) and none_cross
# set circular part 0 visualization
for k, lane in enumerate(segment_road.get_lanes(self.block_network)):
if k == self.positive_lane_num - 1:
lane.line_types = [LineType.NONE, LineType.SIDE]
else:
lane.line_types = [LineType.NONE, LineType.NONE]
# circular part 1
tool_lane_start = straight.position(-5, 0)
tool_lane_end = straight.position(0, 0)
tool_lane = StraightLane(tool_lane_start, tool_lane_end)
bend, straight_to_next_iter_part = create_bend_straight(
tool_lane, 10, radius_big, np.deg2rad(2 * angle - 90), False,
self.lane_width, (LineType.BROKEN, LineType.SIDE))
segment_start_node = segment_end_node
segment_end_node = self.add_road_node()
segment_road = Road(segment_start_node, segment_end_node)
none_cross = CreateRoadFrom(
bend,
self.positive_lane_num,
segment_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking
) and none_cross
self.intermediate_spawn_places.append(
segment_road.get_lanes(self.block_network))
# circular part 2 and exit straight part
length = self.EXIT_PART_LENGTH
tool_lane_start = straight_to_next_iter_part.position(-5, 0)
tool_lane_end = straight_to_next_iter_part.position(0, 0)
tool_lane = StraightLane(tool_lane_start, tool_lane_end)
bend, straight = create_bend_straight(tool_lane, length, radius_exit,
np.deg2rad(angle), True,
self.lane_width,
(LineType.BROKEN, LineType.SIDE))
segment_start_node = segment_end_node
segment_end_node = self.add_road_node(
) if part_idx < 3 else self.pre_block_socket.negative_road.start_node
segment_road = Road(segment_start_node, segment_end_node)
none_cross = CreateRoadFrom(
bend,
self.positive_lane_num,
segment_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking
) and none_cross
# set circular part 2 (curve) visualization
for k, lane in enumerate(segment_road.get_lanes(self.block_network)):
if k == self.positive_lane_num - 1:
lane.line_types = [LineType.NONE, LineType.SIDE]
else:
lane.line_types = [LineType.NONE, LineType.NONE]
exit_start = segment_end_node
exit_end = self.add_road_node()
segment_road = Road(exit_start, exit_end)
if part_idx < 3:
none_cross = CreateRoadFrom(
straight,
self.positive_lane_num,
segment_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking
) and none_cross
self.add_sockets(
self.create_socket_from_positive_road(segment_road))
# add circular part 3 at last
segment_start = self.road_node(part_idx, 1)
segment_end = self.road_node((part_idx + 1) % 4, 0)
segment_road = Road(segment_start, segment_end)
tool_lane_start = straight_to_next_iter_part.position(-6, 0)
tool_lane_end = straight_to_next_iter_part.position(0, 0)
tool_lane = StraightLane(tool_lane_start, tool_lane_end)
beneath = (self.positive_lane_num * 2 -
1) * self.lane_width / 2 + radius_exit
cos = math.cos(np.deg2rad(angle))
radius_this_seg = beneath / cos - radius_exit
bend, _ = create_bend_straight(tool_lane, 5, radius_this_seg,
np.deg2rad(180 - 2 * angle), False,
self.lane_width,
(LineType.BROKEN, LineType.SIDE))
CreateRoadFrom(
bend,
self.positive_lane_num,
segment_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking)
# set circular part 2 visualization
for k, lane in enumerate(segment_road.get_lanes(self.block_network)):
if k == 0:
if self.positive_lane_num > 1:
lane.line_types = [LineType.CONTINUOUS, LineType.BROKEN]
else:
lane.line_types = [LineType.CONTINUOUS, LineType.NONE]
else:
lane.line_types = [LineType.BROKEN, LineType.BROKEN]
return Road(exit_start, exit_end), none_cross
def CreateTwoWayRoad(
road_to_change: "Road",
roadnet_to_get_road: "RoadNetwork", # mostly, block network
roadnet_to_check_cross: "RoadNetwork", # mostly, previous global network
new_road_name: Road = None,
ignore_start: str = None,
ignore_end: str = None,
center_line_type=LineType.
CONTINUOUS, # Identical to Block.CENTER_LINE_TYPE
side_lane_line_type=LineType.SIDE,
inner_lane_line_type=LineType.BROKEN,
ignore_intersection_checking=None) -> bool:
"""
This function will add a new road in reverse direction to the road network
Then the road will change from:
---------->
---------->
to:
<--------->
<--------->
As a result, vehicles can drive in both direction
:return: cross or not
"""
adverse_road = Road(
road_to_change.end_node,
road_to_change.start_node) if new_road_name is None else new_road_name
lanes = get_lanes_on_road(road_to_change, roadnet_to_get_road)
reference_lane = lanes[-1]
num = len(lanes)
width = reference_lane.width_at(0)
if isinstance(reference_lane, StraightLane):
start_point = reference_lane.position(lanes[-1].length,
-(num - 1) * width)
end_point = reference_lane.position(0, -(num - 1) * width)
symmetric_lane = StraightLane(start_point, end_point, width,
lanes[-1].line_types,
reference_lane.forbidden,
reference_lane.speed_limit,
reference_lane.priority)
elif isinstance(reference_lane, CircularLane):
start_phase = reference_lane.end_phase
end_phase = reference_lane.start_phase
clockwise = False if reference_lane.direction == 1 else True
if not clockwise:
radius = reference_lane.radius + (num - 1) * width
else:
radius = reference_lane.radius - (num - 1) * width
symmetric_lane = CircularLane(reference_lane.center, radius,
start_phase, end_phase, clockwise, width,
reference_lane.line_types,
reference_lane.forbidden,
reference_lane.speed_limit,
reference_lane.priority)
else:
raise ValueError("Creating other lanes is not supported")
success = CreateRoadFrom(
symmetric_lane,
num,
adverse_road,
roadnet_to_get_road,
roadnet_to_check_cross,
ignore_start=ignore_start,
ignore_end=ignore_end,
side_lane_line_type=side_lane_line_type,
inner_lane_line_type=inner_lane_line_type,
center_line_type=center_line_type,
ignore_intersection_checking=ignore_intersection_checking)
return success
def _try_plug_into_previous_block(self) -> bool:
acc_lane_len = self.get_config()[Parameter.length]
no_cross = True
fork_lane_num = 2
# extend road and acc raod part, part 0
self.set_part_idx(0)
sin_angle = math.sinsin(np.deg2rad(self.ANGLE))
cos_angle = math.cos(np.deg2rad(self.ANGLE))
longitude_len = sin_angle * self.RADIUS * 2 + cos_angle * self.CONNECT_PART_LEN + self.RAMP_LEN
extend_lane = ExtendStraightLane(
self.positive_basic_lane, longitude_len + self.EXTRA_PART,
[LineType.BROKEN, LineType.CONTINUOUS])
extend_road = Road(self.pre_block_socket.positive_road.end_node,
self.add_road_node())
no_cross = CreateRoadFrom(
extend_lane,
self.positive_lane_num,
extend_road,
self.block_network,
self._global_network,
side_lane_line_type=LineType.CONTINUOUS) and no_cross
no_cross = CreateAdverseRoad(extend_road, self.block_network,
self._global_network) and no_cross
acc_side_lane = ExtendStraightLane(
extend_lane, acc_lane_len + self.lane_width,
[LineType.BROKEN, LineType.CONTINUOUS])
acc_road = Road(extend_road.end_node, self.add_road_node())
no_cross = CreateRoadFrom(acc_side_lane, self.positive_lane_num,
acc_road, self.block_network,
self._global_network) and no_cross
no_cross = CreateAdverseRoad(acc_road, self.block_network,
self._global_network) and no_cross
acc_road.get_lanes(self.block_network)[-1].line_types = [
LineType.BROKEN, LineType.BROKEN
]
# ramp part, part 1
self.set_part_idx(1)
lateral_dist = (1 - cos_angle
) * self.RADIUS * 2 + sin_angle * self.CONNECT_PART_LEN
end_point = extend_lane.position(self.EXTRA_PART + self.RAMP_LEN,
lateral_dist + self.lane_width)
start_point = extend_lane.position(self.EXTRA_PART,
lateral_dist + self.lane_width)
straight_part = StraightLane(start_point,
end_point,
self.lane_width,
self.LANE_TYPE,
speed_limit=self.SPEED_LIMIT)
# p1 road 0, 1
bend_1, connect_part = create_bend_straight(
straight_part,
self.CONNECT_PART_LEN,
self.RADIUS,
np.deg2rad(self.ANGLE),
False,
self.lane_width,
self.LANE_TYPE,
speed_limit=self.SPEED_LIMIT)
bend_1_road = Road(self.add_road_node(), self.add_road_node())
self.add_respawn_roads(bend_1_road)
connect_road = Road(bend_1_road.end_node, self.add_road_node())
self.block_network.add_lane(bend_1_road.start_node,
bend_1_road.end_node, bend_1)
self.block_network.add_lane(connect_road.start_node,
connect_road.end_node, connect_part)
no_cross = CreateRoadFrom(bend_1, fork_lane_num, bend_1_road,
self.block_network, self._global_network,
False) and no_cross
no_cross = CreateRoadFrom(connect_part, fork_lane_num, connect_road,
self.block_network, self._global_network,
False) and no_cross
# p1, road 2, 3
bend_2, acc_lane = create_bend_straight(connect_part,
acc_lane_len + self.lane_width,
self.RADIUS,
np.deg2rad(self.ANGLE),
True,
self.lane_width,
self.LANE_TYPE,
speed_limit=self.SPEED_LIMIT)
acc_lane.line_types = [LineType.BROKEN, LineType.CONTINUOUS]
bend_2_road = Road(connect_road.end_node, self.road_node(
0, 0)) # end at part1 road 0, extend road
acc_road = Road(self.road_node(0, 0), self.road_node(0, 1))
self.block_network.add_lane(bend_2_road.start_node,
bend_2_road.end_node, bend_2)
no_cross = CreateRoadFrom(
bend_2,
fork_lane_num,
bend_2_road,
self.block_network,
self._global_network,
False,
inner_lane_line_type=LineType.BROKEN) and no_cross
no_cross = CreateRoadFrom(
acc_lane,
fork_lane_num,
acc_road,
self.block_network,
self._global_network,
False,
inner_lane_line_type=LineType.BROKEN,
) and no_cross
self.add_sockets(PGBlock.create_socket_from_positive_road(acc_road))
return no_cross