Cleanup (#551)

* rename return raw style

* clean lanes

* optimize sidewalk for PGMap

* use sidewalk optimize

* format

* fix bug

* fix bug

* fix rigid body bug

* cleanup bullet body node error

* format

* add more check

* fix

* fix functionality bug

* fix test

* fix parking lot

* format

* fix bug & add toolds

* fix bug

* fix warning

* fix sidewalk bug

* vertex normal

* format

* all log once

* format

* fix

bridges/ros_bridge/ros_socket_server.py

@@ -46,7 +46,7 @@ def run(self, test=False):
                 image_source="main_camera",
                 show_navi_mark=False,
             ),
-            data_directory=AssetLoader.file_path("nuscenes", return_raw_style=False),
+            data_directory=AssetLoader.file_path("nuscenes", unix_style=False),
         )
 
         env = ScenarioEnv(config)

metadrive/base_class/base_object.py

@@ -1,4 +1,6 @@
 import copy
+from abc import ABC
+
 from metadrive.type import MetaDriveType
 
 import logging
@@ -7,7 +9,7 @@
 
 import numpy as np
 import seaborn as sns
-from panda3d.bullet import BulletWorld, BulletBodyNode
+from panda3d.bullet import BulletWorld, BulletBodyNode, BulletVehicle
 from panda3d.core import LVector3, NodePath, PandaNode
 from metadrive.constants import Semantics
 from metadrive.base_class.base_runnable import BaseRunnable
@@ -96,6 +98,8 @@ def detach_from_physics_world(self, bullet_world: BulletWorld):
             return
         for node in self:
             bullet_world.remove(node)
+            if isinstance(node, BulletVehicle):
+                break
         self.attached = False
 
     def destroy_node_list(self, bullet_world: BulletWorld):
@@ -104,7 +108,7 @@ def destroy_node_list(self, bullet_world: BulletWorld):
         self.clear()
 
 
-class BaseObject(BaseRunnable, MetaDriveType):
+class BaseObject(BaseRunnable, MetaDriveType, ABC):
     """
     BaseObject is something interacting with game engine. If something is expected to have a body in the world or have
     appearance in the world, it must be a subclass of BaseObject.

metadrive/component/block/base_block.py

@@ -1,27 +1,30 @@
 import logging
-from metadrive.constants import Semantics
 import math
-from typing import List, Dict
+from abc import ABC
+from typing import Dict
 
 from panda3d.bullet import BulletBoxShape, BulletGhostNode
-from panda3d.core import Vec3, LQuaternionf, Vec4, TextureStage, RigidBodyCombiner, \
-    SamplerState, NodePath, Texture, Material
+from panda3d.bullet import BulletTriangleMeshShape, BulletTriangleMesh
+from panda3d.core import Vec3, LQuaternionf, RigidBodyCombiner, \
+    SamplerState, NodePath, Texture
 
 from metadrive.base_class.base_object import BaseObject
-from metadrive.component.lane.abs_lane import AbstractLane
-from metadrive.component.lane.point_lane import PointLane
 from metadrive.component.road_network.node_road_network import NodeRoadNetwork
 from metadrive.component.road_network.road import Road
-from metadrive.constants import MetaDriveType, CamMask, PGLineType, PGLineColor, DrivableAreaProperty
+from metadrive.constants import CollisionGroup
+from metadrive.constants import MetaDriveType, CamMask, PGLineType, PGLineColor, PGDrivableAreaProperty
+from metadrive.constants import Semantics
 from metadrive.engine.asset_loader import AssetLoader
 from metadrive.engine.core.physics_world import PhysicsWorld
+from metadrive.engine.physics_node import BulletRigidBodyNode
 from metadrive.utils.coordinates_shift import panda_vector, panda_heading
 from metadrive.utils.math import norm
+from metadrive.utils.vertex import make_polygon_model
 
 logger = logging.getLogger(__name__)
 
 
-class BaseBlock(BaseObject, DrivableAreaProperty):
+class BaseBlock(BaseObject, PGDrivableAreaProperty, ABC):
     """
     Block is a driving area consisting of several roads
     Note: overriding the _sample() function to fill block_network/respawn_roads in subclass
