Add sidewalk support for real-world env (#545)

* test env

* procedural API

* debug sidewalk

* anti-clock wise polygon

* polygon maker

* show sidewalk

* disable sidewalk for render pipe

* set height

* sidewalk works

* side walk

* win test

* add shader/add windows

* no __init__

* add shaders to pip

* fix

* fix pull asset

* asset update

.github/workflows/main.yml

@@ -226,6 +226,26 @@ jobs:
           cd metadrive/
           pytest --cov=./ --cov-config=.coveragerc --cov-report=xml -sv tests/test_examples
 
+  test_policy_windows:
+    runs-on: windows-latest
+    steps:
+      - uses: actions/checkout@v2
+      - name: Set up Python 3.9
+        uses: actions/setup-python@v2
+        with:
+          python-version: 3.9
+      - name: Blackbox tests
+        run: |
+          pip install cython
+          pip install numpy
+          pip install -e .
+          python -m metadrive.pull_asset
+          pip install pytest
+          pip install pytest-cov
+          pip install ray
+          cd metadrive/
+          pytest --cov=./ --cov-config=.coveragerc --cov-report=xml -sv tests/test_policy
+
   test_ipynb:
     runs-on: ubuntu-latest
     steps:

.gitignore

@@ -12,6 +12,7 @@
 /metadrive.egg-info/
 .DS_Store
 **/.DS_Store
+**/**.zip.lock
 **/test_10maps.json
 /metadrive/examples/**.png
 /metadrive/examples/**.jpg

MANIFEST.in

@@ -4,4 +4,8 @@ recursive-include metadrive\examples\ *
 
 recursive-include metadrive/render_pipeline/ *
 recursive-include metadrive\\render_pipeline\\ *
-recursive-include metadrive\render_pipeline\ *
+recursive-include metadrive\render_pipeline\ *
+
+recursive-include metadrive/shaders/ *
+recursive-include metadrive\\shaders\\ *
+recursive-include metadrive\shaders\ *

metadrive/base_class/base_object.py

@@ -276,6 +276,7 @@ def set_position(self, position, height=None):
         """
         Set this object to a place, the default value is the regular height for red car
         :param position: 2d array or list
+        :param height: give a fixed height
         """
         assert len(position) == 2 or len(position) == 3
         if len(position) == 3:

metadrive/component/block/base_block.py

@@ -89,11 +89,16 @@ def __init__(
 
             # side
             self.side_texture = self.loader.loadTexture(AssetLoader.file_path("textures", "sidewalk", "color.png"))
-            self.side_texture.set_format(Texture.F_srgb)
+            # self.side_texture.set_format(Texture.F_srgb)
+            self.side_texture.setWrapU(Texture.WM_repeat)
+            self.side_texture.setWrapV(Texture.WM_repeat)
             self.side_texture.setMinfilter(SamplerState.FT_linear_mipmap_linear)
             self.side_texture.setAnisotropicDegree(8)
             self.side_normal = self.loader.loadTexture(AssetLoader.file_path("textures", "sidewalk", "normal.png"))
-            self.side_normal.set_format(Texture.F_srgb)
+            # self.side_normal.set_format(Texture.F_srgb)
+            self.side_normal.setWrapU(Texture.WM_repeat)
+            self.side_normal.setWrapV(Texture.WM_repeat)
+
             self.sidewalk = self.loader.loadModel(AssetLoader.file_path("models", "box.bam"))
             self.sidewalk.setTwoSided(False)
             self.sidewalk.setTexture(self.ts_color, self.side_texture)

metadrive/component/map/base_map.py

@@ -243,6 +243,10 @@ def get_height_map(
 
             center_p = self.get_center_point()
             need_scale = abs(extension - 1) > 1e-1
+
+            for sidewalk in self.sidewalks.values():
+                polygons.append(sidewalk["polygon"])
+
             for polygon in polygons:
                 points = [
                     [

metadrive/component/scenario_block/scenario_block.py

@@ -1,18 +1,24 @@
 import math
-
+from metadrive.constants import CollisionGroup
+from metadrive.utils.utils import time_me
 import numpy as np
+from panda3d.bullet import BulletConvexHullShape, BulletTriangleMeshShape, BulletTriangleMesh
+from panda3d.core import Vec3, LQuaternionf, LPoint3f
 
 from metadrive.component.block.base_block import BaseBlock
 from metadrive.component.lane.scenario_lane import ScenarioLane
 from metadrive.component.road_network.edge_road_network import EdgeRoadNetwork
 from metadrive.constants import DrivableAreaProperty
 from metadrive.constants import PGLineType, PGLineColor
 from metadrive.engine.engine_utils import get_engine
+from metadrive.engine.physics_node import BulletRigidBodyNode
 from metadrive.scenario.scenario_description import ScenarioDescription
 from metadrive.type import MetaDriveType
-from metadrive.utils.coordinates_shift import panda_heading
+from metadrive.utils.coordinates_shift import panda_vector
 from metadrive.utils.interpolating_line import InterpolatingLine
+from metadrive.utils.math import Vector
 from metadrive.utils.math import norm
+from metadrive.utils.vertex import make_polygon_model
 
 
 class ScenarioBlock(BaseBlock):
@@ -96,6 +102,7 @@ def create_in_world(self):
             # TODO LQY: DO we need sidewalk?
             elif MetaDriveType.is_road_boundary_line(type):
                 self.construct_continuous_line(np.asarray(data[ScenarioDescription.POLYLINE]), color=PGLineColor.GREY)
+        self.construct_sidewalk()
 
     def construct_continuous_line(self, polyline, color):
         line = InterpolatingLine(polyline)
@@ -129,37 +136,35 @@ def construct_broken_line(self, polyline, color):
             node_path_list = ScenarioLane.construct_lane_line_segment(self, start, end, color, PGLineType.BROKEN)
             self._node_path_list.extend(node_path_list)
 
-    def construct_sidewalk(self, polyline):
-        line = InterpolatingLine(polyline)
-        seg_len = DrivableAreaProperty.LANE_SEGMENT_LENGTH
-        segment_num = int(line.length / seg_len)
-        # last_theta = None
-        for segment in range(segment_num):
-            lane_start = line.get_point(segment * seg_len)
-            lane_end = line.get_point((segment + 1) * seg_len)
-            if segment == segment_num - 1:
-                lane_end = line.get_point(line.length)
-            direction_v = lane_end - lane_start
-            theta = panda_heading(math.atan2(direction_v[1], direction_v[0]))
-            # if segment == segment_num - 1:
-            #     factor = 1
-            # else:
-            #     factor = 1.25
-            #     if last_theta is not None:
-            #         diff = wrap_to_pi(theta) - wrap_to_pi(last_theta)
-            #         if diff > 0:
-            #             factor += math.sin(abs(diff) / 2) * DrivableAreaProperty.SIDEWALK_WIDTH / norm(
-            #                 lane_start[0] - lane_end[0], lane_start[1] - lane_end[1]
-            #             ) + 0.15
-            #         else:
-            #             factor -= math.sin(abs(diff) / 2) * DrivableAreaProperty.SIDEWALK_WIDTH / norm(
-            #                 lane_start[0] - lane_end[0], lane_start[1] - lane_end[1]
-            #             )
-            last_theta = theta
-            node_path_list = ScenarioLane.construct_sidewalk_segment(
-                self, lane_start, lane_end, length_multiply=1, extra_thrust=0, width=0.2
-            )
-            self._node_path_list.extend(node_path_list)
+    def construct_sidewalk(self):
+        """
+        Construct the sidewalk with collision shape
+        """
+        if self.engine.global_config["show_sidewalk"] and not self.engine.use_render_pipeline:
+            for sidewalk in self.sidewalks.values():
+                polygon = sidewalk["polygon"]
+                np = make_polygon_model(polygon, 0.2)
+                np.reparentTo(self.sidewalk_node_path)
+                np.setPos(0, 0, 0.1)
+                np.setTexture(self.side_texture)
+                # np.setTexture(self.ts_normal, self.side_normal)
+
+                body_node = BulletRigidBodyNode(MetaDriveType.BOUNDARY_SIDEWALK)
+                body_node.setKinematic(False)
+                body_node.setStatic(True)
+                body_np = self.sidewalk_node_path.attachNewNode(body_node)
+                body_np.setPos(0, 0, 0.1)
+                self._node_path_list.append(body_np)
+
+                geom = np.node().getGeom(0)
+                mesh = BulletTriangleMesh()
+                mesh.addGeom(geom)
+                shape = BulletTriangleMeshShape(mesh, dynamic=False)
+
+                body_node.addShape(shape)
+                self.dynamic_nodes.append(body_node)
+                body_node.setIntoCollideMask(CollisionGroup.Sidewalk)
+                self._node_path_list.append(np)
 
     @property
     def block_network_type(self):

metadrive/component/static_object/traffic_object.py

@@ -60,7 +60,7 @@ def __init__(self, position, heading_theta, lane=None, static: bool = False, ran
             model = self.loader.loadModel(AssetLoader.file_path("models", "traffic_cone", "scene.gltf"))
             # model.node().setTag("type", "vehicle")
             model.setScale(0.02, 0.02, 0.025)
-            model.setPos(0, 0, -self.HEIGHT / 2 + 0.05)
+            model.setPos(0, 0, -self.HEIGHT / 2 - 0.05)
             model.reparentTo(self.origin)
 
     @property
@@ -140,7 +140,7 @@ def __init__(self, position, heading_theta, lane=None, static: bool = False, ran
         if self.render:
             model = self.loader.loadModel(AssetLoader.file_path("models", "barrier", "scene.gltf"))
             model.setH(-90)
-            model.setPos(0, 0, -0.93)
+            model.setPos(0, 0, -1.05)
             model.setScale(0.7)
             model.reparentTo(self.origin)
 

metadrive/constants.py

@@ -200,7 +200,7 @@ def collision_rules(cls):
             (cls.Sidewalk, cls.Sidewalk, False),
             (cls.Sidewalk, cls.LidarBroadDetector, False),
             (cls.Sidewalk, cls.TrafficObject, True),
-            (cls.Sidewalk, cls.TrafficParticipants, False),  # don't allow sidewalk contact
+            (cls.Sidewalk, cls.TrafficParticipants, True),  # don't allow sidewalk contact
 
             # LidarBroadDetector
             (cls.LidarBroadDetector, cls.LidarBroadDetector, False),

metadrive/engine/core/terrain.py

@@ -231,6 +231,10 @@ def _set_terrain_shader(self, engine, attribute_tex):
             self._mesh_terrain.set_shader_input("grass_normal", self.grass_normal)
             self._mesh_terrain.set_shader_input("grass_rough", self.grass_rough)
             self._mesh_terrain.set_shader_input("grass_tex_ratio", self.grass_tex_ratio)
+            #
+            # # side
+            # self._mesh_terrain.set_shader_input("side_tex", self.side_tex)
+            # self._mesh_terrain.set_shader_input("side_normal", self.side_normal)
 
             # road
             self._mesh_terrain.set_shader_input("rock_tex", self.rock_tex)
@@ -423,6 +427,20 @@ def _load_mesh_terrain_textures(self, engine, anisotropic_degree=16, filter_type
             tex.setMagfilter(filter_type)
             tex.setAnisotropicDegree(anisotropic_degree)
 
+        # # sidewalk
+        # self.side_tex = self.loader.loadTexture(AssetLoader.file_path("textures", "sidewalk", "color.png"))
+        # self.side_normal = self.loader.loadTexture(AssetLoader.file_path("textures", "sidewalk", "normal.png"))
+        #
+        # v_wrap = Texture.WMRepeat
+        # u_warp = Texture.WMMirror
+        #
+        # for tex in [self.side_tex, self.side_normal]:
+        #     tex.set_wrap_u(u_warp)
+        #     tex.set_wrap_v(v_wrap)
+        #     tex.setMinfilter(filter_type)
+        #     tex.setMagfilter(filter_type)
+        #     tex.setAnisotropicDegree(anisotropic_degree)
+
         # Road surface
         # self.road_texture = self.loader.loadTexture(AssetLoader.file_path("textures", "sci", "new_color.png"))
         self.road_texture = self.loader.loadTexture(AssetLoader.file_path("textures", "asphalt", "diff_2k.png"))

metadrive/envs/real_data_envs/nuscenes_env.py

@@ -9,10 +9,9 @@
 if __name__ == "__main__":
     env = NuScenesEnv(
         {
-            "use_render": False,
-            "no_map": False,
+            "use_render": True,
             "agent_policy": ReplayEgoCarPolicy,
-            # "manual_control": True,
+            "manual_control": True,
             "show_interface": False,
             # "need_lane_localization": False,
             # "image_observation": True,
@@ -24,33 +23,20 @@
             # "sequential_seed": True,
             "reactive_traffic": True,
             "curriculum_level": 1,
-            "show_fps": False,
+            "show_fps": True,
+            "show_sidewalk": True,
+            # "pstats": True,
+            # "use_mesh_terrain": True,
             # "debug": True,
             "no_static_vehicles": False,
             # "pstats": True,
-            "render_pipeline": True,
-            # "daytime": "22:01",
-            # "no_traffic": True,
-            # "no_light": False,
-            # "debug":True,
-            # Make video
-            # "episodes_to_evaluate_curriculum": 5,
+            # "render_pipeline": True,
             "window_size": (1600, 900),
             "camera_dist": 9,
-            # "camera_height": 0.5,
-            # "camera_pitch": np.pi / 3,
-            # "camera_fov": 60,
-            # "force_render_fps": 10,
             "start_scenario_index": 0,
             "num_scenarios": 10,
             # "force_reuse_object_name": True,
-            # "data_directory": "/home/shady/Downloads/test_processed",
             "horizon": 1000,
-            # "no_static_vehicles": True,
-            # "show_policy_mark": True,
-            # "show_coordinates": True,
-            # "force_destroy": True,
-            # "default_vehicle_in_traffic": True,
             "vehicle_config": dict(
                 # light=True,
                 # random_color=True,
@@ -60,7 +46,8 @@
                 lane_line_detector=dict(num_lasers=0, distance=50),
                 side_detector=dict(num_lasers=12, distance=50)
             ),
-            "data_directory": "/home/shady/data/scenarionet/dataset/nuscenes"
+            "data_directory": AssetLoader.file_path("nuscenes"),
+            # "drivable_area_extension": 0,
         }
     )
 
@@ -95,12 +82,12 @@
                     },
                     # mode="topdown"
                 )
-            if tm or tc:
-                print(
-                    "Time elapse: {:.4f}. Average FPS: {:.4f}, AVG_Reset_time: {:.4f}".format(
-                        time.time() - start, s / (time.time() - start - reset_used_time), reset_used_time / reset_num
-                    )
-                )
-                print("seed:{}, success".format(env.engine.global_random_seed))
-                print(list(env.engine.curriculum_manager.recent_success.dict.values()))
-                break
+            # if tm or tc:
+            #     print(
+            #         "Time elapse: {:.4f}. Average FPS: {:.4f}, AVG_Reset_time: {:.4f}".format(
+            #             time.time() - start, s / (time.time() - start - reset_used_time), reset_used_time / reset_num
+            #         )
+            #     )
+            #     print("seed:{}, success".format(env.engine.global_random_seed))
+            #     print(list(env.engine.curriculum_manager.recent_success.dict.values()))
+            #     break

metadrive/envs/scenario_env.py

@@ -48,6 +48,7 @@
     default_vehicle_in_traffic=False,
     skip_missing_light=True,
     static_traffic_object=True,
+    show_sidewalk=False,
 
     # ===== Agent config =====
     vehicle_config=dict(

metadrive/pull_asset.py

@@ -54,18 +54,13 @@ def pull_asset(update):
     try:
         with lock.acquire():
             # Fetch the zip file
-            logger.info("Pull assets from {}".format(ASSET_URL))
+            logger.info("Pull assets from {} to {}".format(ASSET_URL, zip_path))
             urllib.request.urlretrieve(ASSET_URL, zip_path, MyProgressBar())
             with zipfile.ZipFile(zip_path, 'r') as zip_ref:
                 zip_ref.extractall(TARGET_DIR)
             logger.info(
                 "Successfully download assets, version: {}. MetaDrive version: {}".format(asset_version(), VERSION)
             )
-            # Remove the downloaded zip file (optional)
-        if os.path.exists(zip_path):
-            os.remove(zip_path)
-        if os.path.exists(zip_lock):
-            os.remove(zip_lock)
 
     except Timeout:
         logger.info(
@@ -78,6 +73,13 @@ def pull_asset(update):
 
         logger.info("Assets are now available.")
 
+    finally:
+        # Remove the downloaded zip file (optional)
+        if os.path.exists(zip_path):
+            os.remove(zip_path)
+        if os.path.exists(zip_lock):
+            os.remove(zip_lock)
+
 
 if __name__ == '__main__':
     parser = argparse.ArgumentParser()