Introducing the bounding box models

metadrive/component/navigation_module/trajectory_navigation.py

@@ -33,9 +33,9 @@ def __init__(
         if show_dest_mark or show_line_to_dest:
             get_logger().warning("show_dest_mark and show_line_to_dest are not supported in TrajectoryNavigation")
         super(TrajectoryNavigation, self).__init__(
-            show_navi_mark=False,
-            show_dest_mark=False,
-            show_line_to_dest=False,
+            show_navi_mark=show_navi_mark,
+            show_dest_mark=show_dest_mark,
+            show_line_to_dest=show_line_to_dest,
             panda_color=panda_color,
             name=name,
             vehicle_config=vehicle_config
@@ -145,6 +145,17 @@ def update_localization(self, ego_vehicle):
         # Use RC as the only criterion to determine arrival in Scenario env.
         self._route_completion = long / self.reference_trajectory.length
 
+        if self._show_navi_info:
+            # Whether to visualize little boxes in the scene denoting the checkpoints
+            pos_of_goal = ckpts[1]
+            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)
+            # 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]))
+
     def get_current_lateral_range(self, current_position, engine) -> float:
         return self.current_lane.width * 2
 

metadrive/component/sensors/semantic_camera.py

@@ -38,7 +38,7 @@ def _setup_effect(self):
                 )
             else:
 
