Fix CI & Fix doc & add example for add traffic to sumo map (#746)

* use render for mixed traffic env

* fix edge net bug

* example of adding cars

* fix doc

* fix dependency

* add doc string

* format

.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

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

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.get("need_lane_localization", False)
+        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/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

@@ -38,5 +38,5 @@ 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)
+            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):

metadrive/tests/vis_env/vis_sumo_map.py

@@ -1,10 +1,63 @@
 """use netconvert --opendrive-files CARLA_town01.net.xml first"""
+import logging
+
+import numpy as np
 
+from metadrive.component.lane.point_lane import PointLane
+from metadrive.component.vehicle.vehicle_type import SVehicle
+from metadrive.engine.asset_loader import AssetLoader
 from metadrive.envs import BaseEnv
-from metadrive.obs.observation_base import DummyObservation
-import logging
+from metadrive.manager.base_manager import BaseManager
 from metadrive.manager.sumo_map_manager import SumoMapManager
-from metadrive.engine.asset_loader import AssetLoader
+from metadrive.obs.observation_base import DummyObservation
+from metadrive.policy.idm_policy import TrajectoryIDMPolicy
+from metadrive.utils.pg.utils import ray_localization
+
+
+class SimpleTrafficManager(BaseManager):
+    """
+    A simple traffic creator, which creates one vehicle to follow a specified route with IDM policy.
+    """
+    def __init__(self):
+        super(SimpleTrafficManager, self).__init__()
+        self.generated_v = None
+        self.arrive_dest = False
+
+    def after_reset(self):
+        """
+        Create vehicle and use IDM for controlling it. When there are objects in front of the vehicle, it will yield
+        """
+        self.arrive_dest = False
+        path_to_follow = []
+        for lane_index in ["lane_4_0", "lane_:306_0_0", "lane_22_0"]:
+            path_to_follow.append(self.engine.current_map.road_network.get_lane(lane_index).get_polyline())
+        path_to_follow = np.concatenate(path_to_follow, axis=0)
+
+        self.generated_v = self.spawn_object(
+            SVehicle, vehicle_config=dict(), position=path_to_follow[60], heading=-np.pi
+        )
+        TrajectoryIDMPolicy.NORMAL_SPEED = 20
+        self.add_policy(
+            self.generated_v.id,
+            TrajectoryIDMPolicy,
+            control_object=self.generated_v,
+            random_seed=0,
+            traj_to_follow=PointLane(path_to_follow, 2)
+        )
+
+    def before_step(self):
+        """
+        When arrive destination, stop
+        """
+        policy = self.get_policy(self.generated_v.id)
+        if policy.arrive_destination:
+            self.arrive_dest = True
+
+        if not self.arrive_dest:
+            action = policy.act(do_speed_control=True)
+        else:
+            action = [0., -1]
+        self.generated_v.before_step(action)  # set action
 
 
 class MyEnv(BaseEnv):
@@ -29,15 +82,15 @@ def setup_engine(self):
         super().setup_engine()
         map_path = AssetLoader.file_path("carla", "CARLA_town01.net.xml", unix_style=False)
         self.engine.register_manager("map_manager", SumoMapManager(map_path))
+        self.engine.register_manager("traffic_manager", SimpleTrafficManager())
 
 
 if __name__ == "__main__":
     # create env
     env = MyEnv(
         dict(
             use_render=True,
-            # if you have a screen and OpenGL suppor, you can set use_render=True to use 3D rendering
-            vehicle_config={"spawn_position_heading": [(0, 0), 0]},
+            vehicle_config={"spawn_position_heading": [(0, 0), np.pi / 2]},
             manual_control=True,  # we usually manually control the car to test environment
             use_mesh_terrain=True,
             log_level=logging.CRITICAL
@@ -46,5 +99,11 @@ def setup_engine(self):
     env.reset()
     for i in range(10000):
         # step
-        obs, reward, termination, truncate, info = env.step(env.action_space.sample())
+        obs, reward, termination, truncate, info, = env.step(env.action_space.sample())
+        current_lane_indices = [
+            info[1] for info in
+            ray_localization(env.vehicle.heading, env.vehicle.position, env.engine, use_heading_filter=True)
+        ]
+
+        env.render(text={"current_lane_indices": current_lane_indices})
     env.close()

metadrive/utils/sumo/map_utils.py

@@ -306,17 +306,17 @@ def extract_map_features(graph):
     # # build map boundary
     polygons = []
 
-    for junction_id, junction in graph.junctions.items():
-        if len(junction.shape) <= 2:
-            continue
-        boundary_polygon = Polygon(junction.shape)
-        boundary_polygon = [(x, y) for x, y in boundary_polygon.exterior.coords]
-        id = "junction_{}".format(junction.name)
-        ret[id] = {
-            SD.TYPE: MetaDriveType.LANE_SURFACE_STREET,
-            SD.POLYLINE: junction.shape,
-            SD.POLYGON: boundary_polygon,
-        }
+    # for junction_id, junction in graph.junctions.items():
+    #     if len(junction.shape) <= 2:
+    #         continue
+    #     boundary_polygon = Polygon(junction.shape)
+    #     boundary_polygon = [(x, y) for x, y in boundary_polygon.exterior.coords]
+    #     id = "junction_{}".format(junction.name)
+    #     ret[id] = {
+    #         SD.TYPE: MetaDriveType.LANE_SURFACE_STREET,
+    #         SD.POLYLINE: junction.shape,
+    #         SD.POLYGON: boundary_polygon,
+    #     }
 
     # build map lanes
     for road_id, road in graph.roads.items():