3D: support PoseInFrame and PosesInFrame

packages/studio-base/src/panels/ThreeDeeRender/foxglove.ts

@@ -11,6 +11,12 @@ addFoxgloveSchema(POINTCLOUD_DATATYPES, "foxglove.PointCloud");
 export const SCENE_UPDATE_DATATYPES = new Set<string>();
 addFoxgloveSchema(SCENE_UPDATE_DATATYPES, "foxglove.SceneUpdate");
 
+export const POSE_IN_FRAME_DATATYPES = new Set<string>();
+addFoxgloveSchema(POSE_IN_FRAME_DATATYPES, "foxglove.PoseInFrame");
+
+export const POSES_IN_FRAME_DATATYPES = new Set<string>();
+addFoxgloveSchema(POSES_IN_FRAME_DATATYPES, "foxglove.PosesInFrame");
+
 // Expand a single Foxglove dataType into variations for ROS1 and ROS2 then add
 // them to the given output set
 function addFoxgloveSchema(output: Set<string>, dataType: string): Set<string> {

packages/studio-base/src/panels/ThreeDeeRender/renderables/PoseArrays.ts

@@ -5,6 +5,7 @@
 import * as THREE from "three";
 
 import { toNanoSec } from "@foxglove/rostime";
+import { PosesInFrame } from "@foxglove/schemas/schemas/typescript";
 import { SettingsTreeAction, SettingsTreeFields, Topic } from "@foxglove/studio";
 import type { RosValue } from "@foxglove/studio-base/players/types";
 
@@ -13,8 +14,9 @@ import { Renderer } from "../Renderer";
 import { PartialMessage, PartialMessageEvent, SceneExtension } from "../SceneExtension";
 import { SettingsTreeEntry } from "../SettingsManager";
 import { makeRgba, rgbaGradient, rgbaToCssString, stringToRgba } from "../color";
+import { POSES_IN_FRAME_DATATYPES } from "../foxglove";
 import { vecEqual } from "../math";
-import { normalizeHeader, normalizePose } from "../normalizeMessages";
+import { normalizeHeader, normalizePose, normalizeTime } from "../normalizeMessages";
 import {
   PoseArray,
   POSE_ARRAY_DATATYPES,
@@ -93,6 +95,7 @@ export type PoseArrayUserData = BaseUserData & {
   settings: LayerSettingsPoseArray;
   topic: string;
   poseArrayMessage: PoseArray;
+  originalMessage: Record<string, RosValue>;
   axes: Axis[];
   arrows: RenderableArrow[];
   lineStrip?: RenderableLineStrip;
@@ -107,7 +110,7 @@ export class PoseArrayRenderable extends Renderable<PoseArrayUserData> {
   }
 
   public override details(): Record<string, RosValue> {
-    return this.userData.poseArrayMessage;
+    return this.userData.originalMessage;
   }
 
   public removeArrows(): void {
@@ -140,6 +143,7 @@ export class PoseArrays extends SceneExtension<PoseArrayRenderable> {
     super("foxglove.PoseArrays", renderer);
 
     renderer.addDatatypeSubscriptions(POSE_ARRAY_DATATYPES, this.handlePoseArray);
+    renderer.addDatatypeSubscriptions(POSES_IN_FRAME_DATATYPES, this.handlePosesInFrame);
     renderer.addDatatypeSubscriptions(NAV_PATH_DATATYPES, this.handleNavPath);
   }
 
@@ -148,7 +152,11 @@ export class PoseArrays extends SceneExtension<PoseArrayRenderable> {
     const handler = this.handleSettingsAction;
     const entries: SettingsTreeEntry[] = [];
     for (const topic of this.renderer.topics ?? []) {
-      if (POSE_ARRAY_DATATYPES.has(topic.datatype) || NAV_PATH_DATATYPES.has(topic.datatype)) {
+      if (
+        POSE_ARRAY_DATATYPES.has(topic.datatype) ||
+        NAV_PATH_DATATYPES.has(topic.datatype) ||
+        POSES_IN_FRAME_DATATYPES.has(topic.datatype)
+      ) {
         const config = (configTopics[topic.name] ?? {}) as Partial<LayerSettingsPoseArray>;
         const displayType = config.type ?? getDefaultType(topic);
         const { axisScale, lineWidth } = config;
@@ -208,6 +216,7 @@ export class PoseArrays extends SceneExtension<PoseArrayRenderable> {
       this._updatePoseArrayRenderable(
         renderable,
         renderable.userData.poseArrayMessage,
+        renderable.userData.originalMessage,
         renderable.userData.receiveTime,
         { ...DEFAULT_SETTINGS, ...settings },
       );
@@ -217,7 +226,7 @@ export class PoseArrays extends SceneExtension<PoseArrayRenderable> {
   private handlePoseArray = (messageEvent: PartialMessageEvent<PoseArray>): void => {
     const poseArrayMessage = normalizePoseArray(messageEvent.message);
     const receiveTime = toNanoSec(messageEvent.receiveTime);
-    this.addPoseArray(messageEvent.topic, poseArrayMessage, receiveTime);
+    this.addPoseArray(messageEvent.topic, poseArrayMessage, messageEvent.message, receiveTime);
   };
 
   private handleNavPath = (messageEvent: PartialMessageEvent<NavPath>): void => {
@@ -227,10 +236,21 @@ export class PoseArrays extends SceneExtension<PoseArrayRenderable> {
 
     const poseArrayMessage = normalizeNavPathToPoseArray(messageEvent.message);
     const receiveTime = toNanoSec(messageEvent.receiveTime);
-    this.addPoseArray(messageEvent.topic, poseArrayMessage, receiveTime);
+    this.addPoseArray(messageEvent.topic, poseArrayMessage, messageEvent.message, receiveTime);
   };
 
-  private addPoseArray(topic: string, poseArrayMessage: PoseArray, receiveTime: bigint): void {
+  private handlePosesInFrame = (messageEvent: PartialMessageEvent<PosesInFrame>): void => {
+    const poseArrayMessage = normalizePosesInFrameToPoseArray(messageEvent.message);
+    const receiveTime = toNanoSec(messageEvent.receiveTime);
+    this.addPoseArray(messageEvent.topic, poseArrayMessage, messageEvent.message, receiveTime);
+  };
+
+  private addPoseArray(
+    topic: string,
+    poseArrayMessage: PoseArray,
+    originalMessage: Record<string, RosValue>,
+    receiveTime: bigint,
+  ): void {
     let renderable = this.renderables.get(topic);
     if (!renderable) {
       // Set the initial settings from default values merged with any user settings
@@ -249,6 +269,7 @@ export class PoseArrays extends SceneExtension<PoseArrayRenderable> {
         settings,
         topic,
         poseArrayMessage,
+        originalMessage,
         axes: [],
         arrows: [],
       });
@@ -260,6 +281,7 @@ export class PoseArrays extends SceneExtension<PoseArrayRenderable> {
     this._updatePoseArrayRenderable(
       renderable,
       poseArrayMessage,
+      originalMessage,
       receiveTime,
       renderable.userData.settings,
     );
@@ -338,13 +360,15 @@ export class PoseArrays extends SceneExtension<PoseArrayRenderable> {
   private _updatePoseArrayRenderable(
     renderable: PoseArrayRenderable,
     poseArrayMessage: PoseArray,
+    originalMessage: Record<string, RosValue>,
     receiveTime: bigint,
     settings: LayerSettingsPoseArray,
   ): void {
     renderable.userData.receiveTime = receiveTime;
     renderable.userData.messageTime = toNanoSec(poseArrayMessage.header.stamp);
     renderable.userData.frameId = this.renderer.normalizeFrameId(poseArrayMessage.header.frame_id);
     renderable.userData.poseArrayMessage = poseArrayMessage;
+    renderable.userData.originalMessage = originalMessage;
 
     const { topic, settings: prevSettings } = renderable.userData;
     const axisOrArrowSettingsChanged =
@@ -485,6 +509,13 @@ function normalizeNavPathToPoseArray(navPath: PartialMessage<NavPath>): PoseArra
   };
 }
 
+function normalizePosesInFrameToPoseArray(poseArray: PartialMessage<PosesInFrame>): PoseArray {
+  return {
+    header: { stamp: normalizeTime(poseArray.timestamp), frame_id: poseArray.frame_id ?? "" },
+    poses: poseArray.poses?.map(normalizePose) ?? [],
+  };
+}
+
 function validateNavPath(messageEvent: PartialMessageEvent<NavPath>, renderer: Renderer): boolean {
   const { topic, message: navPath } = messageEvent;
   if (navPath.poses) {

packages/studio-base/src/panels/ThreeDeeRender/renderables/Poses.ts

@@ -5,6 +5,7 @@
 import * as THREE from "three";
 
 import { toNanoSec } from "@foxglove/rostime";
+import { PoseInFrame } from "@foxglove/schemas/schemas/typescript";
 import { SettingsTreeAction, SettingsTreeFields } from "@foxglove/studio";
 import type { RosValue } from "@foxglove/studio-base/players/types";
 
@@ -13,8 +14,14 @@ import { Renderer } from "../Renderer";
 import { PartialMessage, PartialMessageEvent, SceneExtension } from "../SceneExtension";
 import { SettingsTreeEntry } from "../SettingsManager";
 import { makeRgba, rgbaToCssString, stringToRgba } from "../color";
+import { POSE_IN_FRAME_DATATYPES } from "../foxglove";
 import { vecEqual } from "../math";
-import { normalizeHeader, normalizeMatrix6, normalizePose } from "../normalizeMessages";
+import {
+  normalizeHeader,
+  normalizeMatrix6,
+  normalizePose,
+  normalizeTime,
+} from "../normalizeMessages";
 import {
   Marker,
   PoseWithCovarianceStamped,
@@ -73,6 +80,7 @@ export type PoseUserData = BaseUserData & {
   settings: LayerSettingsPose;
   topic: string;
   poseMessage: PoseStamped | PoseWithCovarianceStamped;
+  originalMessage: Record<string, RosValue>;
   axis?: Axis;
   arrow?: RenderableArrow;
   sphere?: RenderableSphere;
@@ -87,7 +95,7 @@ export class PoseRenderable extends Renderable<PoseUserData> {
   }
 
   public override details(): Record<string, RosValue> {
-    return this.userData.poseMessage;
+    return this.userData.originalMessage;
   }
 }
 
@@ -96,6 +104,7 @@ export class Poses extends SceneExtension<PoseRenderable> {
     super("foxglove.Poses", renderer);
 
     renderer.addDatatypeSubscriptions(POSE_STAMPED_DATATYPES, this.handlePoseStamped);
+    renderer.addDatatypeSubscriptions(POSE_IN_FRAME_DATATYPES, this.handlePoseInFrame);
     renderer.addDatatypeSubscriptions(
       POSE_WITH_COVARIANCE_STAMPED_DATATYPES,
       this.handlePoseWithCovariance,
@@ -108,10 +117,11 @@ export class Poses extends SceneExtension<PoseRenderable> {
     const entries: SettingsTreeEntry[] = [];
     for (const topic of this.renderer.topics ?? []) {
       const isPoseStamped = POSE_STAMPED_DATATYPES.has(topic.datatype);
+      const isPoseInFrame = POSE_IN_FRAME_DATATYPES.has(topic.datatype);
       const isPoseWithCovarianceStamped = isPoseStamped
         ? false
         : POSE_WITH_COVARIANCE_STAMPED_DATATYPES.has(topic.datatype);
-      if (isPoseStamped || isPoseWithCovarianceStamped) {
+      if (isPoseStamped || isPoseWithCovarianceStamped || isPoseInFrame) {
         const config = (configTopics[topic.name] ?? {}) as Partial<LayerSettingsPose>;
         const type = config.type ?? DEFAULT_TYPE;
 
@@ -195,6 +205,7 @@ export class Poses extends SceneExtension<PoseRenderable> {
       this._updatePoseRenderable(
         renderable,
         renderable.userData.poseMessage,
+        renderable.userData.originalMessage,
         renderable.userData.receiveTime,
         { ...DEFAULT_SETTINGS, ...settings },
       );
@@ -204,20 +215,27 @@ export class Poses extends SceneExtension<PoseRenderable> {
   private handlePoseStamped = (messageEvent: PartialMessageEvent<PoseStamped>): void => {
     const poseMessage = normalizePoseStamped(messageEvent.message);
     const receiveTime = toNanoSec(messageEvent.receiveTime);
-    this.addPose(messageEvent.topic, poseMessage, receiveTime);
+    this.addPose(messageEvent.topic, poseMessage, messageEvent.message, receiveTime);
+  };
+
+  private handlePoseInFrame = (messageEvent: PartialMessageEvent<PoseInFrame>): void => {
+    const poseMessage = normalizePoseInFrameToPoseStamped(messageEvent.message);
+    const receiveTime = toNanoSec(messageEvent.receiveTime);
+    this.addPose(messageEvent.topic, poseMessage, messageEvent.message, receiveTime);
   };
 
   private handlePoseWithCovariance = (
     messageEvent: PartialMessageEvent<PoseWithCovarianceStamped>,
   ): void => {
     const poseMessage = normalizePoseWithCovarianceStamped(messageEvent.message);
     const receiveTime = toNanoSec(messageEvent.receiveTime);
-    this.addPose(messageEvent.topic, poseMessage, receiveTime);
+    this.addPose(messageEvent.topic, poseMessage, messageEvent.message, receiveTime);
   };
 
   private addPose(
     topic: string,
     poseMessage: PoseStamped | PoseWithCovarianceStamped,
+    originalMessage: Record<string, RosValue>,
     receiveTime: bigint,
   ): void {
     let renderable = this.renderables.get(topic);
@@ -237,6 +255,7 @@ export class Poses extends SceneExtension<PoseRenderable> {
         settings,
         topic,
         poseMessage,
+        originalMessage,
         axis: undefined,
         arrow: undefined,
         sphere: undefined,
@@ -246,19 +265,27 @@ export class Poses extends SceneExtension<PoseRenderable> {
       this.renderables.set(topic, renderable);
     }
 
-    this._updatePoseRenderable(renderable, poseMessage, receiveTime, renderable.userData.settings);
+    this._updatePoseRenderable(
+      renderable,
+      poseMessage,
+      originalMessage,
+      receiveTime,
+      renderable.userData.settings,
+    );
   }
 
   private _updatePoseRenderable(
     renderable: PoseRenderable,
     poseMessage: PoseStamped | PoseWithCovarianceStamped,
+    originalMessage: Record<string, RosValue>,
     receiveTime: bigint,
     settings: LayerSettingsPose,
   ): void {
     renderable.userData.receiveTime = receiveTime;
     renderable.userData.messageTime = toNanoSec(poseMessage.header.stamp);
     renderable.userData.frameId = this.renderer.normalizeFrameId(poseMessage.header.frame_id);
     renderable.userData.poseMessage = poseMessage;
+    renderable.userData.originalMessage = originalMessage;
 
     // Default the covariance sphere to hidden. If showCovariance is set and a valid covariance
     // matrix is present, it will be shown
@@ -403,6 +430,13 @@ export function normalizePoseStamped(pose: PartialMessage<PoseStamped>): PoseSta
   };
 }
 
+function normalizePoseInFrameToPoseStamped(pose: PartialMessage<PoseInFrame>): PoseStamped {
+  return {
+    header: { stamp: normalizeTime(pose.timestamp), frame_id: pose.frame_id ?? "" },
+    pose: normalizePose(pose.pose),
+  };
+}
+
 function normalizePoseWithCovariance(
   pose: PartialMessage<PoseWithCovariance> | undefined,
 ): PoseWithCovariance {

packages/studio-base/src/panels/ThreeDeeRender/stories/GeometryMsgs_PoseArray.stories.tsx

@@ -21,7 +21,7 @@ type Vec4 = [number, number, number, number];
 const vec4ToOrientation = ([x, y, z, w]: Vec4) => ({ x, y, z, w });
 
 GeometryMsgs_PoseArray.parameters = { colorScheme: "dark" };
-function GeometryMsgs_PoseArray(): JSX.Element {
+export function GeometryMsgs_PoseArray(): JSX.Element {
   const topics: Topic[] = [
     { name: "/baselink_path", datatype: "geometry_msgs/PoseArray" },
     { name: "/sensor_path", datatype: "geometry_msgs/PoseArray" },