Merge and splits for ramp (#734)

* US Merge and splits for ramps

* Format

* Fix lines

* Format

metadrive/component/lane/extension_lane.py

@@ -0,0 +1,48 @@
+import numpy as np
+from enum import Enum
+
+from metadrive.component.lane.straight_lane import StraightLane
+
+
+class ExtensionDirection(Enum):
+    EXTEND = 0
+    SHRINK = 1
+
+
+class ExtendingLane(StraightLane):
+    def __init__(self, extension_direction: ExtensionDirection, *args, **kwargs):
+        super(ExtendingLane, self).__init__(*args, **kwargs)
+        self.extension_direction = extension_direction
+
+    def width_at(self, longitudinal: float) -> float:
+        if self.extension_direction == ExtensionDirection.EXTEND:
+            return (longitudinal / self.length) * self.width
+        else:
+            return self.width - (longitudinal / self.length) * self.width
+
+    def get_polyline(self, interval=2, lateral=0):
+        ret = []
+        for i in np.arange(0, self.length, interval):
+            max_lateral = self.width_at(i) - self.width / 2
+            ret.append(self.position(i, min(lateral, max_lateral)))
+        last_lateral = self.width_at(self.length) - self.width / 2
+        ret.append(self.position(self.length, min(lateral, last_lateral)))
+        return np.array(ret)
+
+    @property
+    def polygon(self):
+        if self._polygon is not None:
+            return self._polygon
+
+        polygon = []
+        longs = np.arange(0, self.length + self.POLYGON_SAMPLE_RATE, self.POLYGON_SAMPLE_RATE)
+        for longitude in longs:
+            point = self.position(longitude, -self.width / 2)
+            polygon.append([point[0], point[1]])
+        for longitude in longs[::-1]:
+            latitude = self.width_at(longitude) - self.width / 2
+            point = self.position(longitude, latitude)
+            polygon.append([point[0], point[1]])
+        self._polygon = np.asarray(polygon)
+
+        return self._polygon

metadrive/component/map/pg_map.py

@@ -143,7 +143,7 @@ def get_boundary_line_vector(self, interval):
                             continue
                         color = l.line_colors[side]
                         line_type = self.get_line_type(type, color)
-                        lateral = l.width_at(0) / 2
+                        lateral = l.width / 2
                         if side == 0:
                             lateral *= -1
                         ret["{}_{}".format(l.index, side)] = {

metadrive/component/pg_space.py

@@ -199,6 +199,7 @@ class Parameter:
     change_lane_num = "change_lane_num"
     decrease_increase = "decrease_increase"
     one_side_vehicle_num = "one_side_vehicle_number"
+    extension_length = "extension_length"
 
     # vehicle
     # vehicle_length = "v_len"
@@ -304,7 +305,8 @@ class BlockParameterSpace:
         Parameter.decrease_increase: DiscreteSpace(min=0, max=1)  # 0, decrease, 1 increase
     }
     RAMP_PARAMETER = {
-        Parameter.length: BoxSpace(min=20, max=40)  # accelerate/decelerate part length
+        Parameter.length: BoxSpace(min=20, max=40),  # accelerate/decelerate part length
+        Parameter.extension_length: BoxSpace(min=20, max=40)
     }
     FORK_PARAMETER = {
         Parameter.length: BoxSpace(min=20, max=40),  # accelerate/decelerate part length

metadrive/component/pgblock/create_pg_block_utils.py

@@ -8,6 +8,7 @@
 from metadrive.component.lane.pg_lane import PGLane
 from metadrive.component.lane.circular_lane import CircularLane
 from metadrive.component.lane.straight_lane import StraightLane
+from metadrive.component.lane.extension_lane import ExtendingLane, ExtensionDirection
 from metadrive.component.road_network import Road
 from metadrive.component.road_network.node_road_network import NodeRoadNetwork
 from metadrive.constants import PGLineType, PGLineColor, PGDrivableAreaProperty
@@ -16,6 +17,23 @@
 from metadrive.utils.pg.utils import check_lane_on_road
 
 
+def create_extension(
+    start_position: np.ndarray,
+    extension_lane_length: float,
+    direction: ExtensionDirection,
+    width: float = PGLane.DEFAULT_WIDTH,
+    forbidden: bool = False,
+    speed_limit: float = 20,
+    priority: int = 0
+):
+    extension_end = start_position + [extension_lane_length, 0.0]
+    extension = ExtendingLane(
+        direction, start_position, extension_end, width, [PGLineType.NONE, PGLineType.CONTINUOUS], forbidden,
+        speed_limit, priority
+    )
+    return extension
+
+
 def create_bend_straight(
     previous_lane: "StraightLane",
     following_lane_length,

metadrive/component/pgblock/ramp.py

@@ -3,8 +3,9 @@
 
 from metadrive.component.lane.straight_lane import StraightLane
 from metadrive.component.pgblock.create_pg_block_utils import ExtendStraightLane, CreateRoadFrom, CreateAdverseRoad, \
-    create_bend_straight
+    create_bend_straight, create_extension
 from metadrive.component.pgblock.pg_block import PGBlock
+from metadrive.component.lane.extension_lane import ExtensionDirection
 from metadrive.component.road_network import Road
 from metadrive.constants import Decoration, PGLineType
 from metadrive.utils.pg.utils import check_lane_on_road
@@ -14,13 +15,13 @@
 class Ramp(PGBlock):
     """
                     InRamp                                             OutRamp
-    
+
+     start ----------- end ------------------           start ----------- end ------------------
      start ----------- end ------------------           start ----------- end ------------------
-     start ----------- end ------------------           start ----------- end ------------------              
     (start ----------- end)[----------------]           start ----------- end [----------------]
-                       end -----------------}          (start ---------- {end) 
-                      //                                                      \\                                                         
-    { ---------------//                                                        \\---------------}    
+                       end -----------------}          (start ---------- {end)
+                      //                                                      \\
+    { ---------------//                                                        \\---------------}
     """
     PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.RAMP_PARAMETER)
     SOCKET_NUM = 1
@@ -39,8 +40,14 @@ class InRampOnStraight(Ramp):
     EXTRA_PART = 10
     SOCKET_LEN = 20
 
+    def _get_merge_part(self, att_lane: StraightLane, length: float):
+        start = att_lane.end
+        merge_part = create_extension(start, length, ExtensionDirection.SHRINK, width=self.lane_width)
+        return merge_part
+
     def _try_plug_into_previous_block(self) -> bool:
-        acc_lane_len = self.get_config()[Parameter.length]
+        length = self.get_config()[Parameter.length]
+        extension_length = self.get_config()[Parameter.extension_length]
         no_cross = True
 
         # extend road and acc raod part, part 0
@@ -77,7 +84,7 @@ def _try_plug_into_previous_block(self) -> bool:
 
         # main acc part
         acc_side_lane = ExtendStraightLane(
-            extend_lane, acc_lane_len + self.lane_width, [extend_lane.line_types[0], PGLineType.SIDE]
+            extend_lane, length + extension_length, [extend_lane.line_types[0], PGLineType.SIDE]
         )
         acc_road = Road(extend_road.end_node, self.add_road_node())
         no_cross = CreateRoadFrom(
@@ -170,7 +177,7 @@ def _try_plug_into_previous_block(self) -> bool:
         # p1, road 2, 3
         bend_2, acc_lane = create_bend_straight(
             connect_part,
-            acc_lane_len,
+            length,
             self.RADIUS,
             np.deg2rad(self.ANGLE),
             True,
@@ -194,10 +201,7 @@ def _try_plug_into_previous_block(self) -> bool:
         ) 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,
-            (PGLineType.BROKEN, PGLineType.CONTINUOUS)
-        )
+        merge_lane = self._get_merge_part(acc_lane, extension_length)
         self.block_network.add_lane(Decoration.start, Decoration.end, merge_lane)
 
         return no_cross
@@ -223,35 +227,23 @@ class OutRampOnStraight(Ramp):
     ID = "R"
     EXTRA_LEN = 15
 
-    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, (PGLineType.BROKEN, PGLineType.CONTINUOUS))
-
-    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=(PGLineType.CONTINUOUS, PGLineType.BROKEN)
-        )
+    def _get_merge_part(self, att_lane: StraightLane, length: float):
+        start = att_lane.position(0, self.lane_width)
+        merge_part = create_extension(start, length, ExtensionDirection.EXTEND, width=self.lane_width)
         return merge_part
 
     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))