-                if label == Semantics.PEDESTRIAN.label:
+                if label == Semantics.PEDESTRIAN.label and not self.engine.global_config.get("use_bounding_box", False):
                     # PZH: This is a workaround fix to make pedestrians animated.
                     cam.setTagState(
                         label,

metadrive/component/traffic_participants/base_traffic_participant.py

@@ -14,8 +14,8 @@ class BaseTrafficParticipant(BaseObject):
     COLLISION_MASK = CollisionGroup.TrafficParticipants
     HEIGHT = None
 
-    def __init__(self, position: Sequence[float], heading_theta: float = 0., random_seed=None, name=None):
-        super(BaseTrafficParticipant, self).__init__(random_seed=random_seed, name=name)
+    def __init__(self, position: Sequence[float], heading_theta: float = 0., random_seed=None, name=None, config=None):
+        super(BaseTrafficParticipant, self).__init__(random_seed=random_seed, name=name, config=config)
         self.set_position(position, self.HEIGHT / 2 if hasattr(self, "HEIGHT") else 0)
         self.set_heading_theta(heading_theta)
         assert self.MASS is not None, "No mass for {}".format(self.class_name)

metadrive/component/traffic_participants/cyclist.py

@@ -8,7 +8,8 @@
 from metadrive.constants import CollisionGroup
 from metadrive.engine.asset_loader import AssetLoader
 from metadrive.engine.physics_node import BaseRigidBodyNode
-
+from panda3d.core import TransparencyAttrib, LineSegs, NodePath, BoundingHexahedron
+from panda3d.core import Material, Vec3, LVecBase4
 
 class Cyclist(BaseTrafficParticipant):
     MASS = 80  # kg
@@ -19,13 +20,13 @@ class Cyclist(BaseTrafficParticipant):
 
     HEIGHT = 1.75
 
-    def __init__(self, position, heading_theta, random_seed, name=None):
+    def __init__(self, position, heading_theta, random_seed, name=None, *args, **kwargs):
         super(Cyclist, self).__init__(position, heading_theta, random_seed, name=name)
         self.set_metadrive_type(self.TYPE_NAME)
         n = BaseRigidBodyNode(self.name, self.TYPE_NAME)
         self.add_body(n)
 
-        self.body.addShape(BulletBoxShape((self.LENGTH / 2, self.WIDTH / 2, self.HEIGHT / 2)))
+        self.body.addShape(BulletBoxShape((self.WIDTH / 2, self.LENGTH / 2, self.HEIGHT / 2)))
         if self.render:
             if Cyclist.MODEL is None:
                 model = self.loader.loadModel(AssetLoader.file_path("models", "bicycle", "scene.gltf"))
@@ -45,3 +46,161 @@ def WIDTH(self):
     @property
     def LENGTH(self):
         return 1.75
+
+
+class CyclistBoundingBox(BaseTrafficParticipant):
+    MASS = 80  # kg
+    TYPE_NAME = MetaDriveType.CYCLIST
+    COLLISION_MASK = CollisionGroup.TrafficParticipants
+    SEMANTIC_LABEL = Semantics.BIKE.label
+
+    # for random color choosing
+    MATERIAL_COLOR_COEFF = 1.6  # to resist other factors, since other setting may make color dark
+    MATERIAL_METAL_COEFF = 0.1  # 0-1
+    MATERIAL_ROUGHNESS = 0.8  # smaller to make it more smooth, and reflect more light
+    MATERIAL_SHININESS = 128  # 0-128 smaller to make it more smooth, and reflect more light
+    MATERIAL_SPECULAR_COLOR = (3, 3, 3, 3)
+
+    def __init__(self, position, heading_theta, random_seed, name=None, **kwargs):
+        config = {"width": kwargs["width"], "length": kwargs["length"], "height": kwargs["height"]}
+        # config = {"width": kwargs["length"], "length": kwargs["width"], "height": kwargs["height"]}
+        super(CyclistBoundingBox, self).__init__(position, heading_theta, random_seed, name=name, config=config)
+        self.set_metadrive_type(self.TYPE_NAME)
+        n = BaseRigidBodyNode(self.name, self.TYPE_NAME)
+        self.add_body(n)
+
+        self.body.addShape(BulletBoxShape((self.WIDTH / 2, self.LENGTH / 2, self.HEIGHT / 2)))
+        if self.render:
+            model = AssetLoader.loader.loadModel(AssetLoader.file_path("models", "box.bam"))
+            model.setScale((self.WIDTH, self.LENGTH, self.HEIGHT))
+            model.setTwoSided(False)
+            self._instance = model.instanceTo(self.origin)
+
+            # Add some color to help debug
+            from panda3d.core import Material, LVecBase4
+            import seaborn as sns
+
+            show_contour = self.config["show_contour"] if "show_contour" in self.config else False
+            if show_contour:
+                # ========== Draw the contour of the bounding box ==========
+                # Draw the bottom of the car first
+                line_seg = LineSegs("bounding_box_contour1")
+                zoffset = model.getZ()
+                line_seg.setThickness(2)
+                line_color = [0.0, 0.0, 0.0]
+                out_offset = 0.02
+                w = self.WIDTH / 2 + out_offset
+                l = self.LENGTH / 2 + out_offset
+                h = self.HEIGHT / 2 + out_offset
+                line_seg.moveTo(w, l, h + zoffset)
+                line_seg.drawTo(-w, l, h + zoffset)
+                line_seg.drawTo(-w, l, -h + zoffset)
+                line_seg.drawTo(w, l, -h + zoffset)
+                line_seg.drawTo(w, l, h + zoffset)
+
+                # draw cross line
+                line_seg.moveTo(w, l, h + zoffset)
+                line_seg.drawTo(w, -l, -h + zoffset)
+                line_seg.moveTo(w, -l, h + zoffset)
+                line_seg.drawTo(w, l, -h + zoffset)
+
+                line_seg.moveTo(w, -l, h + zoffset)
+                line_seg.drawTo(-w, -l, h + zoffset)
+                line_seg.drawTo(-w, -l, -h + zoffset)
+                line_seg.drawTo(w, -l, -h + zoffset)
+                line_seg.drawTo(w, -l, h + zoffset)
+
+                # draw vertical & horizontal line
+                line_seg.moveTo(-w, l, 0 + zoffset)
+                line_seg.drawTo(-w, -l, 0 + zoffset)
+                line_seg.moveTo(-w, 0, h + zoffset)
+                line_seg.drawTo(-w, 0, -h + zoffset)
+
+                line_seg.moveTo(w, l, h + zoffset)
+                line_seg.drawTo(w, -l, h + zoffset)
+                line_seg.moveTo(-w, l, h + zoffset)
+                line_seg.drawTo(-w, -l, h + zoffset)
+                line_seg.moveTo(-w, l, -h + zoffset)
+                line_seg.drawTo(-w, -l, -h + zoffset)
+                line_seg.moveTo(w, l, -h + zoffset)
+                line_seg.drawTo(w, -l, -h + zoffset)
+                line_np = NodePath(line_seg.create(True))
+                line_material = Material()
+                line_material.setBaseColor(LVecBase4(*line_color[:3], 1))
+                line_np.setMaterial(line_material, True)
+                line_np.reparentTo(self.origin)
+
+            color = sns.color_palette("colorblind")
+            color.remove(color[2])  # Remove the green and leave it for special vehicle
+            idx = 0
+            rand_c = color[idx]
+            rand_c = (1.0, 0.0, 0.0)
+            self._panda_color = rand_c
+            material = Material()
+            material.setBaseColor(
+                (
+                    self.panda_color[0] * self.MATERIAL_COLOR_COEFF, self.panda_color[1] * self.MATERIAL_COLOR_COEFF,
+                    self.panda_color[2] * self.MATERIAL_COLOR_COEFF, 0.
+                )
+            )
+            material.setMetallic(self.MATERIAL_METAL_COEFF)
+            material.setSpecular(self.MATERIAL_SPECULAR_COLOR)
+            material.setRefractiveIndex(1.5)
+            material.setRoughness(self.MATERIAL_ROUGHNESS)
+            material.setShininess(self.MATERIAL_SHININESS)
+            material.setTwoside(False)
+            self.origin.setMaterial(material, True)
+
+    def reset(self, position, heading_theta: float = 0., random_seed=None, name=None, *args, **kwargs):
+        super(CyclistBoundingBox, self).reset(position, heading_theta, random_seed, name, *args, **kwargs)
+        config = {"width": kwargs["width"], "length": kwargs["length"], "height": kwargs["height"]}
+        self.update_config(config)
+        if self._instance is not None:
+            self._instance.detachNode()
+        if self.render:
+            model = AssetLoader.loader.loadModel(AssetLoader.file_path("models", "box.bam"))
+            model.setScale((self.WIDTH, self.LENGTH, self.HEIGHT))
+            model.setTwoSided(False)
+            self._instance = model.instanceTo(self.origin)
+
+            # Add some color to help debug
+            from panda3d.core import Material, LVecBase4
+            import seaborn as sns
+            color = sns.color_palette("colorblind")
+            color.remove(color[2])  # Remove the green and leave it for special vehicle
+            idx = 0
+            rand_c = color[idx]
+            rand_c = (1.0, 0.0, 0.0)
+            self._panda_color = rand_c
+            material = Material()
+            material.setBaseColor(
+                (
+                    self.panda_color[0] * self.MATERIAL_COLOR_COEFF, self.panda_color[1] * self.MATERIAL_COLOR_COEFF,
+                    self.panda_color[2] * self.MATERIAL_COLOR_COEFF, 0.
+                )
+            )
+            material.setMetallic(self.MATERIAL_METAL_COEFF)
+            material.setSpecular(self.MATERIAL_SPECULAR_COLOR)
+            material.setRefractiveIndex(1.5)
+            material.setRoughness(self.MATERIAL_ROUGHNESS)
+            material.setShininess(self.MATERIAL_SHININESS)
+            material.setTwoside(False)
+            self.origin.setMaterial(material, True)
+
+    def set_velocity(self, direction, value=None, in_local_frame=False):
+        super(CyclistBoundingBox, self).set_velocity(direction, value, in_local_frame)
+        self.standup()
+
+    @property
+    def WIDTH(self):
+        # return self.config["width"]
+        return self.config["length"]
+
+    @property
+    def HEIGHT(self):
+        return self.config["height"]
+
+    @property
+    def LENGTH(self):
+        # return self.config["length"]
+        return self.config["width"]