Configurable exits angle in intersection

pgdrive/pg_config/parameter_space.py

@@ -17,6 +17,9 @@ class Parameter:
     lane_num = "lane_num"
     change_lane_num = "change_lane_num"
     decrease_increase = "decrease_increase"
+    left_exit_angle = "left_exit_angle"
+    right_exit_angle = "right_exit_angle"
+    forward_exit_angle = "forward_exit_angle"
 
     # vehicle
     vehicle_length = "v_len"
@@ -87,6 +90,14 @@ class BlockParameterSpace:
         Parameter.change_lane_num: PGDiscreteSpace(number=2),  # 0, 1
         Parameter.decrease_increase: PGDiscreteSpace(number=2)  # 0, decrease, 1 increase
     }
+    INTERSECTION_V2 = {
+        Parameter.radius: PGConstantSpace(10),
+        Parameter.change_lane_num: PGDiscreteSpace(number=2),  # 0, 1
+        Parameter.decrease_increase: PGDiscreteSpace(number=2),  # 0, decrease, 1 increase
+        Parameter.left_exit_angle: PGBoxSpace(min=45, max=135),
+        Parameter.right_exit_angle: PGBoxSpace(min=45, max=135),
+        Parameter.forward_exit_angle: PGBoxSpace(min=45, max=135)
+    }
     ROUNDABOUT = {
         Parameter.radius_exit: PGBoxSpace(min=5, max=15),  # TODO Should we reduce this?
         Parameter.radius_inner: PGBoxSpace(min=15, max=45),  # TODO Should we reduce this?
@@ -105,3 +116,11 @@ class BlockParameterSpace:
         Parameter.length: PGBoxSpace(min=20, max=40),  # accelerate/decelerate part length
         Parameter.lane_num: PGDiscreteSpace(2)
     }
+    T_INTERSECTION_V2 = {
+        Parameter.radius: PGConstantSpace(10),
+        Parameter.t_intersection_type: PGDiscreteSpace(number=3),  # 3 different t type for previous socket
+        Parameter.change_lane_num: PGDiscreteSpace(2),  # 0,1
+        Parameter.decrease_increase: PGDiscreteSpace(2),  # 0, decrease, 1 increase
+        Parameter.left_exit_angle: PGBoxSpace(min=45, max=135),
+        Parameter.right_exit_angle: PGBoxSpace(min=45, max=135),
+    }

pgdrive/scene_creator/blocks/intersection.py

@@ -60,6 +60,10 @@ def _try_plug_into_previous_block(self) -> bool:
              self.road_node(2, 0), _attach_road.start_node]
         )
 
+        # Consider a single intersection block. You have maximally 4 possible starting point to start driving:
+        # the lanes that facing west/east/north/south. So each "part" here means three set of lanes that all start at
+        # the starting point I mentioned above. For each part, there are a left set, a forward set and a right set of
+        # lanes. That's the reason the next line is a for loop with 4.
         for i in range(4):
             right_lane, success = self._create_part(
                 attach_lanes, attach_road, para[Parameter.radius], intersect_nodes, i
@@ -81,6 +85,9 @@ def _try_plug_into_previous_block(self) -> bool:
 
     def _create_part(self, attach_lanes, attach_road: Road, radius: float, intersect_nodes: deque,
                      part_idx) -> (StraightLane, bool):
+        """
+        Build one of the exits.
+        """
         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]
@@ -105,17 +112,8 @@ def _create_part(self, attach_lanes, attach_road: Road, radius: float, intersect
         )
 
         non_cross = (not check_lane_on_road(self._global_network, right_bend, 1)) 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
-        )
+        self._create_road_from(lane=right_bend, road=Road(attach_road.end_node, intersect_nodes[0]),
+                               toward_smaller_lane_index=True)
 
         intersect_nodes.rotate(-1)
         right_straight.line_types = [LineType.BROKEN, LineType.SIDE]
@@ -138,44 +136,144 @@ def _create_left_turn(self, radius, lane_num, attach_left_lane, attach_road, int
             )
             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
+            self._create_road_from(lane=left_bend, road=Road(attach_road.end_node, pre_left_road_start))
+            self._create_road_from(lane=extra_part, road=Road(pre_left_road_start, left_road_start))
+        else:
+            left_bend, _ = sharpbend(
+                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)
             )