+        dec_lane_len = self.get_config()[Parameter.length]
+        extension_len = self.get_config()[Parameter.extension_length]
         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, dec_lane_len + extension_len,
             [self.positive_basic_lane.line_types[0], PGLineType.SIDE]
         )
         dec_road = Road(self.pre_block_socket.positive_road.end_node, self.add_road_node())
@@ -302,19 +294,22 @@ def _try_plug_into_previous_block(self) -> bool:
 
         # 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)
+        merge_part_lane = self._get_merge_part(dec_right_lane, extension_len)
+        self.block_network.add_lane(Decoration.start, Decoration.end, merge_part_lane)
+
+        deacc_lane_end = dec_right_lane.position(dec_right_lane.length, self.lane_width)
+        deacc_lane = StraightLane(
+            merge_part_lane.end, deacc_lane_end, self.lane_width, (PGLineType.BROKEN, PGLineType.CONTINUOUS)
+        )
+        self.block_network.add_lane(dec_road.start_node, dec_road.end_node, deacc_lane)
         no_cross = (
             not check_lane_on_road(
-                self._global_network,
-                dec_side_right_lane,
-                0.95,
-                ignore_intersection_checking=self.ignore_intersection_checking
+                self._global_network, deacc_lane, 0.95, ignore_intersection_checking=self.ignore_intersection_checking
             )
         ) and no_cross
 
         bend_1, connect_part = create_bend_straight(
-            dec_side_right_lane,
+            deacc_lane,
             self.CONNECT_PART_LEN,
             self.RADIUS,
             np.deg2rad(self.ANGLE),
@@ -369,6 +364,4 @@ def _try_plug_into_previous_block(self) -> bool:
             )
         ) 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