Introducing the Multi-agent Racing environment (#559)

* Init commit * debugging initialize_asset_loader() * fix vis_block * format * fix * debugging initialize_asset_loader() * RacingEnv with racing map * no random seed when resetting the environment * closed track by connecting start and end * Add test for single agent racing tracks * testing multi agent environment for racing tracks * debugging MAEnv * MARacing Environment * Guardrail not working... * debugging Guardrail: Now side lane's type updates as BARRIER * Barrier works now * Init environment * import issue * register update * map_type environment attribute; no reverse road generated for tracks * new environment * fix * circular tracks done in hard coding updates * format * try to fix * revert PG block * revert curve & straight * fix bug in racing env * remove the wrong single-agent racing env * rework the MARL racing env * IDM is not compatible with on-the-fly navigation change * now support inf loop * delete * format * update * format * up * Update * fix bug in MARL racing * fix * up * update * fix bug * fix * fix * Update * Update * use green as a random color (previously we remove green color) * allow to change sidewalk height (for supporting barrier) * allow to put sidewalk in left side * fix problem in first_block * delete werid files * remove wierd things * Revert the wierd IDM policy * remove wrong config disable_mouse * revert * fix * format * refactor StraightWithGuardrail * refactor CurveWithRailguard * clean * fix bug * rename BARRIER to GUARDRAIL * set center_line_type to PGBlock's variable * format * add some docs * add test * format * rename --------- Co-authored-by: claire45 <yliu17@ualberta.ca> Co-authored-by: QuanyiLi <quanyili0057@gmail.com>
metadriverse · Nov 18, 2023 · 1429251 · 1429251
1 parent 55ab1a7
commit 1429251
Show file tree

Hide file tree

Showing 22 changed files with 729 additions and 84 deletions.
diff --git a/metadrive/component/block/base_block.py b/metadrive/component/block/base_block.py
@@ -33,7 +33,11 @@ class BaseBlock(BaseObject, PGDrivableAreaProperty, ABC):
     ID = "B"
 
     def __init__(
-        self, block_index: int, global_network: NodeRoadNetwork, random_seed, ignore_intersection_checking=False
+        self,
+        block_index: int,
+        global_network: NodeRoadNetwork,
+        random_seed,
+        ignore_intersection_checking=False,
     ):
         super(BaseBlock, self).__init__(str(block_index) + self.ID, random_seed, escape_random_seed_assertion=True)
         # block information
@@ -315,9 +319,13 @@ def construct_sidewalk(self):
         if self.engine is None or (self.engine.global_config["show_sidewalk"] and not self.engine.use_render_pipeline):
             for sidewalk in self.sidewalks.values():
                 polygon = sidewalk["polygon"]
-                np = make_polygon_model(polygon, PGDrivableAreaProperty.SIDEWALK_THICKNESS)
+                height = sidewalk.get("height", None)
+                if height is None:
+                    height = PGDrivableAreaProperty.SIDEWALK_THICKNESS
+                z_pos = height / 2
+                np = make_polygon_model(polygon, height)
                 np.reparentTo(self.sidewalk_node_path)
-                np.setPos(0, 0, PGDrivableAreaProperty.SIDEWALK_THICKNESS / 2)
+                np.setPos(0, 0, z_pos)
                 if self.render:
                     np.setTexture(self.side_texture)
                 # np.setTexture(self.ts_normal, self.side_normal)
@@ -326,7 +334,7 @@ def construct_sidewalk(self):
                 body_node.setKinematic(False)
                 body_node.setStatic(True)
                 body_np = self.sidewalk_node_path.attachNewNode(body_node)
-                body_np.setPos(0, 0, 0.1)
+                body_np.setPos(0, 0, z_pos)
                 self._node_path_list.append(body_np)
 
                 geom = np.node().getGeom(0)

diff --git a/metadrive/component/lane/pg_lane.py b/metadrive/component/lane/pg_lane.py
@@ -3,8 +3,6 @@
 from metadrive.component.lane.abs_lane import AbstractLane
 from metadrive.constants import PGDrivableAreaProperty
 from metadrive.constants import PGLineType
-from metadrive.engine.core.draw_line import ColorLineNodePath
-from metadrive.engine.engine_utils import get_engine
 from metadrive.type import MetaDriveType
 
 
@@ -50,14 +48,20 @@ def construct_continuous_line(self, block, lateral, line_color, line_type):
             node_path_list = self.construct_lane_line_segment(block, start, end, line_color, line_type)
             self._node_path_list.extend(node_path_list)
 
-    def construct_sidewalk(self, block):
+    def construct_sidewalk(self, block, sidewalk_height=None, lateral_direction=1):
         """
         Construct the sidewalk for this lane
-        Args:
-            block:
 
-        Returns:
+        Args:
+            block: Current block.
+            sidewalk_height: The height of sidewalk. Default to None and PGDrivableAreaProperty.SIDEWALK_THICKNESS will
+                be used.
+            lateral_direction: Whether to extend the sidewalk outward (to the right of the road) or inward (to the left
+                of the road). It's useful if we want to put sidewalk to the center of the road, e.g. in the scenario
+                where we only have "positive road" and there is no roads in opposite direction. Should be either -1 or
+                +1.
 
+        Returns: None
         """
         # engine = get_engine()
         # cloud_points_vis = ColorLineNodePath(
@@ -72,6 +76,9 @@ def construct_sidewalk(self, block):
         )
         start_lat = +self.width_at(0) / 2 + 0.2
         side_lat = start_lat + PGDrivableAreaProperty.SIDEWALK_WIDTH
