def _exclude_lanes(self):
para = self.get_config()
t_type = para[Parameter.t_intersection_type]
self.add_sockets(self.pre_block_socket)
start_node = self._sockets[PGBlockSocket.get_real_index(self.name, t_type)].negative_road.end_node
end_node = self._sockets[PGBlockSocket.get_real_index(self.name, t_type)].positive_road.start_node
for i in range(4):
if i == t_type:
continue
index_i = PGBlockSocket.get_real_index(self.name, i) if i < 3 else self.pre_block_socket_index
exit_node = self._sockets[index_i].positive_road.start_node if i != Goal.ADVERSE else self._sockets[
index_i].negative_road.start_node
pos_lanes = self.block_network.remove_all_roads(start_node, exit_node)
entry_node = self._sockets[index_i].negative_road.end_node if i != Goal.ADVERSE else self._sockets[
index_i].positive_road.end_node
neg_lanes = self.block_network.remove_all_roads(entry_node, end_node)
# if (i + 2) % 4 == t_type:
# # for vis only, solve a bug existing in a corner case,
# for lane in neg_lanes:
# lane.reset_start_end(lane.start, lane.position(lane.length / 2, 0))
# self.block_network.add_road(Road(Decoration.start, Decoration.end), neg_lanes)
#
# for lane in pos_lanes:
# lane.reset_start_end(lane.position(lane.length / 2, 0), lane.end)
# self.block_network.add_road(Road(Decoration.start, Decoration.end), pos_lanes)
self._change_vis(t_type)
self._sockets.pop(self.pre_block_socket.index)
socket = self._sockets.pop(PGBlockSocket.get_real_index(self.name, t_type))
self.block_network.remove_all_roads(socket.positive_road.start_node, socket.positive_road.end_node)
self.block_network.remove_all_roads(socket.negative_road.start_node, socket.negative_road.end_node)
self._respawn_roads.remove(socket.negative_road)
def _add_one_socket(self, socket: PGBlockSocket):
assert isinstance(socket, PGBlockSocket), "Socket list only accept BlockSocket Type"
# mute warning in T interection
# if socket.index is not None and not socket.index.startswith(self.name):
# logging.warning(
# "The adding socket has index {}, which is not started with this block name {}. This is dangerous! "
# "Current block has sockets: {}.".format(socket.index, self.name, self.get_socket_indices())
# )
if socket.index is None:
# if this socket is self block socket
socket.set_index(self.name, len(self._sockets))
self._sockets[socket.index] = socket
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 _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 _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]