+            self._create_road_from(lane=left_bend, road=Road(attach_road.end_node, intersect_nodes[2]))
+
+    def _create_road_from(self, lane, road, toward_smaller_lane_index=False):
+        CreateRoadFrom(
+            lane=lane,
+            lane_num=min(self.positive_lane_num, self.lane_num_intersect),
+            road=road,
+            roadnet_to_add_lanes=self.block_network,
+            roadnet_to_check_cross=self._global_network,
+            toward_smaller_lane_index=toward_smaller_lane_index,
+            center_line_type=LineType.NONE,
+            side_lane_line_type=LineType.NONE,
+            inner_lane_line_type=LineType.NONE
+        )
 
-            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
+
+class InterSectionV2(InterSection):
+    """
+    A intersection block that can varying the relative heading of three roads, rather a perfect + shape road network.
+    """
+
+    ID = "X"
+    EXTRA_PART = "extra"
+    PARAMETER_SPACE = PGSpace(BlockParameterSpace.INTERSECTION_V2)
+    SOCKET_NUM = 3
+    ANGLE = 90  # may support other angle in the future
+    EXIT_PART_LENGTH = 30
+
+    # LEFT_TURN_NUM = 1 now it is useless
+
+    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]
+        )
+
+        # Consider a single intersection block. You have maximally 4 possible starting point to start driving:
+        # the lanes that facing west/east/north/south. So each "part" here means three set of lanes that all start at
+        # the starting point I mentioned above. For each part, there are a left set, a forward set and a right set of
+        # lanes. That's the reason the next line is a for loop with 4.
+        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
+                ) and no_cross
+                no_cross = CreateAdverseRoad(exit_road, self.block_network, self._global_network) and no_cross
+                socket = BlockSocket(exit_road, -exit_road)
+                self.add_reborn_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 _create_part(self, attach_lanes, attach_road: Road, radius: float, intersect_nodes: deque,
+                     part_idx) -> (StraightLane, bool):
+        """
+        Build one of the exits.
+        """
+        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)
 
+        # 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 = sharpbend(
+            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)) and non_cross
+        self._create_road_from(lane=right_bend, road=Road(attach_road.end_node, intersect_nodes[0]),
+                               toward_smaller_lane_index=True)
+
+        intersect_nodes.rotate(-1)
+        right_straight.line_types = [LineType.BROKEN, LineType.SIDE]
+        return right_straight, non_cross
+
+    def get_socket(self, index: int) -> BlockSocket:
+        socket = super(InterSection, self).get_socket(index)
+        if socket.negative_road in self.get_reborn_roads():
+            self._reborn_roads.remove(socket.negative_road)
+        return socket
+
+    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 = sharpbend(
+                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
+            self._create_road_from(lane=left_bend, road=Road(attach_road.end_node, pre_left_road_start))
+            self._create_road_from(lane=extra_part, road=Road(pre_left_road_start, left_road_start))
         else:
             left_bend, _ = sharpbend(
                 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
-            )
+            self._create_road_from(lane=left_bend, road=Road(attach_road.end_node, left_road_start))

pgdrive/scene_creator/blocks/t_intersection.py

@@ -58,29 +58,29 @@ def _exclude_lanes(self):
         t_type = para[Parameter.t_intersection_type]
         self.add_sockets(self.pre_block_socket)
 
-        start_node = self._sockets[BlockSocket.get_real_index(self._block_name, t_type)].negative_road.end_node
-        end_node = self._sockets[BlockSocket.get_real_index(self._block_name, t_type)].positive_road.start_node
-        for i in range(4):
-            if i == t_type:
-                continue
-            index_i = BlockSocket.get_real_index(self._block_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)
-            # TODO small vis bug may raise in a corner case,
-            #  these code can fix it but will introduce a new get_closest_lane_index bug
-            # 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)
+        # start_node = self._sockets[BlockSocket.get_real_index(self._block_name, t_type)].negative_road.end_node
+        # end_node = self._sockets[BlockSocket.get_real_index(self._block_name, t_type)].positive_road.start_node
+        # for i in range(4):
+        #     if i == t_type:
+        #         continue
+        #     index_i = BlockSocket.get_real_index(self._block_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)
+        #     TODO small vis bug may raise in a corner case,
+        #      these code can fix it but will introduce a new get_closest_lane_index bug
+        #     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)