feat: publish global → proj_frame FrameTransform from proj_frame_offset

AGENTS.md

@@ -47,7 +47,8 @@
 - Common types in history: `feat`, `fix`, `docs`, `style`, `refactor`, `test`, `chore`, `ci`.
 - **Always sign commits**: use `git commit -s -S` (DCO sign-off + GPG signature) for every commit.
 - **Never mention "Copilot"** in commit messages, PR descriptions, code comments, or any other repository content. Do not add `Co-authored-by: Copilot` trailers.
-- PRs should include a short summary, testing notes (e.g., `yarn test`, `yarn lint`), and link related issues. CODEOWNERS are listed in `CODEOWNERS`; follow the repo’s required review rules.
+- **Documentation drift check**: When changing behavior (frame names, output formats, conventions), always grep `docs/` and code comments for references to the old behavior and update them in the same commit.
+- PRs should include a short summary, testing notes (e.g., `yarn test`, `yarn lint`), and link related issues. CODEOWNERS are listed in `CODEOWNERS`; follow the repo's required review rules.
 
 ## Lint & Autofix Safety
 

package.json

@@ -36,6 +36,7 @@
     "@lichtblick/suite": "^1.25.0",
     "@types/jest": "^30.0.0",
     "@types/minimatch": "^5.1.2",
+    "@types/proj4": "^2.5.6",
     "@types/react": "18.2.55",
     "@types/react-dom": "18.2.19",
     "create-lichtblick-extension": "^1.0.0",
@@ -46,6 +47,7 @@
     "jest": "^29.7.0",
     "jest-environment-jsdom": "^29.7.0",
     "prettier": "3.6.2",
+    "proj4": "^2.20.9",
     "react": "18.2.0",
     "react-dom": "18.2.0",
     "ts-essentials": "^10.1.1",

src/config/frameTransformNames.ts

@@ -3,3 +3,17 @@ export const OSI_GLOBAL_FRAME = "global";
 export const OSI_EGO_VEHICLE_BB_CENTER_FRAME = "ego_vehicle_bb_center";
 export const OSI_EGO_VEHICLE_REAR_AXLE_FRAME = "ego_vehicle_rear_axle";
 export const OSI_SENSORDATA_VIRTUAL_MOUNTING_POSITION_FRAME = "virtual_mounting_position";
+
+/**
+ * Shared frame convention: "proj_frame" represents the geographic CRS world.
+ *
+ * When GroundTruth.proj_frame_offset is present, the OSI converter publishes
+ * a FrameTransform with parent_frame_id="global" and child_frame_id="proj_frame".
+ * This keeps "global" as the root of the frame tree (consistent with ego
+ * transforms) and tells Lichtblick how to resolve "proj_frame" entities
+ * (OpenDRIVE map geometry) into the "global" coordinate space.
+ *
+ * See: ASAM OSI GroundTruth.proj_frame_offset (osi_groundtruth.proto field 20)
+ * See: ASAM OpenDRIVE §8.5 <offset> (same affine formula)
+ */
+export const OSI_PROJ_FRAME = "proj_frame";

src/converters/groundTruth/frameTransformConverter.ts

@@ -2,13 +2,14 @@ import { FrameTransform, FrameTransforms } from "@foxglove/schemas";
 import { GroundTruth } from "@lichtblick/asam-osi-types";
 import { MessageConverterAlert, MessageConverterContext, VariableValue } from "@lichtblick/suite";
 import { osiTimestampToTime } from "@utils/helper";
-import { eulerToQuaternion } from "@utils/math";
+import { eulerToQuaternion, invertQuaternion, pointRotationByQuaternion } from "@utils/math";
 import { DeepRequired } from "ts-essentials";
 
 import {
   OSI_GLOBAL_FRAME,
   OSI_EGO_VEHICLE_BB_CENTER_FRAME,
   OSI_EGO_VEHICLE_REAR_AXLE_FRAME,
+  OSI_PROJ_FRAME,
 } from "@/config/frameTransformNames";
 
 export const convertGroundTruthToFrameTransforms = (
@@ -105,6 +106,13 @@ export const convertGroundTruthToFrameTransforms = (
       };
       emitAlert?.(alert, "groundtruth-frametransforms-missing-bbcenter-to-rear");
     }
