Introducing a single agent, multi-goal, four-ways intersection enviro…

…nment (#699)

* better way to handle U turn in Intersection

* INIT

* Revert "better way to handle U turn in Intersection"

This reverts commit d9bf4bb.

* add doc

* fix bug

* add docs

* If navigation_module is set to None, remove navigation in StateObservation. (I am not 100% sure if this is OK. Might cause error.)

* WIP: Implementing a navigation/goal manager

* Implemented goal / navigation managers

* adding a U-turn intersection block

* add assert in node_road_network

* fix potential bug

* Now the environment is running correctly! (but reset is broken)

* introduce observation

* format

* add more docs

* introduce a config to disable navigation arrows

* minor

* use varying dynamics agent in multi-goal env

* support use a list of types to control PG map; allow set map=None to disable shortcut config map.

* allow change radius of the intersection; set accident_prob=1.0

* 1) introduce goal-agnoistic info["obs/ego/*"], 2) rename keys in info dict

* optimize the reward structure, now:
===== timestep 220 =====
route completion:
	route_completion/goals/default: 0.92
	route_completion/goals/go_straight: 0.50
	route_completion/goals/left_turn: 0.46
	route_completion/goals/right_turn: 0.92
	route_completion/goals/u_turn: 0.53

reward:
	reward/default_reward: 1.27
	reward/goal_agnostic_reward: 0.05
	reward/goals/default: 1.27
	reward/goals/go_straight: 0.14
	reward/goals/left_turn: 0.06
	reward/goals/right_turn: 1.27
	reward/goals/u_turn: 0.01
=======================

* to follow setting, use lane_num=1

* format, ready to launch SB3 td3

* Remove varying dynamics

* lane_num=1

* allow do more visualization

* add default arrive_dest

* add some comments

* now we return full observation for different goals in info["obs/goals/xxx"]

* [DANGER] allow to generate sidewalk for "negative road". Not sure the affect of this commit in other cases. Might need further check.

* Add SIDEWALK to the side detector & the lane line detector.

* use 240line for sidedetector, remove vehicle/lane detector

* fix a bug

* Fix a severe bug that messes up observation

* introduce a penalty for wrong way

* When draw the line to next checkpoint, also draw the line from next ckpt to next next ckpt.

* Add crash_sidewalk_penalty for MetaDrive env, default crash_sidewalk_penalty=0

* Set on_continuous_line_done=False for multigoal env

* Add config "out_of_road_done" for MDEnv

* Remove U turn

* Set out_of_road_done=False

* Add penalty for out_of_route (this might be helpful in multigoal setting)

* Change reward scheme

* Change radius to 12

* remove goal_agnostic_reward

* add GOAL_DEPENDENT_STATE

* up

* change obs

* Add some randomness in map

* enable U turn

* WIP: Now support conventional RL env

* fix bug

* Fix a bug and use Customize Observation

* Better handle lidars' configs

* Remove those hyper diff from MetaDriveEnv

* fix

* Fix

* format

* remove a file

* Add an example notebook for multigoalintersection

* Setup FFpmeg in CI to support video gen in docs

* minor

* minor

* Add docs

* Fix test

* format

* try fix ffmpeg

* Fix test

.github/workflows/main.yml

@@ -280,10 +280,31 @@ jobs:
         run: |
           sudo apt-get -y install xvfb
           sudo /usr/bin/Xvfb :0 -screen 0 1280x1024x24 &
+      - name: Setup FFmpeg
+        uses: FedericoCarboni/setup-ffmpeg@v3
+        id: setup-ffmpeg
+        with:
+          # A specific version to download, may also be "release" or a specific version
+          # like "6.1.0". At the moment semver specifiers (i.e. >=6.1.0) are supported
+          # only on Windows, on other platforms they are allowed but version is matched
+          # exactly regardless.
+          ffmpeg-version: release
+          # Target architecture of the ffmpeg executable to install. Defaults to the
+          # system architecture. Only x64 and arm64 are supported (arm64 only on Linux).
+          architecture: ''
+          # Linking type of the binaries. Use "shared" to download shared binaries and
+          # "static" for statically linked ones. Shared builds are currently only available
+          # for windows releases. Defaults to "static"
+          linking-type: static
+          # As of version 3 of this action, builds are no longer downloaded from GitHub
+          # except on Windows: https://github.com/GyanD/codexffmpeg/releases.
+          github-token: ${{ github.server_url == 'https://github.com' && github.token || '' }}
       - name: Blackbox tests
         run: |
           pip install cython
           pip install numpy
