Merge branch 'main' into torch_expert_fix

.github/workflows/main.yml

@@ -317,6 +317,7 @@ jobs:
           pip install gymnasium==0.28
           ls -al
           pip install -e .
+          pip install numpy==1.24
           python -m metadrive.pull_asset
           
           cd bridges/ros_bridge

.gitignore

@@ -53,3 +53,7 @@
 /documentation/**/**.gif
 /documentation/source/img.png
 /documentation/source/demo.png
+
+# ignore documentation build output
+**/filtered_dataset
+**/semantics.png

documentation/requirements.txt

@@ -24,7 +24,7 @@ jinja2==3.1.2
     # via sphinx
 markupsafe==2.1.1
     # via jinja2
-packaging==21.3
+packaging>=22.0
     # via sphinx
 pygments==2.12.0
     # via sphinx

documentation/source/debug_mode.ipynb

@@ -140,17 +140,6 @@
     "In addition to the errors raised from MetaDrive, sometimes the game engine, Panda3D, will throw errors and warnings about the rendering service. To enable the logging of Panda3D, set `env_config[\"debug_panda3d\"]=True`. Besides, you can turn on Panda3D's profiler via `env_config[\"pstats\"]=True` and launch the `pstats` in the terminal. It can be used to analyze your program in terms of the time consumed for different functions like rendering, physics and so on, which is very useful if you are developing some graphics related features."
    ]
   },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "id": "7b3e2ecb",
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# launch pstats (bash)\n",
-    "!pstats"
-   ]
-  },
   {
    "cell_type": "code",
    "execution_count": null,

metadrive/component/map/pg_map.py

@@ -87,6 +87,7 @@ def _config_generate(self, blocks_config: List, parent_node_path: NodePath, phys
             render_root_np=parent_node_path,
             physics_world=physics_world,
             length=self._config.get("exit_length", 50),
+            start_point=self._config.get("start_position", [0, 0]),
             ignore_intersection_checking=True
         )
         self.blocks.append(last_block)

metadrive/component/map/scenario_map.py

@@ -12,10 +12,12 @@
 
 
 class ScenarioMap(BaseMap):
-    def __init__(self, map_index, map_data, random_seed=None):
+    def __init__(self, map_index, map_data, random_seed=None, need_lane_localization=False):
         self.map_index = map_index
         self.map_data = map_data
-        self.need_lane_localization = self.engine.global_config["need_lane_localization"]
+        self.need_lane_localization = need_lane_localization or self.engine.global_config.get(
+            "need_lane_localization", False
+        )
         super(ScenarioMap, self).__init__(dict(id=self.map_index), random_seed=random_seed)
 
     def show_coordinates(self):

metadrive/component/navigation_module/node_network_navigation.py

@@ -199,12 +199,12 @@ def update_localization(self, ego_vehicle):
             ego_vehicle=ego_vehicle
         )
 
+        self.navi_arrow_dir = [lanes_heading1, lanes_heading2]
         if self._show_navi_info:
             # Whether to visualize little boxes in the scene denoting the checkpoints
             pos_of_goal = checkpoint
             self._goal_node_path.setPos(panda_vector(pos_of_goal[0], pos_of_goal[1], self.MARK_HEIGHT))
             self._goal_node_path.setH(self._goal_node_path.getH() + 3)
-            self.navi_arrow_dir = [lanes_heading1, lanes_heading2]
             dest_pos = self._dest_node_path.getPos()
             self._draw_line_to_dest(start_position=ego_vehicle.position, end_position=(dest_pos[0], dest_pos[1]))
             navi_pos = self._goal_node_path.getPos()

metadrive/component/pgblock/first_block.py

@@ -1,5 +1,8 @@
+from typing import Sequence, Union
 from panda3d.core import NodePath
 
+import numpy as np
+
 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
