def _create_left_turn(self, radius, lane_num, attach_left_lane,
attach_road, intersect_nodes, part_idx):
left_turn_radius = radius + lane_num * attach_left_lane.width_at(0)
diff = self.lane_num_intersect - self.positive_lane_num # increase lane num
if ((part_idx == 1 or part_idx == 3) and diff > 0) or (
(part_idx == 0 or part_idx == 2) and diff < 0):
diff = abs(diff)
left_bend, extra_part = create_bend_straight(
attach_left_lane, self.lane_width * diff, left_turn_radius,
np.deg2rad(self.ANGLE), False, attach_left_lane.width_at(0),
(LineType.NONE, LineType.NONE))
left_road_start = intersect_nodes[2]
pre_left_road_start = left_road_start + self.EXTRA_PART
CreateRoadFrom(
left_bend,
min(self.positive_lane_num, self.lane_num_intersect),
Road(attach_road.end_node, pre_left_road_start),
self.block_network,
self._global_network,
toward_smaller_lane_index=False,
center_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
CreateRoadFrom(
extra_part,
min(self.positive_lane_num, self.lane_num_intersect),
Road(pre_left_road_start, left_road_start),
self.block_network,
self._global_network,
toward_smaller_lane_index=False,
center_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
else:
left_bend, _ = create_bend_straight(attach_left_lane,
self.EXIT_PART_LENGTH,
left_turn_radius,
np.deg2rad(self.ANGLE), False,
attach_left_lane.width_at(0),
(LineType.NONE, LineType.NONE))
left_road_start = intersect_nodes[2]
CreateRoadFrom(
left_bend,
min(self.positive_lane_num, self.lane_num_intersect),
Road(attach_road.end_node, left_road_start),
self.block_network,
self._global_network,
toward_smaller_lane_index=False,
center_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
def _try_plug_into_previous_block(self) -> bool:
self.set_part_idx(
0) # only one part in simple block like straight, and curve
para = self.get_config()
length = para[Parameter.length]
basic_lane = self.positive_basic_lane
assert isinstance(
basic_lane,
StraightLane), "Straight road can only connect straight type"
new_lane = ExtendStraightLane(basic_lane, length,
[LineType.BROKEN, LineType.SIDE])
start = self.pre_block_socket.positive_road.end_node
end = self.add_road_node()
socket = Road(start, end)
_socket = -socket
# create positive road
no_cross = CreateRoadFrom(
new_lane,
self.positive_lane_num,
socket,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking)
# create negative road
no_cross = CreateAdverseRoad(socket,
self.block_network,
self._global_network,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
self.add_sockets(PGBlockSocket(socket, _socket))
return no_cross
def _create_u_turn(self, attach_road, part_idx):
# set to CONTINUOUS to debug
line_type = LineType.NONE
lanes = attach_road.get_lanes(
self.block_network) if part_idx != 0 else self.positive_lanes
attach_left_lane = lanes[0]
lane_num = len(lanes)
left_turn_radius = self.lane_width / 2
left_bend, _ = create_bend_straight(attach_left_lane,
0.1, left_turn_radius,
np.deg2rad(180), False,
attach_left_lane.width_at(0),
(LineType.NONE, LineType.NONE))
left_road_start = (-attach_road).start_node
CreateRoadFrom(
left_bend,
lane_num,
Road(attach_road.end_node, left_road_start),
self.block_network,
self._global_network,
toward_smaller_lane_index=False,
center_line_type=line_type,
side_lane_line_type=line_type,
inner_lane_line_type=line_type,
ignore_intersection_checking=self.ignore_intersection_checking)
def _create_part(self, attach_lanes, attach_road: Road, radius: float,
intersect_nodes: deque, part_idx) -> (StraightLane, bool):
lane_num = self.lane_num_intersect if part_idx == 0 or part_idx == 2 else self.positive_lane_num
non_cross = True
attach_left_lane = attach_lanes[0]
# first left part
assert isinstance(
attach_left_lane, StraightLane
), "Can't create a intersection following a circular lane"
self._create_left_turn(radius, lane_num, attach_left_lane, attach_road,
intersect_nodes, part_idx)
# u-turn
if self.enable_u_turn:
adverse_road = -attach_road
self._create_u_turn(attach_road, part_idx)
# go forward part
lanes_on_road = copy.deepcopy(attach_lanes)
straight_lane_len = 2 * radius + (2 * lane_num -
1) * lanes_on_road[0].width_at(0)
for l in lanes_on_road:
next_lane = ExtendStraightLane(l, straight_lane_len,
(LineType.NONE, LineType.NONE))
self.block_network.add_lane(attach_road.end_node,
intersect_nodes[1], next_lane)
# right part
length = self.EXIT_PART_LENGTH
right_turn_lane = lanes_on_road[-1]
assert isinstance(
right_turn_lane, StraightLane
), "Can't create a intersection following a circular lane"
right_bend, right_straight = create_bend_straight(
right_turn_lane, length, radius, np.deg2rad(self.ANGLE), True,
right_turn_lane.width_at(0), (LineType.NONE, LineType.SIDE))
non_cross = (not check_lane_on_road(
self._global_network,
right_bend,
1,
ignore_intersection_checking=self.ignore_intersection_checking)
) and non_cross
CreateRoadFrom(
right_bend,
min(self.positive_lane_num, self.lane_num_intersect),
Road(attach_road.end_node, intersect_nodes[0]),
self.block_network,
self._global_network,
toward_smaller_lane_index=True,
side_lane_line_type=LineType.SIDE,
inner_lane_line_type=LineType.NONE,
center_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
intersect_nodes.rotate(-1)
right_straight.line_types = [LineType.BROKEN, LineType.SIDE]
return right_straight, non_cross
def _try_plug_into_previous_block(self) -> bool:
para = self.get_config()
decrease_increase = -1 if para[Parameter.decrease_increase] == 0 else 1
if self.positive_lane_num <= 1:
decrease_increase = 1
elif self.positive_lane_num >= 4:
decrease_increase = -1
self.lane_num_intersect = self.positive_lane_num + decrease_increase * para[
Parameter.change_lane_num]
no_cross = True
attach_road = self.pre_block_socket.positive_road
_attach_road = self.pre_block_socket.negative_road
attach_lanes = attach_road.get_lanes(self._global_network)
# right straight left node name, rotate it to fit different part
intersect_nodes = deque([
self.road_node(0, 0),
self.road_node(1, 0),
self.road_node(2, 0), _attach_road.start_node
])
for i in range(4):
right_lane, success = self._create_part(attach_lanes, attach_road,
para[Parameter.radius],
intersect_nodes, i)
no_cross = no_cross and success
if i != 3:
lane_num = self.positive_lane_num if i == 1 else self.lane_num_intersect
exit_road = Road(self.road_node(i, 0), self.road_node(i, 1))
no_cross = CreateRoadFrom(
right_lane,
lane_num,
exit_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
no_cross = CreateAdverseRoad(
exit_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
socket = PGBlockSocket(exit_road, -exit_road)
self.add_respawn_roads(socket.negative_road)
self.add_sockets(socket)
attach_road = -exit_road
attach_lanes = attach_road.get_lanes(self.block_network)
return no_cross
def _try_plug_into_previous_block(self) -> bool:
self.set_part_idx(0) # only one part in simple block like straight, and curve
para = self.get_config()
length = para[Parameter.length]
self.BUILDING_LENGTH = length
basic_lane = self.positive_basic_lane
new_lane = ExtendStraightLane(basic_lane, length, [LineType.CONTINUOUS, LineType.SIDE])
start = self.pre_block_socket.positive_road.end_node
end = self.add_road_node()
socket = Road(start, end)
_socket = -socket
# create positive road
no_cross = CreateRoadFrom(
new_lane,
self.positive_lane_num,
socket,
self.block_network,
self._global_network,
center_line_color=LineColor.YELLOW,
center_line_type=LineType.CONTINUOUS,
inner_lane_line_type=LineType.CONTINUOUS,
side_lane_line_type=LineType.SIDE,
ignore_intersection_checking=self.ignore_intersection_checking
)
# create negative road
no_cross = CreateAdverseRoad(
socket,
self.block_network,
self._global_network,
center_line_color=LineColor.YELLOW,
center_line_type=LineType.CONTINUOUS,
inner_lane_line_type=LineType.CONTINUOUS,
side_lane_line_type=LineType.SIDE,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
self.add_sockets(PGBlockSocket(socket, _socket))
self._add_building_and_speed_limit(socket)
self._add_building_and_speed_limit(_socket)
return no_cross
def _try_plug_into_previous_block(self) -> bool:
no_cross = True
parameters = self.get_config()
self.BOTTLENECK_LEN = parameters["bottle_len"]
lane_num_changed = parameters[Parameter.lane_num]
start_ndoe = self.pre_block_socket.positive_road.end_node
straight_lane_num = int(self.positive_lane_num - lane_num_changed)
straight_lane_num = max(1, straight_lane_num)
circular_lane_num = self.positive_lane_num - straight_lane_num
# part 1, straight road 0
basic_lane = self.positive_lanes[straight_lane_num - 1]
ref_lane = ExtendStraightLane(basic_lane, self.BOTTLENECK_LEN,
[LineType.NONE, LineType.NONE])
straight_road = Road(start_ndoe, self.road_node(0, 0))
no_cross = CreateRoadFrom(ref_lane,
straight_lane_num,
straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.CONTINUOUS,
side_lane_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
no_cross = CreateAdverseRoad(straight_road,
self.block_network,
self._global_network,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
center_line_type=LineType.CONTINUOUS,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
# extend for socket ,part 1 road 1
ref_lane = ExtendStraightLane(ref_lane, parameters[Parameter.length],
[LineType.NONE, LineType.NONE])
socket_road = Road(self.road_node(0, 0), self.road_node(0, 1))
no_cross = CreateRoadFrom(ref_lane,
straight_lane_num,
socket_road,
self.block_network,
self._global_network,
center_line_type=LineType.CONTINUOUS,
side_lane_line_type=LineType.SIDE,
inner_lane_line_type=LineType.BROKEN,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
no_cross = CreateAdverseRoad(socket_road,
self.block_network,
self._global_network,
inner_lane_line_type=LineType.BROKEN,
side_lane_line_type=LineType.SIDE,
center_line_type=LineType.CONTINUOUS,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
negative_sockect_road = -socket_road
self.add_sockets(PGBlockSocket(socket_road, negative_sockect_road))
# part 2, circular part
for index, lane in enumerate(self.positive_lanes[straight_lane_num:],
1):
lateral_dist = index * self.lane_width / 2
inner_node = self.road_node(1, index)
side_line_type = LineType.SIDE if index == self.positive_lane_num - straight_lane_num else LineType.NONE
# positive part
circular_1, circular_2, _ = create_wave_lanes(
lane, lateral_dist, self.BOTTLENECK_LEN, 5, self.lane_width)
road_1 = Road(start_ndoe, inner_node)
no_cross = CreateRoadFrom(
circular_1,
1,
road_1,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
side_lane_line_type=side_line_type,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
road_2 = Road(inner_node, self.road_node(0, 0))
no_cross = CreateRoadFrom(
circular_2,
1,
road_2,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
side_lane_line_type=side_line_type,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
# adverse part
lane = negative_sockect_road.get_lanes(self.block_network)[-1]
circular_2, circular_1, _ = create_wave_lanes(
lane, lateral_dist, self.BOTTLENECK_LEN, 5, self.lane_width,
False)
road_2 = -road_2
no_cross = CreateRoadFrom(
circular_2,
1,
road_2,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
side_lane_line_type=side_line_type,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
road_1 = -road_1
no_cross = CreateRoadFrom(
circular_1,
1,
road_1,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
side_lane_line_type=side_line_type,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
return no_cross
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 _try_plug_into_previous_block(self) -> bool:
no_cross = True
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 + self.EXTRA_LEN
self.set_part_idx(0)
# part 0 road 0
dec_lane_len = self.get_config()[Parameter.length]
dec_lane = ExtendStraightLane(
self.positive_basic_lane, dec_lane_len + self.lane_width,
[self.positive_basic_lane.line_types[0], LineType.SIDE]
)
dec_road = Road(self.pre_block_socket.positive_road.end_node, self.add_road_node())
no_cross = CreateRoadFrom(
dec_lane,
self.positive_lane_num,
dec_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(
dec_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
dec_right_lane = dec_road.get_lanes(self.block_network)[-1]
left_line_type = LineType.CONTINUOUS if self.positive_lane_num == 1 else LineType.BROKEN
dec_right_lane.line_types = [left_line_type, LineType.NONE]
# part 0 road 1
extend_lane = ExtendStraightLane(
dec_right_lane, longitude_len, [dec_right_lane.line_types[0], LineType.CONTINUOUS]
)
extend_road = Road(dec_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
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
_extend_road.get_lanes(self.block_network)[-1].line_types = [
LineType.NONE if self.positive_lane_num == 1 else LineType.BROKEN, LineType.SIDE
]
self.add_sockets(self.create_socket_from_positive_road(extend_road))
# part 1 road 0
self.set_part_idx(1)
dec_side_right_lane = self._get_deacc_lane(dec_right_lane)
self.block_network.add_lane(dec_road.start_node, dec_road.end_node, dec_side_right_lane)
no_cross = (
not check_lane_on_road(
self._global_network,
dec_side_right_lane,
0.95,
ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
bend_1, connect_part = create_bend_straight(
dec_side_right_lane,
self.CONNECT_PART_LEN,
self.RADIUS,
np.deg2rad(self.ANGLE),
True,
self.lane_width,
self.LANE_TYPE,
speed_limit=self.SPEED_LIMIT
)
bend_1_road = Road(dec_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
bend_2, straight_part = create_bend_straight(
connect_part,
self.RAMP_LEN,
self.RADIUS,
np.deg2rad(self.ANGLE),
False,
self.lane_width,
self.LANE_TYPE,
speed_limit=self.SPEED_LIMIT
)
bend_2_road = Road(connect_road.end_node, self.add_road_node()) # end at part1 road 0, extend road
straight_road = Road(bend_2_road.end_node, self.add_road_node())
self.block_network.add_lane(bend_2_road.start_node, bend_2_road.end_node, bend_2)
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, bend_2, 0.95, ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
no_cross = (
not check_lane_on_road(
self._global_network,
straight_part,
0.95,
ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
decoration_part = self._get_merge_part(dec_side_right_lane)
self.block_network.add_lane(Decoration.start, Decoration.end, decoration_part)
return no_cross
def _try_plug_into_previous_block(self) -> bool:
self.spawn_roads = []
self.dest_roads = []
no_cross = True
para = self.get_config()
assert self.positive_lane_num == 1, "Lane number of previous block must be 1 in each direction"
self.parking_space_length = para[Parameter.length]
self.parking_space_width = self.lane_width
parking_space_num = para[Parameter.one_side_vehicle_num]
# parking_space_num = 10
radius = para[Parameter.radius]
main_straight_road_length = 2 * radius + (parking_space_num -
1) * self.parking_space_width
main_lane = ExtendStraightLane(self.positive_lanes[0],
main_straight_road_length,
[LineType.BROKEN, LineType.NONE])
road = Road(self.pre_block_socket.positive_road.end_node,
self.road_node(0, 0))
# main straight part
no_cross = CreateRoadFrom(main_lane,
self.positive_lane_num,
road,
self.block_network,
self._global_network,
center_line_type=LineType.BROKEN,
inner_lane_line_type=LineType.BROKEN,
side_lane_line_type=LineType.NONE,
center_line_color=LineColor.GREY,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
no_cross = CreateAdverseRoad(road,
self.block_network,
self._global_network,
center_line_type=LineType.BROKEN,
inner_lane_line_type=LineType.BROKEN,
side_lane_line_type=LineType.NONE,
center_line_color=LineColor.GREY,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
# socket part
parking_lot_out_lane = ExtendStraightLane(
main_lane, self.SOCKET_LENGTH, [LineType.BROKEN, LineType.NONE])
parking_lot_out_road = Road(self.road_node(0, 0), self.road_node(0, 1))
# out socket part
no_cross = CreateRoadFrom(parking_lot_out_lane,
self.positive_lane_num,
parking_lot_out_road,
self.block_network,
self._global_network,
center_line_type=LineType.BROKEN,
inner_lane_line_type=LineType.BROKEN,
side_lane_line_type=LineType.SIDE,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
no_cross = CreateAdverseRoad(parking_lot_out_road,
self.block_network,
self._global_network,
center_line_type=LineType.BROKEN,
inner_lane_line_type=LineType.BROKEN,
side_lane_line_type=LineType.SIDE,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
socket = self.create_socket_from_positive_road(parking_lot_out_road)
self.add_sockets(socket)
# add parking space
for i in range(int(parking_space_num)):
no_cross = self._add_one_parking_space(
copy.copy(self.get_socket_list()[0]).get_socket_in_reverse(),
self.pre_block_socket.get_socket_in_reverse(), i + 1, radius,
i * self.parking_space_width, (parking_space_num - i - 1) *
self.parking_space_width) and no_cross
for i in range(parking_space_num, 2 * parking_space_num):
index = i + 1
i -= parking_space_num
no_cross = self._add_one_parking_space(
self.pre_block_socket, copy.copy(self.get_socket_list()[0]),
index, radius, i * self.parking_space_width,
(parking_space_num - i - 1) *
self.parking_space_width) and no_cross
return no_cross
def _add_one_parking_space(self, in_socket: PGBlockSocket,
out_socket: PGBlockSocket, part_idx: int,
radius, dist_to_in, dist_to_out) -> bool:
no_cross = True
# lane into parking space and parking space, 1
if in_socket.is_same_socket(
self.pre_block_socket) or in_socket.is_same_socket(
self.pre_block_socket.get_socket_in_reverse()):
net = self._global_network
else:
net = self.block_network
in_lane = in_socket.positive_road.get_lanes(net)[0]
start_node = in_socket.positive_road.end_node
if dist_to_in > 1e-3:
# a straight part will be added
in_lane = ExtendStraightLane(in_lane, dist_to_in,
[LineType.NONE, LineType.NONE])
in_road = Road(in_socket.positive_road.end_node,
self.road_node(part_idx, 0))
CreateRoadFrom(
in_lane,
self.positive_lane_num,
in_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
start_node = self.road_node(part_idx, 0)
bend, straight = create_bend_straight(in_lane,
self.parking_space_length,
radius, self.ANGLE, True,
self.parking_space_width)
bend_road = Road(start_node, self.road_node(part_idx, 1))
bend_no_cross = CreateRoadFrom(
bend,
self.positive_lane_num,
bend_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.SIDE
if dist_to_in < 1e-3 else LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
if dist_to_in < 1e-3:
no_cross = no_cross and bend_no_cross
straight_road = Road(self.road_node(part_idx, 1),
self.road_node(part_idx, 2))
self.dest_roads.append(straight_road)
no_cross = no_cross and CreateRoadFrom(
straight,
self.positive_lane_num,
straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.CONTINUOUS,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.SIDE
if dist_to_in < 1e-3 else LineType.NONE,
center_line_color=LineColor.GREY,
ignore_intersection_checking=self.ignore_intersection_checking)
# lane into parking space and parking space, 2
neg_road: Road = out_socket.negative_road
if out_socket.is_same_socket(
self.pre_block_socket) or out_socket.is_same_socket(
self.pre_block_socket.get_socket_in_reverse()):
net = self._global_network
else:
net = self.block_network
neg_lane = \
neg_road.get_lanes(net)[0]
start_node = neg_road.end_node
if dist_to_out > 1e-3:
# a straight part will be added
neg_lane = ExtendStraightLane(neg_lane, dist_to_out,
[LineType.NONE, LineType.NONE])
neg_road = Road(neg_road.end_node, self.road_node(part_idx, 3))
CreateRoadFrom(
neg_lane,
self.positive_lane_num,
neg_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
start_node = self.road_node(part_idx, 3)
bend, straight = create_bend_straight(neg_lane, self.lane_width,
radius, self.ANGLE, False,
self.parking_space_width)
bend_road = Road(start_node, self.road_node(part_idx, 4))
CreateRoadFrom(
bend,
self.positive_lane_num,
bend_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
straight_road = Road(self.road_node(part_idx, 4),
self.road_node(part_idx, 1))
CreateRoadFrom(
straight,
self.positive_lane_num,
straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
# give it a new road name to make it be a two way road (1,2) = (5,6) = parking space !
parking_road = Road(self.road_node(part_idx, 5),
self.road_node(part_idx, 6))
self.spawn_roads.append(parking_road)
CreateTwoWayRoad(
Road(self.road_node(part_idx, 1), self.road_node(part_idx, 2)),
self.block_network,
self._global_network,
parking_road,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.SIDE
if dist_to_out < 1e-3 else LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
# out part
parking_lane = parking_road.get_lanes(self.block_network)[0]
# out part 1
bend, straight = create_bend_straight(
parking_lane, 0.1 if dist_to_out < 1e-3 else dist_to_out, radius,
self.ANGLE, True, parking_lane.width)
out_bend_road = Road(
self.road_node(part_idx, 6),
self.road_node(part_idx, 7)
if dist_to_out > 1e-3 else out_socket.positive_road.start_node)
bend_success = CreateRoadFrom(
bend,
self.positive_lane_num,
out_bend_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.SIDE
if dist_to_out < 1e-3 else LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
if dist_to_out < 1e-3:
no_cross = no_cross and bend_success
if dist_to_out > 1e-3:
out_straight_road = Road(self.road_node(part_idx, 7),
out_socket.positive_road.start_node)
no_cross = no_cross and CreateRoadFrom(
straight,
self.positive_lane_num,
out_straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
# out part 2
extend_lane = ExtendStraightLane(parking_lane, self.lane_width,
[LineType.NONE, LineType.NONE])
CreateRoadFrom(
extend_lane,
self.positive_lane_num,
Road(self.road_node(part_idx, 6), self.road_node(part_idx, 8)),
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
bend, straight = create_bend_straight(
extend_lane, 0.1 if dist_to_in < 1e-3 else dist_to_in, radius,
self.ANGLE, False, parking_lane.width)
out_bend_road = Road(
self.road_node(part_idx, 8),
self.road_node(part_idx, 9)
if dist_to_in > 1e-3 else in_socket.negative_road.start_node)
CreateRoadFrom(
bend,
self.positive_lane_num,
out_bend_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
if dist_to_in > 1e-3:
out_straight_road = Road(self.road_node(part_idx, 9),
in_socket.negative_road.start_node)
CreateRoadFrom(
straight,
self.positive_lane_num,
out_straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking)
return no_cross
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 _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