@@ -51,42 +54,11 @@ def __init__(
         self._respawn_roads = []
         self._block_objects = None
 
-        if self.render and not self.use_render_pipeline:
-            self.ts_color = TextureStage("color")
-            self.ts_normal = TextureStage("normal")
-            self.ts_normal.setMode(TextureStage.M_normal)
-
-            # Only maintain one copy of asset
-            if self.naive_draw_map:
-                self.road_texture = self.loader.loadTexture(AssetLoader.file_path("textures", "sci", "new_color.png"))
-                self.road_normal = self.loader.loadTexture(AssetLoader.file_path("textures", "sci", "normal.jpg"))
-                self.road_texture.set_format(Texture.F_srgb)
-                self.road_normal.set_format(Texture.F_srgb)
-                self.road_texture.setMinfilter(SamplerState.FT_linear_mipmap_linear)
-                self.road_texture.setAnisotropicDegree(8)
-
-                # # continuous line
-                # self.lane_line_model = self.loader.loadModel(AssetLoader.file_path("models", "box.bam"))
-                # self.lane_line_model.setPos(0, 0, -DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT / 2)
-                self.lane_line_texture = self.loader.loadTexture(AssetLoader.file_path("textures", "sci", "floor.jpg"))
-            # self.lane_line_model.setScale(DrivableAreaProperty.STRIPE_LENGTH*4,
-            #                                    DrivableAreaProperty.LANE_LINE_WIDTH,
-            #                                    DrivableAreaProperty.LANE_LINE_THICKNESS)
-            # # self.lane_line_normal = self.loader.loadTexture(
-            # #     AssetLoader.file_path("textures", "sci", "floor_normal.jpg"))
-            # # self.lane_line_texture.set_format(Texture.F_srgb)
-            # # self.lane_line_normal.set_format(Texture.F_srgb)
-            # self.lane_line_model.setTexture(self.ts_color, self.lane_line_texture)
-            # # self.lane_line_model.setTexture(self.ts_normal, self.lane_line_normal)
-            #
-            # # # broken line
-            # self.broken_lane_line_model = self.loader.loadModel(AssetLoader.file_path("models", "box.bam"))
-            # self.broken_lane_line_model.setScale(DrivableAreaProperty.STRIPE_LENGTH,
-            #                                           DrivableAreaProperty.LANE_LINE_WIDTH,
-            #                                           DrivableAreaProperty.LANE_LINE_THICKNESS)
-            # self.broken_lane_line_model.setPos(0, 0, -DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT / 2)
-            # self.broken_lane_line_model.setTexture(self.ts_color, self.lane_line_texture)
+        # polygons representing crosswalk and sidewalk
+        self.crosswalks = {}
+        self.sidewalks = {}
 
+        if self.render and not self.use_render_pipeline:
             # side
             self.side_texture = self.loader.loadTexture(AssetLoader.file_path("textures", "sidewalk", "color.png"))
             # self.side_texture.set_format(Texture.F_srgb)
@@ -101,9 +73,7 @@ def __init__(
 
             self.sidewalk = self.loader.loadModel(AssetLoader.file_path("models", "box.bam"))
             self.sidewalk.setTwoSided(False)
-            self.sidewalk.setTexture(self.ts_color, self.side_texture)
-            # self.sidewalk = self.loader.loadModel(AssetLoader.file_path("models", "output.egg"))
-            # self.sidewalk.setTexture(self.ts_normal, self.side_normal)
+            self.sidewalk.setTexture(self.side_texture)
 
     def _sample_topology(self) -> bool:
         """
@@ -160,6 +130,8 @@ def destruct_block(self, physics_world: PhysicsWorld):
         for obj in self._block_objects:
             obj.destroy()
         self._block_objects = None
+        self.sidewalks = {}
+        self.crosswalks = {}
 
     def construct_from_config(self, config: Dict, root_render_np: NodePath, physics_world: PhysicsWorld):
         success = self.construct_block(root_render_np, physics_world, config)
@@ -222,7 +194,7 @@ def _create_in_world(self, skip=False):
 
         self.sidewalk_node_path.flattenStrong()
         self.sidewalk_node_path.node().collect()
-        self.sidewalk_node_path.hide(CamMask.ScreenshotCam)
+        self.sidewalk_node_path.hide(CamMask.ScreenshotCam | CamMask.Shadow)
 
         # only bodies reparent to this node
         self.lane_node_path.flattenStrong()
@@ -238,9 +210,6 @@ def _create_in_world(self, skip=False):
         self.lane_line_node_path.reparentTo(self.origin)
         self.lane_node_path.reparentTo(self.origin)
 
-        if self.naive_draw_map:
-            self.lane_vis_node_path.reparentTo(self.origin)
-
         try:
             self._bounding_box = self.block_network.get_bounding_box()
         except:
@@ -278,24 +247,6 @@ def set_state(self, state: Dict):
         """
         pass
 
-    def _add_lane_line(self, lane: AbstractLane, colors: List[Vec4], contruct_two_side=True):
-        raise DeprecationWarning("Leave for argoverse using")
-        if isinstance(lane, PointLane):
-            parent_np = self.lane_line_node_path
-            lane_width = lane.width_at(0)
-            for c, i in enumerate([-1, 1]):
-                line_color = colors[c]
-                acc_length = 0
-                if lane.line_types[c] == PGLineType.CONTINUOUS:
-                    for segment in lane.segment_property:
-                        lane_start = lane.position(acc_length, i * lane_width / 2)
-                        acc_length += segment["length"]
-                        lane_end = lane.position(acc_length, i * lane_width / 2)
-                        middle = (lane_start + lane_end) / 2
-                        self._add_lane_line2bullet(
-                            lane_start, lane_end, middle, parent_np, line_color, lane.line_types[c]
-                        )
-
     def _add_box_body(self, lane_start, lane_end, middle, parent_np: NodePath, line_type, line_color):
         raise DeprecationWarning("Useless, currently")
         length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1])