+          pip install mediapy
+          conda install ffmpeg
           pip install -e .
           pip install -e .[gym]
           python -m metadrive.pull_asset

documentation/source/index.rst

@@ -47,6 +47,7 @@ Please feel free to contact us if you have any suggestions or ideas!
    action.ipynb
    reward_cost_done.ipynb
    training.ipynb
+   multigoal_intersection.ipynb
 
 .. toctree::
    :hidden:

metadrive/component/algorithm/BIG.py

@@ -73,9 +73,17 @@ def generate(self, generate_method: str, parameter: Union[str, int]):
             assert isinstance(parameter, int), "When generating map by assigning block num, the parameter should be int"
             self.block_num = parameter + 1
         elif generate_method == BigGenerateMethod.BLOCK_SEQUENCE:
-            assert isinstance(parameter, str), "When generating map from block sequence, the parameter should be a str"
-            self.block_num = len(parameter) + 1
-            self._block_sequence = FirstPGBlock.ID + parameter
+            if isinstance(parameter, list):
+                self.block_num = len(parameter) + 1
+                self._block_sequence = [FirstPGBlock] + parameter
+            else:
+                assert isinstance(
+                    parameter, str
+                ), "When generating map from block sequence, the parameter should be a str. But got {}".format(
+                    type(parameter)
+                )
+                self.block_num = len(parameter) + 1
+                self._block_sequence = FirstPGBlock.ID + parameter
         while True:
             if self.big_helper_func():
                 break
@@ -104,8 +112,11 @@ def sample_block(self) -> PGBlock:
             block_type = self.np_random.choice(block_types, p=block_probabilities)
             block_type = get_metadrive_class(block_type)
         else:
-            type_id = self._block_sequence[len(self.blocks)]
-            block_type = self.block_dist_config.get_block(type_id)
+            if isinstance(self._block_sequence[0], str):
+                type_id = self._block_sequence[len(self.blocks)]
+                block_type = self.block_dist_config.get_block(type_id)
+            else:
+                block_type = self._block_sequence[len(self.blocks)]
 
         socket = self.np_random.choice(self.blocks[-1].get_socket_indices())
         block = block_type(

metadrive/component/algorithm/blocks_prob_dist.py

@@ -37,7 +37,8 @@ class PGBlockDistConfig:
         "Split": 0.00,
         "ParkingLot": 0.00,
         "TollGate": 0.00,
-        "Bidirection": 0.00
+        "Bidirection": 0.00,
+        "StdInterSectionWithUTurn": 0.00
     }
 
     @classmethod

metadrive/component/map/pg_map.py

@@ -20,7 +20,7 @@ def parse_map_config(easy_map_config, new_map_config, default_config):
     assert isinstance(default_config, Config)
 
     # Return the user specified config if overwritten
-    if not default_config["map_config"].is_identical(new_map_config):
+    if easy_map_config is None or not default_config["map_config"].is_identical(new_map_config):
         new_map_config = default_config["map_config"].copy(unchangeable=False).update(new_map_config)
         assert default_config["map"] == easy_map_config
         return new_map_config

metadrive/component/navigation_module/base_navigation.py