+        assert lateral_direction == -1 or lateral_direction == 1
+        start_lat *= lateral_direction
+        side_lat *= lateral_direction
         if self.radius != 0 and side_lat > self.radius:
             raise ValueError(
                 "The sidewalk width ({}) is too large."
@@ -87,7 +94,7 @@ def construct_sidewalk(self, block):
                 # draw_lists[k].append([point[0], point[1], 1])
         # cloud_points_vis.drawLines(draw_lists)
         # cloud_points_vis.create()
-        block.sidewalks[str(self.index)] = {"polygon": polygon}
+        block.sidewalks[str(self.index)] = {"polygon": polygon, "height": sidewalk_height}
 
     def construct_lane_line_in_block(self, block, construct_left_right=(True, True)):
         """
@@ -104,10 +111,15 @@ def construct_lane_line_in_block(self, block, construct_left_right=(True, True))
             elif line_type == PGLineType.SIDE:
                 self.construct_continuous_line(block, lateral, line_color, line_type)
                 self.construct_sidewalk(block)
+            elif line_type == PGLineType.GUARDRAIL:
+                self.construct_continuous_line(block, lateral, line_color, line_type)
+                self.construct_sidewalk(
+                    block, sidewalk_height=PGDrivableAreaProperty.GUARDRAIL_HEIGHT, lateral_direction=idx
+                )
             elif line_type == PGLineType.NONE:
                 continue
             else:
                 raise ValueError(
-                    "You have to modify this cuntion and implement a constructing method for line type: {}".
+                    "You have to modify this function and implement a constructing method for line type: {}".
                     format(line_type)
                 )
diff --git a/metadrive/component/pgblock/create_pg_block_utils.py b/metadrive/component/pgblock/create_pg_block_utils.py
@@ -1,8 +1,7 @@
 import copy
-from metadrive.type import MetaDriveType
+import math
 from typing import Tuple, Union, List
 
-import math
 import numpy as np
 
 from metadrive.component.lane.abs_lane import AbstractLane
@@ -11,6 +10,7 @@
 from metadrive.component.road_network import Road
 from metadrive.component.road_network.node_road_network import NodeRoadNetwork
 from metadrive.constants import PGLineType, PGLineColor, PGDrivableAreaProperty
+from metadrive.type import MetaDriveType
 from metadrive.utils.math import get_vertical_vector
 from metadrive.utils.pg.utils import check_lane_on_road
 
@@ -55,23 +55,47 @@ def CreateRoadFrom(
     toward_smaller_lane_index: bool = True,
     ignore_start: str = None,
     ignore_end: str = None,
-    center_line_type=PGLineType.CONTINUOUS,  # Identical to Block.CENTER_LINE_TYPE
+    center_line_type=None,  # Identical to Block.CENTER_LINE_TYPE
     detect_one_side=True,
-    side_lane_line_type=PGLineType.SIDE,
-    inner_lane_line_type=PGLineType.BROKEN,
-    center_line_color=PGLineColor.YELLOW,
+    side_lane_line_type=None,
+    inner_lane_line_type=None,
+    center_line_color=None,
     ignore_intersection_checking=None,
     metadrive_lane_type=None,
 ) -> bool:
     """
+    Give the far left lane, then create lane_num lanes including the far left lane itself.
         | | | |
         | | | |
         | | | |
         | | | |
-    <-----smaller direction = inside direction
-    Usage: give the far left lane, then create lane_num lanes including itself
-    :return if the lanes created cross other lanes
+
+    Args:
+        lane:
+        lane_num:
+        road:
+        roadnet_to_add_lanes: The block network
+        roadnet_to_check_cross: Existing global network
+        toward_smaller_lane_index:
+        ignore_start:
+        ignore_end:
+        center_line_type:
+        detect_one_side:
+        side_lane_line_type:
+        inner_lane_line_type:
+        center_line_color:
+        ignore_intersection_checking:
+        metadrive_lane_type:
+
+    Return:
+        A Boolean indicator saying whether the road is constructed successfully.
     """
+    center_line_type = center_line_type or PGLineType.CONTINUOUS
+    side_lane_line_type = side_lane_line_type or PGLineType.SIDE
+    side_lane_line_type = side_lane_line_type or PGLineType.SIDE
+    inner_lane_line_type = inner_lane_line_type or PGLineType.BROKEN
+    center_line_color = center_line_color or PGLineColor.YELLOW
+
     lane_num -= 1  # include lane itself
     origin_lane = lane
     lanes = []
@@ -180,13 +204,37 @@ def CreateAdverseRoad(
     roadnet_to_check_cross: "NodeRoadNetwork",  # mostly, previous global network
     ignore_start: str = None,
     ignore_end: str = None,
-    center_line_type=PGLineType.CONTINUOUS,  # Identical to Block.CENTER_LINE_TYPE
-    side_lane_line_type=PGLineType.SIDE,
-    inner_lane_line_type=PGLineType.BROKEN,
-    center_line_color=PGLineColor.YELLOW,
+    center_line_type=None,
+    side_lane_line_type=None,
+    inner_lane_line_type=None,
+    center_line_color=None,
     ignore_intersection_checking=None,
     metadrive_lane_type=None
 ) -> bool:
+    """
+    Create the adverse road given the road network.
+
+    Args:
+        positive_road:
+        roadnet_to_get_road: The block network
+        roadnet_to_check_cross: The existing global network
+        ignore_start:
+        ignore_end:
+        center_line_type:
+        side_lane_line_type:
+        inner_lane_line_type:
+        center_line_color:
+        ignore_intersection_checking:
+        metadrive_lane_type:
+
+    Returns:
+        A Boolean indicator saying whether the road is constructed successfully.
+    """
+    center_line_type = center_line_type or PGLineType.CONTINUOUS  # Identical to Block.CENTER_LINE_TYPE
+    side_lane_line_type = side_lane_line_type or PGLineType.SIDE
+    inner_lane_line_type = inner_lane_line_type or PGLineType.BROKEN
+    center_line_color = center_line_color or PGLineColor.YELLOW
+
     adverse_road = -positive_road
     lanes = get_lanes_on_road(positive_road, roadnet_to_get_road)
     reference_lane = lanes[-1]

diff --git a/metadrive/component/pgblock/curve.py b/metadrive/component/pgblock/curve.py
@@ -1,10 +1,10 @@
 import numpy as np
 
+from metadrive.component.pg_space import ParameterSpace, Parameter, BlockParameterSpace
 from metadrive.component.pgblock.create_pg_block_utils import CreateAdverseRoad, CreateRoadFrom, create_bend_straight
 from metadrive.component.pgblock.pg_block import PGBlock
 from metadrive.component.road_network import Road
 from metadrive.constants import PGLineType
-from metadrive.component.pg_space import ParameterSpace, Parameter, BlockParameterSpace
 
 
 class Curve(PGBlock):
@@ -33,23 +33,33 @@ def _try_plug_into_previous_block(self) -> bool:
         angle = parameters[Parameter.angle]
         radius = parameters[Parameter.radius]
         curve, straight = create_bend_straight(
-            basic_lane, length, radius, np.deg2rad(angle), direction, basic_lane.width,
-            (PGLineType.BROKEN, PGLineType.SIDE)
+            basic_lane,
+            length,
+            radius,
+            np.deg2rad(angle),
+            direction,
+            width=basic_lane.width,
+            line_types=(PGLineType.BROKEN, self.side_lane_line_type)
         )
         no_cross = CreateRoadFrom(
             curve,
             lane_num,
             positive_road,
             self.block_network,
             self._global_network,
-            ignore_intersection_checking=self.ignore_intersection_checking
+            ignore_intersection_checking=self.ignore_intersection_checking,
+            side_lane_line_type=self.side_lane_line_type,
+            center_line_type=self.center_line_type
         )
-        no_cross = CreateAdverseRoad(
-            positive_road,
-            self.block_network,
-            self._global_network,
-            ignore_intersection_checking=self.ignore_intersection_checking
-        ) and no_cross
+        if not self.remove_negative_lanes:
+            no_cross = CreateAdverseRoad(
+                positive_road,
+                self.block_network,
+                self._global_network,
+                ignore_intersection_checking=self.ignore_intersection_checking,
+                side_lane_line_type=self.side_lane_line_type,
+                center_line_type=self.center_line_type
+            ) and no_cross
 
         # part 2
         start_node = end_node
@@ -61,14 +71,19 @@ def _try_plug_into_previous_block(self) -> bool:
             positive_road,
             self.block_network,
             self._global_network,
-            ignore_intersection_checking=self.ignore_intersection_checking
-        ) and no_cross
-        no_cross = CreateAdverseRoad(
-            positive_road,
-            self.block_network,
-            self._global_network,
-            ignore_intersection_checking=self.ignore_intersection_checking
+            ignore_intersection_checking=self.ignore_intersection_checking,
+            side_lane_line_type=self.side_lane_line_type,
+            center_line_type=self.center_line_type
         ) and no_cross
+        if not self.remove_negative_lanes:
+            no_cross = CreateAdverseRoad(
+                positive_road,
+                self.block_network,
+                self._global_network,
+                ignore_intersection_checking=self.ignore_intersection_checking,
+                side_lane_line_type=self.side_lane_line_type,
+                center_line_type=self.center_line_type
+            ) and no_cross
 
         # common properties
         self.add_sockets(self.create_socket_from_positive_road(positive_road))

diff --git a/metadrive/component/pgblock/first_block.py b/metadrive/component/pgblock/first_block.py
@@ -1,13 +1,13 @@
 from panda3d.core import NodePath
 
 from metadrive.component.lane.straight_lane import StraightLane
+from metadrive.component.pg_space import ParameterSpace
 from metadrive.component.pgblock.create_pg_block_utils import CreateRoadFrom, CreateAdverseRoad, ExtendStraightLane
 from metadrive.component.pgblock.pg_block import PGBlock, PGBlockSocket
 from metadrive.component.road_network import Road
 from metadrive.component.road_network.node_road_network import NodeRoadNetwork
 from metadrive.constants import Decoration, PGLineType
 from metadrive.engine.core.physics_world import PhysicsWorld
-from metadrive.component.pg_space import ParameterSpace
 
 
 class FirstPGBlock(PGBlock):
@@ -30,11 +30,17 @@ def __init__(
         render_root_np: NodePath,
         physics_world: PhysicsWorld,
         length: float = 30,
-        ignore_intersection_checking=False
+        ignore_intersection_checking=False,
+        remove_negative_lanes=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
+            0,
+            place_holder,
+            global_network,
+            random_seed=0,
+            ignore_intersection_checking=ignore_intersection_checking,
+            remove_negative_lanes=remove_negative_lanes
         )
         if length < self.ENTRANCE_LENGTH:
             print("Warning: first block length is two small", length, "<", self.ENTRANCE_LENGTH)
@@ -51,12 +57,13 @@ def __init__(
             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
-        )
+        if not remove_negative_lanes:
+            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, [PGLineType.BROKEN, PGLineType.SIDE])
         other_v_spawn_road = Road(self.NODE_2, self.NODE_3)
@@ -68,12 +75,13 @@ def __init__(
             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
-        )
+        if not remove_negative_lanes:
+            CreateAdverseRoad(
+                other_v_spawn_road,
+                self.block_network,
+                self._global_network,
+                ignore_intersection_checking=self.ignore_intersection_checking
+            )
 
         self._create_in_world()