Finalizing the MetaDrive MultiAgentRacingEnv (#563)

* fix problems in Lidar, make it use single-agent RL obs

* update

* clean code

* up

* Up

* fix a critical bug in the local coordinates in circular lane.

* fix a bug in difference between w/ & w/o panda renderer

* WIP: I am still debugging

* (Trying to) Fix a critical issue in the circular lane local position computing

* clean

* fix the bug in circular lane

* format

* fix issues in test, PR ready

* fix an issue in base_engine

* update

* change config

* a simpler version of racing map

* a simpler version of racing map

* update config

* format

* simplify racing env

* fix test

* hot fix BEV rendering issue

* finish

* finish

* revert a bug

* fix bug in sidewalk collision detection

* introduce crash sidewalk done (for debug)

* format

* Default this env to only allow 2 agents.

* fix issues

* format

---------

Co-authored-by: claire45 <yliu17@ualberta.ca>

metadrive/component/block/base_block.py

@@ -343,9 +343,6 @@ def construct_sidewalk(self):
                 shape = BulletTriangleMeshShape(mesh, dynamic=False)
 
                 body_node.addShape(shape)
-                if self.render:
-                    self.dynamic_nodes.append(body_node)
-                else:
-                    self.static_nodes.append(body_node)
+                self.dynamic_nodes.append(body_node)
                 body_node.setIntoCollideMask(CollisionGroup.Sidewalk)
                 self._node_path_list.append(np)

metadrive/component/lane/circular_lane.py

@@ -1,11 +1,10 @@
 import math
-from metadrive.constants import MetaDriveType
 from typing import Tuple
 
 import numpy as np
 
 from metadrive.component.lane.pg_lane import PGLane
-from metadrive.constants import PGDrivableAreaProperty
+from metadrive.constants import MetaDriveType
 from metadrive.constants import PGLineType
 from metadrive.utils.math import wrap_to_pi, norm, Vector
 
@@ -32,22 +31,24 @@ def __init__(
         self.center = Vector(center)
         self.radius = radius
         self._clock_wise = clockwise
-        self.start_phase = start_phase
-        self.end_phase = self.start_phase + (-angle if self.is_clockwise() else angle)
+        self.start_phase = wrap_to_pi(start_phase)
         self.angle = angle
+        # TODO(pzh): Don't do the wrap_to_pi for self.end_phase because sometime we might want to use self.end_phase
+        #  - self.start_phase to get self.angle (or we probably should eliminate those use cases?)
+        self.end_phase = self.start_phase + (-self.angle if self.is_clockwise() else self.angle)
         self.direction = -1 if clockwise else 1
         self.width = width
         self.line_types = line_types
         self.forbidden = forbidden
         self.priority = priority
 
-        self.length = self.radius * (self.end_phase - self.start_phase) * self.direction
+        self.length = abs(self.radius * (self.end_phase - self.start_phase))
         assert self.length > 0, "end_phase should > (<) start_phase if anti-clockwise (clockwise)"
         self.start = self.position(0, 0)
         self.end = self.position(self.length, 0)
 
     def update_properties(self):
-        self.length = self.radius * (self.end_phase - self.start_phase) * self.direction
+        self.length = abs(self.radius * (self.end_phase - self.start_phase))
         assert self.length > 0, "end_phase should > (<) start_phase if anti-clockwise (clockwise)"
         self.start = self.position(0, 0)
         self.end = self.position(self.length, 0)
@@ -68,14 +69,55 @@ def width_at(self, longitudinal: float) -> float:
         return self.width
 
     def local_coordinates(self, position: Tuple[float, float]) -> Tuple[float, float]:
+        """Compute the local coordinates (longitude, lateral) of the given position in this circular lane.
+
+        Args:
+            position: floats in 2D
+
+        Returns:
+            longtidue in float
+            lateral in float
+        """
         delta_x = position[0] - self.center[0]
         delta_y = position[1] - self.center[1]
-        phi = math.atan2(delta_y, delta_x)
-        phi = self.start_phase + wrap_to_pi(phi - self.start_phase)
-        r = norm(delta_x, delta_y)
-        longitudinal = self.direction * (phi - self.start_phase) * self.radius
-        # lateral = self.direction * (self.radius - r)
-        lateral = -self.direction * (self.radius - r)
+        abs_phase = math.atan2(delta_y, delta_x)
+        abs_phase = wrap_to_pi(abs_phase)
+
+        # Processing the relative phase (angle) is extremely complex.
+        start_phase = wrap_to_pi(self.start_phase)
+        end_phase = wrap_to_pi(self.end_phase)
+
+        diff_to_start_phase = abs(wrap_to_pi(abs_phase - start_phase))
+        diff_to_end_phase = abs(wrap_to_pi(abs_phase - end_phase))
+
+        if diff_to_start_phase > np.pi and diff_to_end_phase > np.pi:
+            raise ValueError(
+                f"Undetermined position. Relative phase of the given point to the start phase is"
+                f" {wrap_to_pi(abs_phase - start_phase)} while the phase to the end phase is "
+                f"{wrap_to_pi(abs_phase - end_phase)}. Both of them are > 180deg. "
+                f"We don't know how to compute the longitudinal in this case."
+            )
+
+        # If the point is closer to the end point
+        if diff_to_start_phase > diff_to_end_phase:
+            if self.is_clockwise():
+                diff = self.end_phase - abs_phase
+            else:
+                diff = abs_phase - self.end_phase
+            relative_phase = wrap_to_pi(diff)
+            longitudinal = relative_phase * self.radius + self.length
+        else:
+            if self.is_clockwise():
+                diff = self.start_phase - abs_phase
+            else:
+                diff = abs_phase - self.start_phase
+            relative_phase = wrap_to_pi(diff)
+            longitudinal = relative_phase * self.radius
+
+        # Compute the lateral
+        distance_to_center = norm(delta_x, delta_y)
+        lateral = self.direction * (distance_to_center - self.radius)
+
         return longitudinal, lateral
 
     @property

metadrive/component/pgblock/first_block.py

@@ -31,7 +31,9 @@ def __init__(
         physics_world: PhysicsWorld,
         length: float = 30,
         ignore_intersection_checking=False,
-        remove_negative_lanes=False
+        remove_negative_lanes=False,
+        side_lane_line_type=None,
+        center_line_type=None,
     ):
         place_holder = PGBlockSocket(Road(Decoration.start, Decoration.end), Road(Decoration.start, Decoration.end))
         super(FirstPGBlock, self).__init__(
@@ -40,7 +42,9 @@ def __init__(
             global_network,
             random_seed=0,
             ignore_intersection_checking=ignore_intersection_checking,
-            remove_negative_lanes=remove_negative_lanes
+            remove_negative_lanes=remove_negative_lanes,
+            side_lane_line_type=side_lane_line_type,
+            center_line_type=center_line_type,
         )
         if length < self.ENTRANCE_LENGTH:
             print("Warning: first block length is two small", length, "<", self.ENTRANCE_LENGTH)
@@ -55,14 +59,18 @@ def __init__(
             ego_v_spawn_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,
         )
         if not remove_negative_lanes:
             CreateAdverseRoad(
                 ego_v_spawn_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,
             )
 
         next_lane = ExtendStraightLane(basic_lane, length - self.ENTRANCE_LENGTH, [PGLineType.BROKEN, PGLineType.SIDE])
@@ -73,14 +81,18 @@ def __init__(
             other_v_spawn_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,
         )
         if not remove_negative_lanes:
             CreateAdverseRoad(
                 other_v_spawn_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,
             )
 
         self._create_in_world()

metadrive/component/sensors/lidar.py

@@ -60,7 +60,8 @@ def perceive(
                 "This result in errors "
                 "when generating point cloud and sensing surrounding objects."
                 "Please align the two parameters in config "
-                "to get the best Lidar performance".format(error)
+                "to get the best Lidar performance. That is, please align `config['sensors']` and "
+                "`config['vehicle_config']['lidar']['distance']`.".format(error)
             )
         return super(Lidar, self).perceive(
             base_vehicle,

metadrive/component/vehicle/base_vehicle.py

@@ -747,7 +747,7 @@ def _state_check(self):
             self.LENGTH,
             self.WIDTH,
             CollisionGroup.Sidewalk,
-            in_static_world=True if not self.render else False
+            in_static_world=False  # Sidewalk will be hosted in the dynamic_world. So here we set to False.
         )
         if res.hasHit() and res.getNode().getName() == MetaDriveType.BOUNDARY_LINE:
             self.crash_sidewalk = True
@@ -761,6 +761,9 @@ def _state_check(self):
             self.crash_sidewalk = True
             contacts.add(MetaDriveType.GUARDRAIL)
 
+        # elif res.hasHit():
+        #     print("Unclassified collision: ", res.getNode().getName())
+
         # only for visualization detection
         if self.render:
             res = rect_region_detection(

metadrive/constants.py

@@ -26,8 +26,10 @@ class TerminationState:
     CRASH_HUMAN = "crash_human"
     CRASH_OBJECT = "crash_object"
     CRASH_BUILDING = "crash_building"
+    CRASH_SIDEWALK = "crash_sidewalk"
     CURRENT_BLOCK = "current_block"
     ENV_SEED = "env_seed"
+    IDLE = "idle"
 
 
 HELP_MESSAGE = "Keyboard Shortcuts:\n" \
@@ -304,7 +306,7 @@ class PGDrivableAreaProperty:
     SIDEWALK_WIDTH = 2
     SIDEWALK_LINE_DIST = 0.6
 
-    GUARDRAIL_HEIGHT = 2.5
+    GUARDRAIL_HEIGHT = 4.0
 
     # visualization color property
     LAND_COLOR = (0.4, 0.4, 0.4, 1)

metadrive/engine/base_engine.py

@@ -361,6 +361,9 @@ def process_memory():
 
             cm = process_memory()
 
+        if self.lane_coordinates_debug_node is not None:
+            self.lane_coordinates_debug_node.removeNode()
+
         # reset manager
         for manager_name, manager in self._managers.items():
             # clean all manager
@@ -570,6 +573,9 @@ def close(self):
             del self._top_down_renderer
             self._top_down_renderer = None
 
+        if self.lane_coordinates_debug_node is not None:
+            self.lane_coordinates_debug_node.removeNode()
+
     def __del__(self):
         logger.debug("{} is destroyed".format(self.__class__.__name__))