Update OpenDrive conversion

Author: DanielDauner

.flake8

@@ -16,4 +16,7 @@ ignore =
     E731
     # Local variable name is assigned to but never used
     F841
+per-file-ignores =
+    # imported but unused
+    __init__.py: F401
 max-line-length = 120

d123/common/visualization/matplotlib/observation.py

@@ -5,6 +5,7 @@
 import shapely.geometry as geom
 
 from d123.common.datatypes.detection.detection import BoxDetectionWrapper, TrafficLightDetectionWrapper
+from d123.common.datatypes.detection.detection_types import DetectionType
 from d123.common.datatypes.vehicle_state.ego_state import EgoStateSE2, EgoStateSE3
 from d123.common.geometry.base import Point2D
 from d123.common.geometry.bounding_box.bounding_box import BoundingBoxSE2, BoundingBoxSE3
@@ -27,6 +28,7 @@
 from d123.dataset.maps.abstract_map import AbstractMap
 from d123.dataset.maps.abstract_map_objects import AbstractLane
 from d123.dataset.maps.map_datatypes import MapLayer
+from d123.dataset.scene.abstract_scene import AbstractScene
 
 
 def add_default_map_on_ax(
@@ -87,6 +89,39 @@ def add_box_detections_to_ax(ax: plt.Axes, box_detections: BoxDetectionWrapper)
         add_bounding_box_to_ax(ax, box_detection.bounding_box, plot_config)
 
 
+def add_box_future_detections_to_ax(ax: plt.Axes, scene: AbstractScene, iteration: int) -> None:
+
+    # TODO: Refactor this function
+    initial_agents = scene.get_box_detections_at_iteration(iteration)
+    agents_poses = {
+        agent.metadata.track_token: [agent.center_se3]
+        for agent in initial_agents
+        if agent.metadata.detection_type == DetectionType.VEHICLE
+    }
+    frequency = 1
+    for iteration in range(iteration + frequency, scene.get_number_of_iterations(), frequency):
+        agents = scene.get_box_detections_at_iteration(iteration)
+        for agent in agents:
+            if agent.metadata.track_token in agents_poses:
+                agents_poses[agent.metadata.track_token].append(agent.center_se3)
+
+    for track_token, poses in agents_poses.items():
+        if len(poses) < 2:
+            continue
+        poses = np.array([pose.point_2d.array for pose in poses])
+        num_poses = poses.shape[0]
+        alphas = 1 - np.linspace(0.2, 1.0, num_poses)  # Start low, end high
+        for i in range(num_poses - 1):
+            ax.plot(
+                poses[i : i + 2, 0],
+                poses[i : i + 2, 1],
+                color=BOX_DETECTION_CONFIG[DetectionType.VEHICLE].fill_color.hex,
+                alpha=alphas[i + 1],
+                linewidth=BOX_DETECTION_CONFIG[DetectionType.VEHICLE].line_width * 5,
+                zorder=BOX_DETECTION_CONFIG[DetectionType.VEHICLE].zorder,
+            )
+
+
 def add_ego_vehicle_to_ax(ax: plt.Axes, ego_vehicle_state: Union[EgoStateSE3, EgoStateSE2]) -> None:
     add_bounding_box_to_ax(ax, ego_vehicle_state.bounding_box, EGO_VEHICLE_CONFIG)
 

d123/common/visualization/viser/server.py

@@ -33,7 +33,7 @@
 LINE_WIDTH: float = 4.0
 
 # Bounding box config:
-BOUNDING_BOX_TYPE: Literal["mesh", "lines"] = "mesh"
+BOUNDING_BOX_TYPE: Literal["mesh", "lines"] = "lines"
 
 # Map config:
 MAP_AVAILABLE: bool = True
@@ -182,6 +182,9 @@ def _(_) -> None:
                 else:
                     raise ValueError(f"Unknown bounding box type: {BOUNDING_BOX_TYPE}")
 
+                current_frame_handle.remove()
+                current_frame_handle = mew_frame_handle
+
                 for camera_type in VISUALIZE_CAMERA_GUI:
                     if camera_type in scene.available_camera_types:
                         camera_gui_handles[camera_type].image = scene.get_camera_at_iteration(
@@ -208,8 +211,6 @@ def _(_) -> None:
                 rendering_time = time.time() - start
                 sleep_time = 1.0 / gui_framerate.value - rendering_time
                 time.sleep(max(sleep_time, 0.0))
-                current_frame_handle.remove()
-                current_frame_handle = mew_frame_handle
                 self.server.flush()  # Optional!
 
             # Load in frames.

d123/common/visualization/viser/utils.py

@@ -97,14 +97,15 @@ def get_map_meshes(scene: AbstractScene):
     center = initial_ego_vehicle_state.center_se3
     map_layers = [
         MapLayer.LANE_GROUP,
-        # MapLayer.LANE,
-        # MapLayer.WALKWAY,
+        MapLayer.LANE,
+        MapLayer.WALKWAY,
         MapLayer.CROSSWALK,
-        # MapLayer.CARPARK,
+        MapLayer.CARPARK,
         MapLayer.GENERIC_DRIVABLE,
     ]
 
     map_objects_dict = scene.map_api.get_proximal_map_objects(center.point_2d, radius=MAP_RADIUS, layers=map_layers)
+    print(map_objects_dict.keys())
     output = {}
 
     for map_layer in map_objects_dict.keys():
@@ -115,13 +116,13 @@ def get_map_meshes(scene: AbstractScene):
             if map_layer in [
                 MapLayer.WALKWAY,
                 MapLayer.CROSSWALK,
-                # MapLayer.GENERIC_DRIVABLE,
+                MapLayer.GENERIC_DRIVABLE,
                 MapLayer.CARPARK,
             ]:
                 # Push meshes up by a few centimeters to avoid overlap with the ground in the visualization.
                 trimesh_mesh.vertices -= Point3D(x=center.x, y=center.y, z=center.z - 0.1).array
             else:
-                trimesh_mesh.vertices -= Point3D(x=center.x, y=center.y, z=center.z + 0.1).array
+                trimesh_mesh.vertices -= Point3D(x=center.x, y=center.y, z=center.z).array
 
             if not scene.log_metadata.map_has_z:
                 trimesh_mesh.vertices += Point3D(
@@ -131,7 +132,6 @@ def get_map_meshes(scene: AbstractScene):
             trimesh_mesh = configure_trimesh(trimesh_mesh, MAP_SURFACE_CONFIG[map_layer].fill_color)
             surface_meshes.append(trimesh_mesh)
         output[f"{map_layer.serialize()}"] = trimesh.util.concatenate(surface_meshes)
-
     return output