@@ -30,6 +33,7 @@ def __init__(
         render_root_np: NodePath,
         physics_world: PhysicsWorld,
         length: float = 30,
+        start_point: Union[np.ndarray, Sequence[float]] = [0, 0],
         ignore_intersection_checking=False,
         remove_negative_lanes=False,
         side_lane_line_type=None,
@@ -48,9 +52,15 @@ def __init__(
         )
         if length < self.ENTRANCE_LENGTH:
             print("Warning: first block length is two small", length, "<", self.ENTRANCE_LENGTH)
+        if not isinstance(start_point, np.ndarray):
+            start_point = np.array(start_point)
+
         self._block_objects = []
         basic_lane = StraightLane(
-            [0, 0], [self.ENTRANCE_LENGTH, 0], line_types=(PGLineType.BROKEN, PGLineType.SIDE), width=lane_width
+            start_point,
+            start_point + [self.ENTRANCE_LENGTH, 0],
+            line_types=(PGLineType.BROKEN, PGLineType.SIDE),
+            width=lane_width
         )
         ego_v_spawn_road = Road(self.NODE_1, self.NODE_2)
         CreateRoadFrom(

metadrive/component/road_network/edge_road_network.py

@@ -24,10 +24,10 @@ def add_lane(self, lane) -> None:
         assert lane.index is not None, "Lane index can not be None"
         self.graph[lane.index] = lane_info(
             lane=lane,
-            entry_lanes=lane.entry_lanes,
-            exit_lanes=lane.exit_lanes,
-            left_lanes=lane.left_lanes,
-            right_lanes=lane.right_lanes
+            entry_lanes=lane.entry_lanes or [],
+            exit_lanes=lane.exit_lanes or [],
+            left_lanes=lane.left_lanes or [],
+            right_lanes=lane.right_lanes or []
         )
 
     def get_lane(self, index: LaneIndex):

metadrive/component/scenario_block/scenario_block.py

@@ -81,7 +81,7 @@ def create_in_world(self):
     def _construct_continuous_line(self, points, color):
         for index in range(0, len(points) - 1):
             node_path_list = self._construct_lane_line_segment(
-                points[index], points[index + 1], color, PGLineType.BROKEN
+                points[index], points[index + 1], color, MetaDriveType.LINE_SOLID_SINGLE_WHITE
             )
             self._node_path_list.extend(node_path_list)
 
@@ -92,7 +92,7 @@ def _construct_broken_line(self, points, color):
         for index in range(0, len(points) - 1, 2):
             if index + 1 < len(points) - 1:
                 node_path_list = self._construct_lane_line_segment(
-                    points[index], points[index + 1], color, PGLineType.BROKEN
+                    points[index], points[index + 1], color, MetaDriveType.LINE_BROKEN_SINGLE_WHITE
                 )
                 self._node_path_list.extend(node_path_list)
 

metadrive/component/sensors/base_camera.py

@@ -97,16 +97,23 @@ def _make_cuda_texture(self):
     def enable_cuda(self):
         return self is not None and self._enable_cuda
 
-    def save_image(self, base_object, name="debug.png"):
+    def get_image(self, base_object):
         """
-        Put camera to an object and save the image to the disk
+        Put camera to an object and get the image.
         """
         original_parent = self.cam.getParent()
         original_position = self.cam.getPos()
         original_hpr = self.cam.getHpr()
         self.cam.reparentTo(base_object.origin)
         img = self.get_rgb_array_cpu()
         self.track(original_parent, original_position, original_hpr)
+        return img
+
+    def save_image(self, base_object, name="debug.png"):
+        """
+        Put camera to an object and save the image to the disk
+        """
+        img = self.get_image(base_object)
         cv2.imwrite(name, img)
 
     def track(self, new_parent_node: NodePath, position, hpr):
@@ -152,7 +159,7 @@ def perceive(
             if position is None:
                 position = constants.DEFAULT_SENSOR_OFFSET
             if hpr is None:
-                position = constants.DEFAULT_SENSOR_HPR
+                hpr = constants.DEFAULT_SENSOR_HPR
 
             # return camera to original state
             original_object = self.cam.getParent()

metadrive/component/sensors/semantic_camera.py

@@ -37,10 +37,25 @@ def _setup_effect(self):
                     label, Terrain.make_render_state(self.engine, "terrain.vert.glsl", "terrain_semantics.frag.glsl")
                 )
             else:
-                cam.setTagState(
-                    label,
-                    RenderState.make(
-                        ShaderAttrib.makeOff(), LightAttrib.makeAllOff(), TextureAttrib.makeOff(),
-                        ColorAttrib.makeFlat((c[0] / 255, c[1] / 255, c[2] / 255, 1)), 1
+
+                if label == Semantics.PEDESTRIAN.label:
+                    # PZH: This is a workaround fix to make pedestrians animated.
+                    cam.setTagState(
+                        label,
+                        RenderState.make(
+                            # ShaderAttrib.makeOff(),
+                            LightAttrib.makeAllOff(),
+                            TextureAttrib.makeOff(),
+                            ColorAttrib.makeFlat((c[0] / 255, c[1] / 255, c[2] / 255, 1)),
+                            1
+                        )
+                    )
+
+                else:
+                    cam.setTagState(
+                        label,
+                        RenderState.make(
+                            ShaderAttrib.makeOff(), LightAttrib.makeAllOff(), TextureAttrib.makeOff(),
+                            ColorAttrib.makeFlat((c[0] / 255, c[1] / 255, c[2] / 255, 1)), 1
+                        )
                     )
-                )

metadrive/component/vehicle/base_vehicle.py

@@ -232,13 +232,37 @@ def before_step(self, action=None):
         return step_info
 
     def after_step(self):
+        step_info = {}
         if self.navigation and self.config["navigation_module"]:
             self.navigation.update_localization(self)
+            lanes_heading = self.navigation.navi_arrow_dir
+            lane_0_heading = lanes_heading[0]
+            lane_1_heading = lanes_heading[1]
+            navigation_straight = False
+            navigation_turn_left = False
+            navigation_turn_right = False
+            if abs(wrap_to_pi(lane_0_heading - lane_1_heading)) < 10 / 180 * math.pi:
+                navigation_straight = True
+            else:
+                dir_0 = np.array([math.cos(lane_0_heading), math.sin(lane_0_heading), 0])
+                dir_1 = np.array([math.cos(lane_1_heading), math.sin(lane_1_heading), 0])
+                cross_product = np.cross(dir_1, dir_0)
+                navigation_turn_left = True if cross_product[-1] < 0 else False
+                navigation_turn_right = not navigation_turn_left
+            step_info.update(
+                {
+                    "navigation_command": "forward" if navigation_straight else
+                    ("left" if navigation_turn_left else "right"),
+                    "navigation_forward": navigation_straight,
+                    "navigation_left": navigation_turn_left,
+                    "navigation_right": navigation_turn_right
+                }
+            )
         self._state_check()
         self.update_dist_to_left_right()
         step_energy, episode_energy = self._update_energy_consumption()
         self.out_of_route = self._out_of_route()
-        step_info = self._update_overtake_stat()
+        step_info.update(self._update_overtake_stat())
         my_policy = self.engine.get_policy(self.name)
         step_info.update(
             {

metadrive/engine/core/engine_core.py

@@ -30,6 +30,8 @@
 from metadrive.engine.logger import get_logger
 from metadrive.utils.utils import is_mac, setup_logger
 import logging
+import subprocess
+from metadrive.utils.utils import is_port_occupied
 
 logger = get_logger()
 
@@ -121,9 +123,21 @@ def __init__(self, global_config):
         self.pid = os.getpid()
         EngineCore.global_config = global_config
         self.mode = global_config["_render_mode"]
+        self.pstats_process = None
         if self.global_config["pstats"]:
             # pstats debug provided by panda3d
             loadPrcFileData("", "want-pstats 1")
+            if not is_port_occupied(5185):
+                self.pstats_process = subprocess.Popen(['pstats'])
+                logger.info(
+                    "pstats is launched successfully, tutorial is at: "
+                    "https://docs.panda3d.org/1.10/python/optimization/using-pstats"
+                )
+            else:
+                logger.warning(
+                    "pstats is already launched! tutorial is at: "
+                    "https://docs.panda3d.org/1.10/python/optimization/using-pstats"
+                )
 
         # Setup onscreen render
         if self.global_config["use_render"]:

metadrive/envs/legacy_envs/mixed_traffic_env.py

@@ -23,7 +23,7 @@ def setup_engine(self):
         {
             "rl_agent_ratio": 0.5,
             "manual_control": True,
-            # "use_render": True,
+            "use_render": True,
             "disable_model_compression": True,
             # "map": "SS",
             "num_scenarios": 100,

metadrive/envs/metadrive_env.py

@@ -29,6 +29,7 @@
         BaseMap.LANE_WIDTH: 3.5,
         BaseMap.LANE_NUM: 3,
         "exit_length": 50,
+        "start_position": [0, 0],
     },
     store_map=True,
 

metadrive/examples/drive_in_single_agent_env.py

@@ -66,6 +66,7 @@
                     "Keyboard Control": "W,A,S,D",
                 }
             )
+            print("Navigation information: ", info["navigation_command"])
 
             if args.observation == "rgb_camera":
                 cv2.imshow('RGB Image in Observation', o["image"][..., -1])

metadrive/manager/scenario_light_manager.py

@@ -13,6 +13,8 @@
 class ScenarioLightManager(BaseManager):
     CLEAR_LIGHTS = False
 
+    OBJECT_PREFIX = "traffic_light_"
+
     def __init__(self):
         super(ScenarioLightManager, self).__init__()
         self._scenario_id_to_obj_id = {}
@@ -41,12 +43,15 @@ def after_reset(self):
                     )
             lane_info = self.engine.current_map.road_network.graph[str(scenario_lane_id)]
             position = self._get_light_position(light_info)
-            name = scenario_lane_id if self.engine.global_config["force_reuse_object_name"] else None
+            name = self.OBJECT_PREFIX + scenario_lane_id if self.engine.global_config["force_reuse_object_name"
+                                                                                      ] else None
             traffic_light = self.spawn_object(ScenarioTrafficLight, lane=lane_info.lane, position=position, name=name)
             self._scenario_id_to_obj_id[scenario_lane_id] = traffic_light.id
             self._obj_id_to_scenario_id[traffic_light.id] = scenario_lane_id
             if self.engine.global_config["force_reuse_object_name"]:
-                assert scenario_lane_id == traffic_light.id, "Original id should be assigned to traffic lights"
+                assert self.OBJECT_PREFIX + scenario_lane_id == traffic_light.id, (
+                    "Original id should be assigned to traffic lights"
+                )
             self._lane_index_to_obj[lane_info.lane.index] = traffic_light
             status = light_info[SD.TRAFFIC_LIGHT_STATUS][self.episode_step]
             traffic_light.set_status(status)

metadrive/manager/sumo_map_manager.py

@@ -0,0 +1,42 @@
+from metadrive.component.map.scenario_map import ScenarioMap
+from metadrive.manager.base_manager import BaseManager
+from metadrive.utils.sumo.map_utils import extract_map_features, RoadLaneJunctionGraph
+
+
+class SumoMapManager(BaseManager):
+    """
+    It currently only support load one map into the simulation.
+    """
+    PRIORITY = 0  # Map update has the most high priority
+
+    def __init__(self, sumo_map_path):
+        """
+        Init the map manager. It can be extended to manage more maps
+        """
+        super(SumoMapManager, self).__init__()
+        self.current_map = None
+        self.graph = RoadLaneJunctionGraph(sumo_map_path)
+        self.map_feature = extract_map_features(self.graph)
+
+    def destroy(self):
+        """
+        Delete the map manager
+        """
+        self.current_map.destroy()
+        super(SumoMapManager, self).destroy()
+        self.current_map = None
+
+    def before_reset(self):
+        """
+        Detach existing maps
+        """
+        if self.current_map:
+            self.current_map.detach_from_world()
+
+    def reset(self):
+        """
+        Rebuild the map and load it into the scene
+        """
+        if not self.current_map:
+            self.current_map = ScenarioMap(map_index=0, map_data=self.map_feature, need_lane_localization=True)
+        self.current_map.attach_to_world()

metadrive/policy/idm_policy.py

@@ -459,7 +459,7 @@ def steering_control(self, target_lane) -> float:
         lane_heading = target_lane.heading_theta_at(long + 1)
         v_heading = ego_vehicle.heading_theta
         steering = self.heading_pid.get_result(-wrap_to_pi(lane_heading - v_heading))
-        # steering += self.lateral_pid.get_result(-lat)
+        steering += self.lateral_pid.get_result(-lat)
         return float(steering)
 
     def act(self, do_speed_control, *args, **kwargs):