@@ -68,6 +68,7 @@ def __init__(
         self.navi_arrow_dir = [0, 0]
         self._dest_node_path = None
         self._goal_node_path = None
+        self._goal_node_path2 = None
 
         self._node_path_list = []
 
@@ -78,15 +79,20 @@ def __init__(
             # nodepath
             self._line_to_dest = self.origin.attachNewNode("line")
             self._goal_node_path = self.origin.attachNewNode("target")
+            self._goal_node_path2 = self.origin.attachNewNode("target2")
             self._dest_node_path = self.origin.attachNewNode("dest")
 
             self._node_path_list.append(self._line_to_dest)
             self._node_path_list.append(self._goal_node_path)
+            self._node_path_list.append(self._goal_node_path2)
             self._node_path_list.append(self._dest_node_path)
 
             if show_navi_mark:
                 navi_point_model = AssetLoader.loader.loadModel(AssetLoader.file_path("models", "box.bam"))
                 navi_point_model.reparentTo(self._goal_node_path)
+
+                navi_point_model2 = AssetLoader.loader.loadModel(AssetLoader.file_path("models", "box.bam"))
+                navi_point_model2.reparentTo(self._goal_node_path2)
             if show_dest_mark:
                 dest_point_model = AssetLoader.loader.loadModel(AssetLoader.file_path("models", "box.bam"))
                 dest_point_model.reparentTo(self._dest_node_path)
@@ -108,18 +114,20 @@ def __init__(
                 line_seg.setColor(self.navi_mark_color[0], self.navi_mark_color[1], self.navi_mark_color[2], 1.0)
                 line_seg.setThickness(4)
                 self._dynamic_line_np_2 = NodePath(line_seg.create(True))
-
                 self._node_path_list.append(self._dynamic_line_np_2)
-
                 self._dynamic_line_np_2.reparentTo(self.origin)
                 self._line_to_navi = line_seg
 
             self._goal_node_path.setTransparency(TransparencyAttrib.M_alpha)
+            self._goal_node_path2.setTransparency(TransparencyAttrib.M_alpha)
             self._dest_node_path.setTransparency(TransparencyAttrib.M_alpha)
 
             self._goal_node_path.setColor(
                 self.navi_mark_color[0], self.navi_mark_color[1], self.navi_mark_color[2], 0.7
             )
+            self._goal_node_path2.setColor(
+                self.navi_mark_color[0], self.navi_mark_color[1], self.navi_mark_color[2], 0.5
+            )
             self._dest_node_path.setColor(
                 self.navi_mark_color[0], self.navi_mark_color[1], self.navi_mark_color[2], 0.7
             )
@@ -180,6 +188,7 @@ def destroy(self):
                 pass
             self._dest_node_path.removeNode()
             self._goal_node_path.removeNode()
+            self._goal_node_path2.removeNode()
 
         for np in self._node_path_list:
             np.detachNode()
@@ -234,12 +243,16 @@ def _draw_line_to_dest(self, start_position, end_position):
         self._dynamic_line_np.hide(CamMask.Shadow | CamMask.RgbCam)
         self._dynamic_line_np.reparentTo(self.origin)
 
-    def _draw_line_to_navi(self, start_position, end_position):
+    def _draw_line_to_navi(self, start_position, end_position, next_checkpoint=None):
         if not self._show_line_to_navi_mark:
             return
         line_seg = self._line_to_navi
         line_seg.moveTo(panda_vector(start_position, self.LINE_TO_DEST_HEIGHT))
         line_seg.drawTo(panda_vector(end_position, self.LINE_TO_DEST_HEIGHT))
+
+        if next_checkpoint is not None:
+            line_seg.drawTo(panda_vector(next_checkpoint, self.LINE_TO_DEST_HEIGHT))
+
         self._dynamic_line_np_2.removeNode()
         self._dynamic_line_np_2 = NodePath(line_seg.create(False))
 

metadrive/component/navigation_module/edge_network_navigation.py

@@ -100,27 +100,37 @@ def update_localization(self, ego_vehicle):
 
         self._navi_info.fill(0.0)
         half = self.CHECK_POINT_INFO_DIM
-        self._navi_info[:half], lanes_heading1, checkpoint = self._get_info_for_checkpoint(
+        self._navi_info[:half], lanes_heading1, next_checkpoint = self._get_info_for_checkpoint(
             lanes_id=0,
             ref_lane=self.map.road_network.get_lane(self.current_checkpoint_lane_index),
             ego_vehicle=ego_vehicle
         )
 
-        self._navi_info[half:], lanes_heading2, _ = self._get_info_for_checkpoint(
+        self._navi_info[half:], lanes_heading2, next_next_checkpoint = self._get_info_for_checkpoint(
             lanes_id=1,
             ref_lane=self.map.road_network.get_lane(self.next_checkpoint_lane_index),
             ego_vehicle=ego_vehicle
         )
 
         if self._show_navi_info:  # Whether to visualize little boxes in the scene denoting the checkpoints
-            pos_of_goal = checkpoint
+            pos_of_goal = next_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)
+
+            pos_of_goal = next_next_checkpoint
+            self._goal_node_path2.setPos(panda_vector(pos_of_goal[0], pos_of_goal[1], self.MARK_HEIGHT))
+            self._goal_node_path2.setH(self._goal_node_path2.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()
-            self._draw_line_to_navi(start_position=ego_vehicle.position, end_position=(navi_pos[0], navi_pos[1]))
+            next_navi_pos = self._goal_node_path2.getPos()
+            self._draw_line_to_navi(
+                start_position=ego_vehicle.position,
+                end_position=(navi_pos[0], navi_pos[1]),
+                next_checkpoint=(next_navi_pos[0], next_navi_pos[1])
+            )
 
     def _update_target_checkpoints(self, ego_lane_index) -> bool:
         """

metadrive/component/navigation_module/node_network_navigation.py

@@ -40,7 +40,7 @@ def __init__(
         self.current_road = None
         self.next_road = None
 
-    def reset(self, vehicle):
+    def reset(self, vehicle, dest=None, random_seed=None):
         possible_lanes = ray_localization(vehicle.heading, vehicle.spawn_place, self.engine, use_heading_filter=False)
         possible_lane_indexes = [lane_index for lane, lane_index, dist in possible_lanes]
 
@@ -56,11 +56,12 @@ def reset(self, vehicle):
             assert len(possible_lanes) > 0
             lane, new_l_index = possible_lanes[0][:-1]
 
-        dest = vehicle.config["destination"]
+        if dest is None:
+            dest = vehicle.config["destination"]
 
         current_lane = lane
         destination = dest if dest is not None else None
-        random_seed = self.engine.global_random_seed
+        random_seed = self.engine.global_random_seed if random_seed is None else random_seed
         assert current_lane is not None, "spawn place is not on road!"
         super(NodeNetworkNavigation, self).reset(current_lane)
         assert self.map.road_network_type == NodeRoadNetwork, "This Navigation module only support NodeRoadNetwork type"
@@ -188,12 +189,12 @@ def update_localization(self, ego_vehicle):
         self._navi_info.fill(0.0)
         half = self.CHECK_POINT_INFO_DIM
         # Put the next checkpoint's information into the first half of the navi_info
-        self._navi_info[:half], lanes_heading1, checkpoint = self._get_info_for_checkpoint(
+        self._navi_info[:half], lanes_heading1, next_checkpoint = self._get_info_for_checkpoint(
             lanes_id=0, ref_lane=self.current_ref_lanes[0], ego_vehicle=ego_vehicle
         )
 
         # Put the next of the next checkpoint's information into the first half of the navi_info
-        self._navi_info[half:], lanes_heading2, _ = self._get_info_for_checkpoint(
+        self._navi_info[half:], lanes_heading2, next_next_checkpoint = self._get_info_for_checkpoint(
             lanes_id=1,
             ref_lane=self.next_ref_lanes[0] if self.next_ref_lanes is not None else self.current_ref_lanes[0],
             ego_vehicle=ego_vehicle
@@ -202,13 +203,23 @@ def update_localization(self, 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
+            pos_of_goal = next_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)
+
+            pos_of_goal = next_next_checkpoint
+            self._goal_node_path2.setPos(panda_vector(pos_of_goal[0], pos_of_goal[1], self.MARK_HEIGHT))
+            self._goal_node_path2.setH(self._goal_node_path2.getH() + 3)
+
             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()
-            self._draw_line_to_navi(start_position=ego_vehicle.position, end_position=(navi_pos[0], navi_pos[1]))
+            next_navi_pos = self._goal_node_path2.getPos()
+            self._draw_line_to_navi(
+                start_position=ego_vehicle.position,
+                end_position=(navi_pos[0], navi_pos[1]),
+                next_checkpoint=(next_navi_pos[0], next_navi_pos[1])
+            )
 
     def _update_target_checkpoints(self, ego_lane_index, ego_lane_longitude) -> bool:
         """
@@ -250,7 +261,9 @@ def get_current_lateral_range(self, current_position, engine) -> float:
 
     def _get_current_lane(self, ego_vehicle):
         """
-        Called in update_localization to find current lane information
+        Called in update_localization to find current lane information. If the vehicle is in the current reference lane,
+        meaning it is not yet moving to the next road segment, then return the current reference lane. Otherwise, return
+        the next reference lane. If the vehicle is not in any of the reference lanes, then return the closest lane.
         """
         possible_lanes, on_lane = ray_localization(
             ego_vehicle.heading, ego_vehicle.position, ego_vehicle.engine, return_on_lane=True

metadrive/component/pgblock/intersection.py

@@ -110,7 +110,6 @@ def _create_part(self, attach_lanes, attach_road: Road, radius: float, intersect
 
         # u-turn
         if self._enable_u_turn_flag:
-            adverse_road = -attach_road
             self._create_u_turn(attach_road, part_idx)
 
         # go forward part
@@ -221,6 +220,17 @@ def _create_left_turn(self, radius, lane_num, attach_left_lane, attach_road, int
             )
 
     def _create_u_turn(self, attach_road, part_idx):
+        """
+        Create a U turn.
+
+        Args:
+            attach_road: the road where the U turn starts.
+            part_idx: in [0, 1, 2, 3]. When part_idx!=0, we grab the lanes from road network. Otherwise we use the
+                initial lanes (positive_lanes).
+
+        Returns:
+            None.
+        """
         # set to CONTINUOUS to debug
         line_type = PGLineType.NONE
         lanes = attach_road.get_lanes(self.block_network) if part_idx != 0 else self.positive_lanes
@@ -253,3 +263,9 @@ def get_intermediate_spawn_lanes(self):
         """Override this function for intersection so that we won't spawn vehicles in the center of intersection."""
         respawn_lanes = self.get_respawn_lanes()
         return respawn_lanes
+
+
+class InterSectionWithUTurn(InterSection):
+    ID = "U"
+    _enable_u_turn_flag = True
+    SOCKET_NUM = 4

metadrive/component/pgblock/pg_block.py

@@ -252,10 +252,15 @@ def create_in_world(self):
                 for _id, lane in enumerate(lanes):
 
                     self._construct_lane(lane, (_from, _to, _id))
+
+                    # choose_side is a two-elemental list, the first element is for left side,
+                    # the second element is for right side. If False, then the left/right side line (broken line or
+                    # continuous line) will not be constructed.
+
                     choose_side = [True, True] if _id == len(lanes) - 1 else [True, False]
-                    if Road(_from, _to).is_negative_road() and _id == 0:
-                        # draw center line with positive road
-                        choose_side = [False, False]
+                    # if Road(_from, _to).is_negative_road() and _id == 0:
+                    #     # draw center line with positive road
+                    #     choose_side = [False, False]
                     self._construct_lane_line_in_block(lane, choose_side)
         self._construct_sidewalk()
         self._construct_crosswalk()

metadrive/component/pgblock/std_intersection.py

@@ -1,9 +1,16 @@
-from metadrive.component.pgblock.intersection import InterSection
 from metadrive.component.pg_space import Parameter
+from metadrive.component.pgblock.intersection import InterSection, InterSectionWithUTurn
 
 
 class StdInterSection(InterSection):
     def _try_plug_into_previous_block(self) -> bool:
         self._config[Parameter.change_lane_num] = 0
         success = super(StdInterSection, self)._try_plug_into_previous_block()
         return success
+
+
+class StdInterSectionWithUTurn(InterSectionWithUTurn):
+    def _try_plug_into_previous_block(self) -> bool:
+        self._config[Parameter.change_lane_num] = 0
+        success = super(StdInterSectionWithUTurn, self)._try_plug_into_previous_block()
+        return success

metadrive/component/road_network/node_road_network.py

@@ -272,6 +272,7 @@ def shortest_path(self, start: str, goal: str) -> List[str]:
         Returns:
             The shortest checkpoints from start to goal.
         """
+        assert isinstance(goal, str)
         start_road_node = start[0]
         assert start != goal
         return next(self.bfs_paths(start_road_node, goal), [])

metadrive/component/sensors/distance_detector.py

@@ -89,7 +89,6 @@ class DistanceDetector(BaseSensor):
     """
     It is a module like lidar, used to detect sidewalk/center line or other static things
     """
-    Lidar_point_cloud_obs_dim = 240
     DEFAULT_HEIGHT = 0.2
 
     # for vis debug
@@ -196,7 +195,7 @@ def __init__(self, engine):
         super(SideDetector, self).__init__(engine)
         self.set_start_phase_offset(90)
         self.origin.hide(CamMask.RgbCam | CamMask.Shadow | CamMask.Shadow | CamMask.DepthCam | CamMask.SemanticCam)
-        self.mask = CollisionGroup.ContinuousLaneLine
+        self.mask = CollisionGroup.ContinuousLaneLine | CollisionGroup.Sidewalk
 
 
 class LaneLineDetector(SideDetector):
@@ -206,4 +205,4 @@ def __init__(self, engine):
         super(SideDetector, self).__init__(engine)
         self.set_start_phase_offset(90)
         self.origin.hide(CamMask.RgbCam | CamMask.Shadow | CamMask.Shadow | CamMask.DepthCam | CamMask.SemanticCam)
-        self.mask = CollisionGroup.ContinuousLaneLine | CollisionGroup.BrokenLaneLine
+        self.mask = CollisionGroup.ContinuousLaneLine | CollisionGroup.BrokenLaneLine | CollisionGroup.Sidewalk