Merge branch 'main' into on_broken_line_done

.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,

documentation/source/obs.ipynb

@@ -4,7 +4,6 @@
    "cell_type": "markdown",
    "id": "72c167e8",
    "metadata": {
-    "editable": true,
     "slideshow": {
      "slide_type": ""
     },
@@ -415,7 +414,6 @@
    "execution_count": 4,
    "id": "ff7a70aa",
    "metadata": {
-    "editable": true,
     "slideshow": {
      "slide_type": ""
     },
@@ -544,7 +542,6 @@
    "execution_count": 45,
    "id": "3562290f",
    "metadata": {
-    "editable": true,
     "slideshow": {
      "slide_type": ""
     },
@@ -592,7 +589,6 @@
    "execution_count": 12,
    "id": "995d5314-92a7-4e68-8bb8-05f1bd8ab718",
    "metadata": {
-    "editable": true,
     "slideshow": {
      "slide_type": ""
     },
@@ -619,7 +615,6 @@
    "execution_count": 13,
    "id": "7f20c293-77f9-451c-a552-882def3d6257",
    "metadata": {
-    "editable": true,
     "slideshow": {
      "slide_type": ""
     },
@@ -712,7 +707,6 @@
    "execution_count": 10,
    "id": "9ea9966f-f123-40e8-a432-1ac19f396431",
    "metadata": {
-    "editable": true,
     "slideshow": {
      "slide_type": ""
     },
@@ -806,13 +800,16 @@
     "from metadrive.envs.metadrive_env import MetaDriveEnv\n",
     "from metadrive.obs.state_obs import LidarStateObservation\n",
     "from metadrive.component.sensors.rgb_camera import RGBCamera\n",
+    "import os\n",
+    "test_doc = os.getenv('TEST_DOC')\n",
+    "sensor_size = (84, 60) if test_doc else (200, 100)\n",
     "\n",
     "env = MetaDriveEnv(config=dict(\n",
     "    use_render=False,\n",
     "    agent_observation=LidarStateObservation,\n",
     "    image_observation=True,\n",
     "    norm_pixel=False,\n",
-    "    sensors=dict(rgb_camera=(RGBCamera, 512, 256)),\n",
+    "    sensors=dict(rgb_camera=(RGBCamera, *sensor_size)),\n",
     "))\n",
     "\n",
     "obs, info = env.reset()\n",
@@ -822,9 +819,10 @@
     "image = env.engine.get_sensor(\"rgb_camera\").perceive(to_float=False)\n",
     "image = image[..., [2, 1, 0]]\n",
     "\n",
-    "import matplotlib.pyplot as plt\n",
-    "plt.imshow(image)\n",
-    "plt.show()"
+    "if not test_doc:\n",
+    "    import matplotlib.pyplot as plt\n",
+    "    plt.imshow(image)\n",
+    "    plt.show()"
    ]
   }
  ],
@@ -844,7 +842,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.10.13"
+   "version": "3.7.13"
   },
   "mystnb": {
    "execution_mode": "force"

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/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/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(
             {
@@ -250,30 +274,6 @@ def after_step(self):
                 "policy": my_policy.name if my_policy is not None else my_policy
             }
         )
-
-        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
-            }
-        )
         return step_info
 
     def _out_of_route(self):

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/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):