SimGen-related code

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/base_camera.py

@@ -97,7 +97,7 @@ def _make_cuda_texture(self):
     def enable_cuda(self):
         return self is not None and self._enable_cuda
 
-    def get_image(self, base_object):
+    def get_image(self, base_object, mode="bgr"):
         """
         Put camera to an object and get the image.
         """
@@ -107,13 +107,18 @@ def get_image(self, base_object):
         self.cam.reparentTo(base_object.origin)
         img = self.get_rgb_array_cpu()
         self.track(original_parent, original_position, original_hpr)
-        return img
+        if mode == "bgr":
+            return img
+        elif mode == "rgb":
+            return img[..., ::-1]
+        else:
+            raise ValueError("Unknown mode: {}".format(mode))
 
     def save_image(self, base_object, name="debug.png"):
         """
         Put camera to an object and save the image to the disk
         """
-        img = self.get_image(base_object)
+        img = self.get_image(base_object, mode="bgr")
         cv2.imwrite(name, img)
 
     def track(self, new_parent_node: NodePath, position, hpr):

metadrive/component/sensors/instance_camera.py

@@ -1,4 +1,8 @@
-from panda3d.core import RenderState, LightAttrib, ColorAttrib, ShaderAttrib, TextureAttrib, FrameBufferProperties
+from typing import Union
+
+import numpy as np
+from panda3d.core import NodePath
+from panda3d.core import RenderState, LightAttrib, ColorAttrib, ShaderAttrib, TextureAttrib
 
 from metadrive.component.sensors.base_camera import BaseCamera
 from metadrive.constants import CamMask
@@ -21,11 +25,16 @@ def track(self, new_parent_node, position, hpr):
         self._setup_effect()
         super().track(new_parent_node, position, hpr)
 
+    def perceive(
+        self, to_float=True, new_parent_node: Union[NodePath, None] = None, position=None, hpr=None
+    ) -> np.ndarray:
+        self._setup_effect()
+        return super().perceive(to_float, new_parent_node, position, hpr)
+
     def _setup_effect(self):
         """
-        Use tag to apply color to different object class
+        Use tag to apply color to different objects (instances)
         Returns: None
-
         """
         # setup camera
 
@@ -34,6 +43,11 @@ def _setup_effect(self):
         else:
             mapping = get_engine().id_c
             spawned_objects = get_engine().get_objects()
+
+            ##Ensure consistency between color mapping and the objects actually active in the engine.
+            mapping_set, object_set = set(list(mapping.keys())), set(list(spawned_objects.keys()))
+            assert (len(mapping_set.difference(object_set)) == 0)
+
             for id, obj in spawned_objects.items():
                 obj.origin.setTag(CameraTagStateKey.ID, id)
             cam = self.get_cam().node()
@@ -44,5 +58,8 @@ def _setup_effect(self):
                     ColorAttrib.makeFlat((0, 0, 0, 1)), 1
                 )
             )
-            for id, c in mapping.items():
+
+            for id in spawned_objects.keys():
+                c = mapping[id]
+                assert c in self.engine.COLORS_OCCUPIED
                 cam.setTagState(id, RenderState.make(ColorAttrib.makeFlat((c[0], c[1], c[2], 1)), 1))

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

@@ -1,11 +1,9 @@
-from metadrive.component.traffic_participants.base_traffic_participant import BaseTrafficParticipant
-from typing import Tuple
-
 from panda3d.bullet import BulletBoxShape
-from panda3d.bullet import BulletCylinderShape
+from panda3d.core import LineSegs, NodePath
 
-from metadrive.constants import MetaDriveType, Semantics
+from metadrive.component.traffic_participants.base_traffic_participant import BaseTrafficParticipant
 from metadrive.constants import CollisionGroup
+from metadrive.constants import MetaDriveType, Semantics
 from metadrive.engine.asset_loader import AssetLoader
 from metadrive.engine.physics_node import BaseRigidBodyNode
 
@@ -19,13 +17,29 @@ class Cyclist(BaseTrafficParticipant):
 
     HEIGHT = 1.75
 
-    def __init__(self, position, heading_theta, random_seed, name=None):
+    DEFAULT_LENGTH = 1.75  # meters
+    DEFAULT_HEIGHT = 1.75  # meters
+    DEFAULT_WIDTH = 0.4  # meters
+
+    @property
+    def LENGTH(self):
+        return self.DEFAULT_LENGTH
+
+    @property
+    def HEIGHT(self):
+        return self.DEFAULT_HEIGHT
+
+    @property
+    def WIDTH(self):
+        return self.DEFAULT_WIDTH
+
+    def __init__(self, position, heading_theta, random_seed, name=None, **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"))
@@ -38,10 +52,169 @@ def set_velocity(self, direction, value=None, in_local_frame=False):
         super(Cyclist, self).set_velocity(direction, value, in_local_frame)
         self.standup()
 
+    def get_state(self):
+        state = super(Cyclist, self).get_state()
+        state.update({
+            "length": self.LENGTH,
+            "width": self.WIDTH,
+            "height": self.HEIGHT,
+        })
+        return state
+
+
+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 0.4
+        # return self.config["width"]
+        return self.config["length"]
+
+    @property
+    def HEIGHT(self):
+        return self.config["height"]
 
     @property
     def LENGTH(self):
-        return 1.75
+        # return self.config["length"]
+        return self.config["width"]