+
+    // [OSI §GT] Publish global → proj_frame when proj_frame_offset is present.
+    // This allows OpenDRIVE map geometry (published in "proj_frame") to align
+    // with OSI objects (published in "global").
+    if (message.proj_frame_offset?.position) {
+      transforms.transforms.push(buildProjFrameTransform(message as DeepRequired<GroundTruth>));
+    }
   } catch (error) {
     console.error(
       "Error during FrameTransform message conversion:\n%s\nSkipping message! (Input message not compatible?)",
@@ -163,3 +171,44 @@ function buildEgoVehicleRearAxleFrameTransform(
     rotation: eulerToQuaternion(0, 0, 0),
   };
 }
+
+/**
+ * Build FrameTransform placing "proj_frame" (CRS world) as a child of "global" (OSI inertial).
+ *
+ * The proj_frame_offset defines where the "global" origin sits in "proj_frame" coordinates:
+ *   - position: translation of "global" origin in "proj_frame" (tx, ty, tz)
+ *   - yaw: rotation of "global" axes relative to "proj_frame"
+ *
+ * We publish parent="global", child="proj_frame" so that "global" stays the root of the
+ * frame tree (consistent with ego transforms: global → ego_vehicle_bb_center).
+ * This requires inverting the offset using the existing math helpers:
+ *   R_inv = invertQuaternion(R(yaw))
+ *   t_inv = -pointRotationByQuaternion(t, R_inv)
+ *
+ * Lichtblick resolves entities in "proj_frame" (OpenDRIVE map) through this chain,
+ * enabling alignment with OSI objects in "global".
+ */
+function buildProjFrameTransform(osiGroundTruth: DeepRequired<GroundTruth>): FrameTransform {
+  const offset = osiGroundTruth.proj_frame_offset;
+
+  // Original rotation: "global" rotated by yaw in "proj_frame"
+  const rotation = eulerToQuaternion(0, 0, offset.yaw);
+  // Inverse rotation: "proj_frame" rotated in "global"
+  const rotationInv = invertQuaternion(rotation);
+
+  // Inverse translation: rotate original offset by inverse, then negate
+  const t = { x: offset.position.x, y: offset.position.y, z: offset.position.z };
+  const rotatedT = pointRotationByQuaternion(t, rotationInv);
+
+  return {
+    timestamp: osiTimestampToTime(osiGroundTruth.timestamp),
+    parent_frame_id: OSI_GLOBAL_FRAME,
+    child_frame_id: OSI_PROJ_FRAME,
+    translation: {
+      x: -rotatedT.x,
+      y: -rotatedT.y,
+      z: -rotatedT.z,
+    },
+    rotation: rotationInv,
+  };
+}

tests/frameTransformConverter.spec.ts

@@ -184,3 +184,74 @@ describe("frameTransformConverter — host vehicle ID divergence", () => {
     );
   });
 });
+
+describe("frameTransformConverter — proj_frame_offset", () => {
+  it("publishes proj_frame transform when proj_frame_offset is present", () => {
+    const msg = minimalGroundTruth({
+      proj_frame_offset: {
+        position: { x: 349210.32, y: 5648717.38, z: 0 },
+        yaw: 0.029,
+      },
+    });
+
+    const result = convertGroundTruthToFrameTransforms(msg, undefined, undefined, undefined);
+
+    // Should have 3 transforms: bb_center, rear_axle, proj_frame
+    expect(result.transforms).toHaveLength(3);
+
+    const projTransform = result.transforms.find(
+      (t) => t.parent_frame_id === "global" && t.child_frame_id === "proj_frame",
+    )!;
+    expect(projTransform).toBeDefined();
+
+    // Inverted offset: t_inv = -R(-yaw) * t
+    const yaw = 0.029;
+    const cosYaw = Math.cos(yaw);
+    const sinYaw = Math.sin(yaw);
+    const tx = 349210.32;
+    const ty = 5648717.38;
+    expect(projTransform.translation.x).toBeCloseTo(-(tx * cosYaw + ty * sinYaw), 2);
+    expect(projTransform.translation.y).toBeCloseTo(tx * sinYaw - ty * cosYaw, 2);
+    expect(projTransform.translation.z).toBeCloseTo(0, 6);
+  });
+
+  it("does not publish proj_frame transform when proj_frame_offset is missing", () => {
+    const msg = minimalGroundTruth();
+    const result = convertGroundTruthToFrameTransforms(msg, undefined, undefined, undefined);
+
+    expect(result.transforms).toHaveLength(2);
+    const projTransform = result.transforms.find(
+      (t) => t.parent_frame_id === "global" && t.child_frame_id === "proj_frame",
+    );
+    expect(projTransform).toBeUndefined();
+  });
+
+  it("does not publish proj_frame transform when position is missing from offset", () => {
+    const msg = minimalGroundTruth({
+      proj_frame_offset: { yaw: 0.5 },
+    });
+    const result = convertGroundTruthToFrameTransforms(msg, undefined, undefined, undefined);
+
+    expect(result.transforms).toHaveLength(2);
+  });
+
+  it("handles proj_frame_offset with zero yaw", () => {
+    const msg = minimalGroundTruth({
+      proj_frame_offset: {
+        position: { x: 1000, y: 2000, z: 50 },
+      },
+    });
+
+    const result = convertGroundTruthToFrameTransforms(msg, undefined, undefined, undefined);
+    expect(result.transforms).toHaveLength(3);
+
+    const projTransform = result.transforms.find(
+      (t) => t.parent_frame_id === "global" && t.child_frame_id === "proj_frame",
+    )!;
+    expect(projTransform).toBeDefined();
+    // With yaw=0, inversion is simply negation
+    expect(projTransform.translation).toEqual({ x: -1000, y: -2000, z: -50 });
+    // rotation with yaw=0 inverted should still produce identity quaternion
+    expect(projTransform.rotation).toEqual({ x: 0, y: 0, z: 0, w: 1 });
+  });
+});