Convert nuplan (#429)

* rename convert API

* rename all utils

* fix .sh

* top down cam set position

* initial

* rename script

* extract map features

* convert nuplan

* add sanity check

* fix velocity bug

* fix bug

* manual seed

* when ego car replay don't apply collision filer

* add flag to control filtering overlapping cars

* all nuplan internal line broken

* get nuplan ego velocity from positions

* fix state rendering bug

* format

* warm up model load

* format

metadrive/base_class/base_object.py

@@ -17,9 +17,9 @@
 from metadrive.utils import get_np_random
 from metadrive.utils import random_string
 from metadrive.utils.coordinates_shift import panda_vector, metadrive_vector, panda_heading
-from metadrive.utils.math_utils import clip
-from metadrive.utils.math_utils import norm
-from metadrive.utils.math_utils import wrap_to_pi
+from metadrive.utils.math import clip
+from metadrive.utils.math import norm
+from metadrive.utils.math import wrap_to_pi
 
 logger = logging.getLogger(__name__)
 

metadrive/base_class/randomizable.py

@@ -1,4 +1,4 @@
-from metadrive.utils.random_utils import get_np_random
+from metadrive.utils.random import get_np_random
 
 
 class Randomizable:

metadrive/component/block/base_block.py

@@ -15,7 +15,7 @@
 from metadrive.engine.asset_loader import AssetLoader
 from metadrive.engine.core.physics_world import PhysicsWorld
 from metadrive.utils.coordinates_shift import panda_vector, panda_heading
-from metadrive.utils.math_utils import norm
+from metadrive.utils.math import norm
 
 logger = logging.getLogger(__name__)
 

metadrive/component/buildings/tollgate_building.py

@@ -1,7 +1,7 @@
 from metadrive.component.buildings.base_building import BaseBuilding
 from metadrive.type import MetaDriveType
 from metadrive.engine.asset_loader import AssetLoader
-from metadrive.utils.pg_utils.utils import generate_static_box_physics_body
+from metadrive.utils.pg.utils import generate_static_box_physics_body
 
 
 class TollGateBuilding(BaseBuilding):

metadrive/component/lane/abs_lane.py

@@ -18,7 +18,7 @@
 from metadrive.engine.physics_node import BulletRigidBodyNode
 from metadrive.utils import norm
 from metadrive.utils.coordinates_shift import panda_vector, panda_heading
-from metadrive.utils.math_utils import Vector
+from metadrive.utils.math import Vector
 
 
 class AbstractLane:

metadrive/component/lane/circular_lane.py

@@ -6,7 +6,7 @@
 from metadrive.component.lane.pg_lane import PGLane
 from metadrive.constants import DrivableAreaProperty
 from metadrive.constants import PGLineType
-from metadrive.utils.math_utils import wrap_to_pi, norm, Vector
+from metadrive.utils.math import wrap_to_pi, norm, Vector
 
 
 class CircularLane(PGLane):

metadrive/component/lane/opendrive_lane.py

@@ -6,8 +6,8 @@
 
 from metadrive.component.lane.abs_lane import AbstractLane
 from metadrive.utils.interpolating_line import InterpolatingLine
-from metadrive.utils.opendrive_map_utils.elements.geometry import Line, Arc
-from metadrive.utils.opendrive_map_utils.map_load import get_lane_id
+from metadrive.utils.opendrive.elements.geometry import Line, Arc
+from metadrive.utils.opendrive.map_load import get_lane_id
 
 
 class OpenDriveLane(AbstractLane, InterpolatingLine):

metadrive/component/lane/point_lane.py

@@ -9,8 +9,8 @@
 from metadrive.constants import DrivableAreaProperty
 from metadrive.constants import PGLineType
 from metadrive.utils.interpolating_line import InterpolatingLine
-from metadrive.utils.math_utils import get_points_bounding_box
-from metadrive.utils.math_utils import wrap_to_pi
+from metadrive.utils.math import get_points_bounding_box
+from metadrive.utils.math import wrap_to_pi
 
 
 class PointLane(AbstractLane, InterpolatingLine):

metadrive/component/lane/scenario_lane.py

@@ -8,7 +8,7 @@
 from metadrive.component.lane.point_lane import PointLane
 from metadrive.scenario.scenario_description import ScenarioDescription
 from metadrive.engine.asset_loader import AssetLoader
-from metadrive.utils.math_utils import norm, mph_to_kmh
+from metadrive.utils.math import norm, mph_to_kmh
 from metadrive.scenario.utils import read_scenario_data
 
 

metadrive/component/lane/straight_lane.py

@@ -5,7 +5,7 @@
 
 from metadrive.component.lane.pg_lane import PGLane
 from metadrive.constants import PGLineType
-from metadrive.utils.math_utils import norm
+from metadrive.utils.math import norm
 
 
 class StraightLane(PGLane):

metadrive/component/nuplan_block/nuplan_block.py

@@ -20,7 +20,7 @@
 from metadrive.constants import DrivableAreaProperty
 from metadrive.constants import PGLineColor, PGLineType
 from metadrive.utils.interpolating_line import InterpolatingLine
-from metadrive.utils.math_utils import wrap_to_pi, norm
+from metadrive.utils.math import wrap_to_pi, norm
 
 
 @dataclass

metadrive/component/opendrive_block/opendrive_block.py

@@ -1,7 +1,7 @@
 from metadrive.component.block.base_block import BaseBlock
 from metadrive.component.lane.opendrive_lane import OpenDriveLane
 from metadrive.component.road_network.edge_road_network import OpenDriveRoadNetwork
-from metadrive.utils.opendrive_map_utils.map_load import get_lane_width
+from metadrive.utils.opendrive.map_load import get_lane_width
 
 
 class OpenDriveBlock(BaseBlock):

metadrive/component/pgblock/create_pg_block_utils.py

@@ -10,8 +10,8 @@
 from metadrive.component.road_network import Road
 from metadrive.component.road_network.node_road_network import NodeRoadNetwork
 from metadrive.constants import PGLineType, PGLineColor, DrivableAreaProperty
-from metadrive.utils.math_utils import get_vertical_vector
-from metadrive.utils.pg_utils.utils import check_lane_on_road
+from metadrive.utils.math import get_vertical_vector
+from metadrive.utils.pg.utils import check_lane_on_road
 
 
 def create_bend_straight(

metadrive/component/pgblock/fork.py

@@ -8,7 +8,7 @@
 from metadrive.component.pgblock.ramp import Ramp
 from metadrive.component.road_network import Road
 from metadrive.constants import Decoration, PGLineType
-from metadrive.utils.pg_utils.utils import check_lane_on_road
+from metadrive.utils.pg.utils import check_lane_on_road
 from metadrive.component.pg_space import ParameterSpace, Parameter, BlockParameterSpace
 
 

metadrive/component/pgblock/intersection.py

@@ -9,7 +9,7 @@
 from metadrive.component.pgblock.pg_block import PGBlock, PGBlockSocket
 from metadrive.component.road_network import Road
 from metadrive.constants import PGLineType
-from metadrive.utils.pg_utils.utils import check_lane_on_road
+from metadrive.utils.pg.utils import check_lane_on_road
 from metadrive.component.pg_space import ParameterSpace, Parameter, BlockParameterSpace
 
 

metadrive/component/pgblock/ramp.py

@@ -7,7 +7,7 @@
 from metadrive.component.pgblock.pg_block import PGBlock
 from metadrive.component.road_network import Road
 from metadrive.constants import Decoration, PGLineType
-from metadrive.utils.pg_utils.utils import check_lane_on_road
+from metadrive.utils.pg.utils import check_lane_on_road
 from metadrive.component.pg_space import ParameterSpace, Parameter, BlockParameterSpace
 
 

metadrive/component/road_network/edge_road_network.py

@@ -5,8 +5,8 @@
 
 from metadrive.component.road_network.base_road_network import BaseRoadNetwork
 from metadrive.component.road_network.base_road_network import LaneIndex
-from metadrive.utils.math_utils import get_boxes_bounding_box
-from metadrive.utils.pg_utils.utils import get_lanes_bounding_box
+from metadrive.utils.math import get_boxes_bounding_box
+from metadrive.utils.pg.utils import get_lanes_bounding_box
 
 lane_info = namedtuple("neighbor_lanes", "lane entry_lanes exit_lanes left_lanes right_lanes")
 

metadrive/component/road_network/node_road_network.py

@@ -7,8 +7,8 @@
 from metadrive.component.road_network.base_road_network import BaseRoadNetwork
 from metadrive.component.road_network.road import Road
 from metadrive.constants import Decoration
-from metadrive.utils.math_utils import get_boxes_bounding_box
-from metadrive.utils.pg_utils.utils import get_lanes_bounding_box
+from metadrive.utils.math import get_boxes_bounding_box
+from metadrive.utils.pg.utils import get_lanes_bounding_box
 
 logger = logging.getLogger(__name__)
 

metadrive/component/traffic_light/base_traffic_light.py

@@ -3,7 +3,7 @@
 from metadrive.scenario.scenario_description import ScenarioDescription
 from metadrive.constants import MetaDriveType
 from metadrive.engine.asset_loader import AssetLoader
-from metadrive.utils.pg_utils.utils import generate_static_box_physics_body
+from metadrive.utils.pg.utils import generate_static_box_physics_body
 
 
 class BaseTrafficLight(BaseObject):
@@ -26,7 +26,7 @@ def __init__(
         self.lane = lane
         self.status = MetaDriveType.LIGHT_UNKNOWN
 
-        self.lane_width = lane.width_at(0)
+        self.lane_width = lane.width_at(0) if lane else 4
         air_wall = generate_static_box_physics_body(
             self.AIR_WALL_LENGTH,
             self.lane_width,
@@ -42,7 +42,7 @@ def __init__(
             position = lane.position(self.PLACE_LONGITUDE, 0)
 
         self.set_position(position, self.AIR_WALL_HEIGHT / 2)
-        self.set_heading_theta(lane.heading_theta_at(self.PLACE_LONGITUDE))
+        self.set_heading_theta(lane.heading_theta_at(self.PLACE_LONGITUDE) if lane else 0)
         self.current_light = None
 
         if self.render:

metadrive/component/traffic_participants/pedestrian.py

@@ -6,7 +6,7 @@
 from metadrive.constants import MetaDriveType
 from metadrive.engine.asset_loader import AssetLoader
 from metadrive.engine.physics_node import BaseRigidBodyNode
-from metadrive.utils.math_utils import norm
+from metadrive.utils.math import norm
 
 
 class Pedestrian(BaseTrafficParticipant):

metadrive/component/vehicle/base_vehicle.py

@@ -28,10 +28,10 @@
 from metadrive.engine.engine_utils import get_engine, engine_initialized
 from metadrive.engine.physics_node import BaseRigidBodyNode
 from metadrive.utils import Config, safe_clip_for_small_array
-from metadrive.utils.math_utils import get_vertical_vector, norm, clip
-from metadrive.utils.math_utils import wrap_to_pi
-from metadrive.utils.pg_utils.utils import ray_localization
-from metadrive.utils.pg_utils.utils import rect_region_detection
+from metadrive.utils.math import get_vertical_vector, norm, clip
+from metadrive.utils.math import wrap_to_pi
+from metadrive.utils.pg.utils import ray_localization
+from metadrive.utils.pg.utils import rect_region_detection
 from metadrive.utils.utils import get_object_from_node
 
 
@@ -801,7 +801,7 @@ def _state_check(self):
                 self.LENGTH,
                 self.WIDTH,
                 CollisionGroup.LaneSurface,
-                in_static_world=True if not self.render else False
+                in_static_world=True if not self.engine.global_config["debug_static_world"] else False
             )
             if not res.hasHit() or res.getNode().getName() != MetaDriveType.LANE_SURFACE_STREET:
                 contacts.add(MetaDriveType.GROUND)

metadrive/component/vehicle_model/behavior.py

@@ -4,10 +4,10 @@
 from metadrive.component.vehicle_model.controller import ControlledVehicle
 from metadrive.component.vehicle_model.kinematics import Vehicle
 
-import metadrive.utils.math_utils as utils
+import metadrive.utils.math as utils
 from metadrive.constants import Route, LaneIndex
 from metadrive.manager.traffic_manager import PGTrafficManager
-from metadrive.utils.math_utils import clip
+from metadrive.utils.math import clip
 
 
 class IDMVehicle(ControlledVehicle):

metadrive/component/vehicle_module/distance_detector.py

@@ -10,7 +10,7 @@
 from metadrive.engine.asset_loader import AssetLoader
 from metadrive.engine.engine_utils import get_engine
 from metadrive.utils.coordinates_shift import panda_vector
-from metadrive.utils.math_utils import panda_vector, get_laser_end
+from metadrive.utils.math import panda_vector, get_laser_end
 
 detect_result = namedtuple("detect_result", "cloud_points detected_objects")
 

metadrive/component/vehicle_module/lidar.py

@@ -10,7 +10,7 @@
 from metadrive.constants import CamMask, CollisionGroup
 from metadrive.engine.engine_utils import get_engine
 from metadrive.utils.coordinates_shift import panda_vector
-from metadrive.utils.math_utils import norm, clip
+from metadrive.utils.math import norm, clip
 from metadrive.utils.utils import get_object_from_node
 
 

metadrive/component/vehicle_navigation_module/edge_network_navigation.py

@@ -5,8 +5,8 @@
 from metadrive.component.road_network.edge_road_network import EdgeRoadNetwork
 from metadrive.component.vehicle_navigation_module.base_navigation import BaseNavigation
 from metadrive.utils import clip, norm
-from metadrive.utils.math_utils import panda_vector
-from metadrive.utils.pg_utils.utils import ray_localization
+from metadrive.utils.math import panda_vector
+from metadrive.utils.pg.utils import ray_localization
 
 
 class EdgeNetworkNavigation(BaseNavigation):

metadrive/component/vehicle_navigation_module/node_network_navigation.py

@@ -10,8 +10,8 @@
 from metadrive.component.road_network.node_road_network import NodeRoadNetwork
 from metadrive.component.vehicle_navigation_module.base_navigation import BaseNavigation
 from metadrive.utils import clip, norm, get_np_random
-from metadrive.utils.math_utils import panda_vector
-from metadrive.utils.pg_utils.utils import ray_localization
+from metadrive.utils.math import panda_vector
+from metadrive.utils.pg.utils import ray_localization
 
 
 class NodeNetworkNavigation(BaseNavigation):

metadrive/component/vehicle_navigation_module/trajectory_navigation.py

@@ -2,8 +2,8 @@
 
 from metadrive.component.pg_space import BlockParameterSpace, Parameter
 from metadrive.component.vehicle_navigation_module.base_navigation import BaseNavigation
-from metadrive.utils.math_utils import norm, clip
-from metadrive.utils.math_utils import panda_vector
+from metadrive.utils.math import norm, clip
+from metadrive.utils.math import panda_vector
 
 
 class TrajectoryNavigation(BaseNavigation):

metadrive/engine/base_engine.py

@@ -312,6 +312,7 @@ def process_memory():
                 self.main_camera.track(current_track_vehicle)
                 if bev_cam:
                     self.main_camera.stop_track()
+                    self.main_camera.set_bird_view_pos(current_track_vehicle.position)
 
                 # if self.global_config["is_multi_agent"]:
                 #     self.main_camera.stop_track(bird_view_on_current_position=False)
@@ -620,11 +621,21 @@ def warmup(self):
         This function automatically initialize models/objects. It can prevent the lagging when creating some objects
         for the first time.
         """
-        if self.global_config["preload_pedestrian"]:
+        if self.global_config["preload_models"]:
             from metadrive.component.traffic_participants.pedestrian import Pedestrian
+            from metadrive.component.traffic_light.base_traffic_light import BaseTrafficLight
+            from metadrive.component.static_object.traffic_object import TrafficBarrier
+            from metadrive.component.static_object.traffic_object import TrafficCone
             Pedestrian.init_pedestrian_model()
             warm_up_pedestrian = self.spawn_object(Pedestrian, position=[0, 0], heading_theta=0, record=False)
+            warm_up_light = self.spawn_object(BaseTrafficLight, lane=None, position=[0, 0], record=False)
+            barrier = self.spawn_object(TrafficBarrier, position=[0, 0], heading_theta=0)
+            cone = self.spawn_object(TrafficCone, position=[0, 0], heading_theta=0)
             for vel in Pedestrian.SPEED_LIST:
                 warm_up_pedestrian.set_velocity([1, 0], vel - 0.1)
                 self.taskMgr.step()
-            self.clear_objects([warm_up_pedestrian.id], record=False)
+            self.clear_objects([warm_up_pedestrian.id, warm_up_light.id, barrier.id, cone.id], record=False)
+            warm_up_pedestrian = None
+            warm_up_light = None
+            barrier = None
+            cone = None

metadrive/engine/scene_cull.py

@@ -1,7 +1,7 @@
 import logging
 from typing import List
 
-from metadrive.utils.math_utils import norm
+from metadrive.utils.math import norm
 
 logger = logging.getLogger(__name__)
 

metadrive/envs/base_env.py

@@ -175,7 +175,7 @@
     interface_panel=[MiniMap, RGBCamera, VehiclePanel],
     multi_thread_render=True,
     multi_thread_render_mode="Cull",  # or "Cull/Draw"
-    preload_pedestrian=True,  # preload pedestrian Object for avoiding lagging when creating it for the first time
+    preload_models=True,  # preload pedestrian Object for avoiding lagging when creating it for the first time
 
     # record/replay metadata
     record_episode=False,  # when replay_episode is not None ,this option will be useless
@@ -546,7 +546,7 @@ def setup_engine(self):
         Engine setting after launching
         """
         self.engine.accept("r", self.reset)
-        self.engine.accept("p", self.capture)
+        self.engine.accept("c", self.capture)
         self.engine.register_manager("agent_manager", self.agent_manager)
         self.engine.register_manager("record_manager", RecordManager())
         self.engine.register_manager("replay_manager", ReplayManager())

metadrive/envs/marl_envs/marl_bidirection.py

@@ -6,7 +6,7 @@
 from metadrive.envs.marl_envs.multi_agent_metadrive import MultiAgentMetaDrive
 from metadrive.manager.map_manager import PGMapManager
 from metadrive.utils import Config
-from metadrive.utils.math_utils import clip
+from metadrive.utils.math import clip
 
 MABidirectionConfig = dict(
     spawn_roads=[Road(FirstPGBlock.NODE_2, FirstPGBlock.NODE_3), -Road(Split.node(3, 0, 0), Split.node(3, 0, 1))],

metadrive/envs/marl_envs/marl_bottleneck.py

@@ -5,7 +5,7 @@
 from metadrive.envs.marl_envs.multi_agent_metadrive import MultiAgentMetaDrive
 from metadrive.manager.map_manager import PGMapManager
 from metadrive.utils import Config
-from metadrive.utils.math_utils import clip
+from metadrive.utils.math import clip
 
 MABottleneckConfig = dict(
     spawn_roads=[Road(FirstPGBlock.NODE_2, FirstPGBlock.NODE_3), -Road(Split.node(2, 0, 0), Split.node(2, 0, 1))],

metadrive/envs/marl_envs/marl_parking_lot.py

@@ -17,7 +17,7 @@
 from metadrive.manager.map_manager import PGMapManager
 from metadrive.utils import get_np_random, Config
 from metadrive.utils.coordinates_shift import panda_vector, panda_heading
-from metadrive.utils.pg_utils.utils import rect_region_detection
+from metadrive.utils.pg.utils import rect_region_detection
 
 MAParkingLotConfig = dict(
     in_spawn_roads=[

metadrive/envs/marl_envs/tinyinter.py

@@ -8,7 +8,7 @@
 from metadrive.obs.state_obs import LidarStateObservation
 from metadrive.policy.idm_policy import IDMPolicy
 from metadrive.utils import Config
-from metadrive.utils.math_utils import Vector, norm, clip
+from metadrive.utils.math import Vector, norm, clip
 
 
 class CommunicationObservation(LidarStateObservation):