feat: add 3d model setting

Author: myemural

common/utils/marker.ts

@@ -692,7 +692,8 @@ export function objectToModelPrimitive(
   length: number,
   height: number,
   color: Color,
-  data: Uint8Array,
+  url = "",
+  data: Uint8Array = new Uint8Array(),
 ): ModelPrimitive {
   return {
     pose: {
@@ -710,8 +711,8 @@ export function objectToModelPrimitive(
     },
     color,
     override_color: false,
-    url: "",
+    url,
     media_type: "model/gltf-binary",
-    data,
+    data: url.length === 0 ? data : new Uint8Array(),
   };
 }

src/index.ts

@@ -1,5 +1,6 @@
 import {
   LineType,
+  ModelPrimitive,
   SceneEntityDeletionType,
   SceneUpdate,
   TextPrimitive,
@@ -39,7 +40,11 @@ import {
 import { ExtensionContext, Immutable, MessageEvent, PanelSettings } from "@lichtblick/suite";
 import { eulerToQuaternion, quaternionMultiplication } from "@utils/geometry";
 import { ColorCode } from "@utils/helper";
-import { objectToCubePrimitive, pointListToTriangleListPrimitive } from "@utils/marker";
+import {
+  objectToCubePrimitive,
+  pointListToTriangleListPrimitive,
+  objectToModelPrimitive,
+} from "@utils/marker";
 import { PartialSceneEntity, generateSceneEntityId } from "@utils/scene";
 import { DeepPartial, DeepRequired } from "ts-essentials";
 
@@ -82,15 +87,35 @@ type Config = {
   showAxes: boolean;
   showPhysicalLanes: boolean;
   showLogicalLanes: boolean;
+  showBoundingBox: boolean;
+  show3dModels: boolean;
 };
 
+function createModelPrimitive(movingObject: DeepRequired<MovingObject>): ModelPrimitive {
+  const model_primitive = objectToModelPrimitive(
+    movingObject.base.position.x,
+    movingObject.base.position.y,
+    movingObject.base.position.z - movingObject.base.dimension.height / 2,
+    movingObject.base.orientation.roll,
+    movingObject.base.orientation.pitch,
+    movingObject.base.orientation.yaw,
+    1,
+    1,
+    1,
+    { r: 0, g: 0, b: 0, a: 0 },
+    "file://" + movingObject.model_reference, // Should be absolute path
+  );
+  return model_primitive;
+}
+
 function buildObjectEntity(
   osiObject: DeepRequired<MovingObject> | DeepRequired<StationaryObject>,
   color: Color,
   id_prefix: string,
   frame_id: string,
   time: Time,
   config: Config | undefined,
+  modelCache: Map<string, ModelPrimitive>,
   metadata?: KeyValuePair[],
 ): PartialSceneEntity {
   const cube = objectToCubePrimitive(
@@ -148,15 +173,39 @@ function buildObjectEntity(
     ];
   }
 
+  function getUpdatedModelPrimitives(): ModelPrimitive[] {
+    if (config != null && config.show3dModels) {
+      const model_primitive = modelCache.get(osiObject.model_reference);
+      if (model_primitive == undefined) {
+        return [];
+      }
+
+      model_primitive.pose.position.x = osiObject.base.position.x;
+      model_primitive.pose.position.y = osiObject.base.position.y;
+      model_primitive.pose.position.z =
+        osiObject.base.position.z - osiObject.base.dimension.height / 2;
+      model_primitive.pose.orientation = eulerToQuaternion(
+        osiObject.base.orientation.roll,
+        osiObject.base.orientation.pitch,
+        osiObject.base.orientation.yaw,
+      );
+
+      return [model_primitive];
+    }
+
+    return [];
+  }
+
   return {
     timestamp: time,
     frame_id,
     id: generateSceneEntityId(id_prefix, osiObject.id.value),
     lifetime: { sec: 0, nsec: 0 },
     frame_locked: true,
-    cubes: [cube],
+    cubes: config != null && config.showBoundingBox ? [cube] : [],
     arrows: buildAxes(),
     metadata,
+    models: getUpdatedModelPrimitives(),
   };
 }
 
@@ -389,6 +438,7 @@ function buildSceneEntities(
   osiGroundTruth: DeepRequired<GroundTruth>,
   updateFlags: OSISceneEntitesUpdate,
   config: Config | undefined,
+  modelCache: Map<string, ModelPrimitive>,
 ): OSISceneEntities {
   const time: Time = osiTimestampToTime(osiGroundTruth.timestamp);
 
@@ -403,6 +453,12 @@ function buildSceneEntities(
           value: MovingObject_Type[obj.type],
         },
       ];
+
+      const modelPathKey = obj.model_reference;
+      if (!modelCache.has(modelPathKey) && modelPathKey.startsWith("/")) {
+        modelCache.set(modelPathKey, createModelPrimitive(obj));
+      }
+
       if (obj.id.value === osiGroundTruth.host_vehicle_id.value) {
         metadata = [...metadata, ...buildVehicleMetadata(obj.vehicle_classification)];
         entity = buildObjectEntity(
@@ -412,6 +468,7 @@ function buildSceneEntities(
           ROOT_FRAME,
           time,
           config,
+          modelCache,
           metadata,
         );
       } else {
@@ -429,6 +486,7 @@ function buildSceneEntities(
           ROOT_FRAME,
           time,
           config,
+          modelCache,
           metadata,
         );
       }
@@ -449,6 +507,7 @@ function buildSceneEntities(
         ROOT_FRAME,
         time,
         config,
+        modelCache,
         metadata,
       );
     });
@@ -761,6 +820,7 @@ export function activate(extensionContext: ExtensionContext): void {
   let groundTruthFrameCache = new WeakMap<GroundTruth, PartialSceneEntity[]>(); // Weakly stores scene entities for each individual OSI ground truth frame
   const laneBoundaryCache = new Map<string, PartialSceneEntity[]>(); // Note: A maximum of one entry is kept in this cache.
   const laneCache = new Map<string, PartialSceneEntity[]>(); // Note: A maximum of one entry is kept in this cache.
+  const modelCache = new Map<string, ModelPrimitive>(); // This cache will hold the first time loaded models with model path key
 
   const state = {
     previousMovingObjectIds: new Set<number>(),
@@ -912,7 +972,7 @@ export function activate(extensionContext: ExtensionContext): void {
         logicalLaneBoundaries,
         lanes,
         logicalLanes,
-      } = buildSceneEntities(osiGroundTruthReq, updateFlags, config);
+      } = buildSceneEntities(osiGroundTruthReq, updateFlags, config, modelCache);
 
       // Concatenate newly generated and cached scene entities
       sceneEntities = [
@@ -1078,6 +1138,16 @@ export function activate(extensionContext: ExtensionContext): void {
               input: "boolean",
               value: config?.showLogicalLanes,
             },
+            showBoundingBox: {
+              label: "Show Bounding Box",
+              input: "boolean",
+              value: config?.showBoundingBox,
+            },
+            show3dModels: {
+              label: "Show 3D Models",
+              input: "boolean",
+              value: config?.show3dModels,
+            },
           },
         }),
         handler: (action, config: Config | undefined) => {
@@ -1096,12 +1166,20 @@ export function activate(extensionContext: ExtensionContext): void {
           if (action.action === "update" && action.payload.path[2] === "showLogicalLanes") {
             config.showLogicalLanes = action.payload.value as boolean;
           }
+          if (action.action === "update" && action.payload.path[2] === "showBoundingBox") {
+            config.showBoundingBox = action.payload.value as boolean;
+          }
+          if (action.action === "update" && action.payload.path[2] === "show3dModels") {
+            config.show3dModels = action.payload.value as boolean;
+          }
         },
         defaultConfig: {
           caching: true,
           showAxes: true,
           showPhysicalLanes: true,
           showLogicalLanes: false,
+          showBoundingBox: true,
+          show3dModels: false,
         },
       }),
     },

src/trafficlights/index.ts

@@ -40,7 +40,8 @@ export const buildTrafficLightModel = (
     item.base.dimension.length,
     item.base.dimension.height,
     color,
-    modelCacheMap.get(mapKey)!,
+    "",
+    modelCacheMap.get(mapKey),
   );
 };
 

src/trafficsigns/index.ts

@@ -54,7 +54,8 @@ export const buildTrafficSignModel = (
     item.base.dimension.length,
     item.base.dimension.height,
     { r: 0, g: 0, b: 0, a: 0 },
-    modelSignCacheMap[category].get(mapKey)!,
+    "",
+    modelSignCacheMap[category].get(mapKey),
   );
 };