@@ -312,14 +263,14 @@ def _add_box_body(self, lane_start, lane_end, middle, parent_np: NodePath, line_
         self._node_path_list.append(body_np)
 
         shape = BulletBoxShape(
-            Vec3(length / 2, DrivableAreaProperty.LANE_LINE_WIDTH / 2, DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT)
+            Vec3(length / 2, PGDrivableAreaProperty.LANE_LINE_WIDTH / 2, PGDrivableAreaProperty.LANE_LINE_GHOST_HEIGHT)
         )
         body_np.node().addShape(shape)
-        mask = DrivableAreaProperty.CONTINUOUS_COLLISION_MASK if line_type != PGLineType.BROKEN else DrivableAreaProperty.BROKEN_COLLISION_MASK
+        mask = PGDrivableAreaProperty.CONTINUOUS_COLLISION_MASK if line_type != PGLineType.BROKEN else PGDrivableAreaProperty.BROKEN_COLLISION_MASK
         body_np.node().setIntoCollideMask(mask)
         self.static_nodes.append(body_np.node())
 
-        body_np.setPos(panda_vector(middle, DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT / 2))
+        body_np.setPos(panda_vector(middle, PGDrivableAreaProperty.LANE_LINE_GHOST_HEIGHT / 2))
         direction_v = lane_end - lane_start
         # theta = -numpy.arctan2(direction_v[1], direction_v[0])
         theta = panda_heading(math.atan2(direction_v[1], direction_v[0]))
@@ -344,6 +295,8 @@ def destroy(self):
         self.PART_IDX = 0
         self.ROAD_IDX = 0
         self._respawn_roads.clear()
+        self.crosswalks = {}
+        self.sidewalks = {}
         self._global_network = None
         super(BaseBlock, self).destroy()
 
@@ -355,11 +308,36 @@ def destroy(self):
     def bounding_box(self):
         return self._bounding_box
 
-    @property
-    def naive_draw_map(self):
+    def construct_sidewalk(self):
         """
-        Use the most naive way to draw map
-        Returns: Boolean
-
+        Construct the sidewalk with collision shape
         """
-        return self.engine is None and self.render
+        if self.engine is None or (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, PGDrivableAreaProperty.SIDEWALK_THICKNESS)
+                np.reparentTo(self.sidewalk_node_path)
+                np.setPos(0, 0, PGDrivableAreaProperty.SIDEWALK_THICKNESS / 2)
+                if self.render:
+                    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)
+                if self.render:
+                    self.dynamic_nodes.append(body_node)
+                else:
+                    self.static_nodes.append(body_node)
+                body_node.setIntoCollideMask(CollisionGroup.Sidewalk)
+                self._node_path_list.append(np)