geometry decode fixes

.changeset/tidy-moons-argue.md

@@ -0,0 +1,5 @@
+---
+'@viamrobotics/motion-tools': minor
+---
+
+Render STL collision meshes alongside PLY, and rotate unoriented link geometry into its own frame

src/lib/__tests__/mesh.spec.ts

@@ -0,0 +1,152 @@
+import { describe, expect, it } from 'vitest'
+
+import { meshContentType, parseMeshInput } from '$lib/mesh'
+
+const asciiStl = `solid tri
+facet normal 0 0 1
+  outer loop
+    vertex 0 0 0
+    vertex 1 0 0
+    vertex 0 1 0
+  endloop
+endfacet
+endsolid tri
+`
+
+const asciiPly = `ply
+format ascii 1.0
+element vertex 3
+property float x
+property float y
+property float z
+element face 1
+property list uchar int vertex_index
+end_header
+0 0 0
+1 0 0
+0 1 0
+3 0 1 2
+`
+
+const bytes = (text: string) => new TextEncoder().encode(text)
+
+/**
+ * Binary on purpose: `STLLoader` classifies ASCII by regex, so an ASCII fixture parses out of an
+ * oversized buffer and the subarray and short-input cases below pass with their guards deleted.
+ */
+const binaryStl = (triangles = 1): Uint8Array => {
+	const buffer = new ArrayBuffer(84 + 50 * triangles)
+	const view = new DataView(buffer)
+	view.setUint32(80, triangles, true)
+
+	let offset = 84
+	for (let i = 0; i < triangles; i += 1) {
+		// normal (0,0,1) then the three corners of a unit triangle
+		for (const value of [0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0]) {
+			view.setFloat32(offset, value, true)
+			offset += 4
+		}
+		offset += 2
+	}
+
+	return new Uint8Array(buffer)
+}
+
+// Mapped rather than spread: a spread passes one argument per byte and blows the call stack on a
+// fixture of any size.
+const base64 = (data: Uint8Array) =>
+	btoa(Array.from(data, (byte) => String.fromCharCode(byte)).join(''))
+
+describe('meshContentType', () => {
+	it.each([
+		['ply', 'ply'],
+		['stl', 'stl'],
+		['STL', 'stl'],
+		['  Ply  ', 'ply'],
+		['model/stl', 'stl'],
+		['application/ply; charset=binary', 'ply'],
+	])('reads %s as %s', (raw, expected) => {
+		expect(meshContentType(raw)).toBe(expected)
+	})
+
+	it.each([['obj'], ['dae'], [''], [undefined]])('does not claim to handle %s', (raw) => {
+		expect(meshContentType(raw)).toBeUndefined()
+	})
+
+	it.each([['meshes/ur20/collision/base.stl'], ['package://arm/link_1.stl'], ['/etc/thing.ply']])(
+		'does not read a file path like %s as a content type',
+		(raw) => {
+			expect(meshContentType(raw)).toBeUndefined()
+		}
+	)
+})
+
+describe('parseMeshInput', () => {
+	it('parses an stl mesh into real vertices', () => {
+		const geometry = parseMeshInput(bytes(asciiStl), 'stl')
+		expect(geometry.getAttribute('position').count).toBe(3)
+	})
+
+	it('parses a ply mesh into real vertices', () => {
+		const geometry = parseMeshInput(bytes(asciiPly), 'ply')
+		expect(geometry.getAttribute('position').count).toBe(3)
+	})
+
+	it.each([[undefined], [''], ['obj']])('falls back to ply for a content type of %s', (raw) => {
+		const geometry = parseMeshInput(bytes(asciiPly), raw)
+		expect(geometry.getAttribute('position').count).toBe(3)
+	})
+
+	it.each([['ply'], ['stl']])('returns an empty geometry for empty %s bytes', (contentType) => {
+		expect(parseMeshInput(new Uint8Array(), contentType).getAttribute('position')).toBeUndefined()
+	})
+
+	it('parses a binary stl mesh into real vertices', () => {
+		expect(parseMeshInput(binaryStl(2), 'stl').getAttribute('position').count).toBe(6)
+	})
+
+	it.each([
+		['ascii', () => btoa(asciiStl)],
+		['binary', () => base64(binaryStl())],
+	])('accepts a base64 %s stl as well as bytes', (_label, encode) => {
+		expect(parseMeshInput(encode(), 'stl').getAttribute('position').count).toBe(3)
+	})
+
+	it('parses a binary stl mesh held in a subarray', () => {
+		const stl = binaryStl()
+		const padded = new Uint8Array(stl.length + 8)
+		padded.set(stl, 4)
+		const view = padded.subarray(4, 4 + stl.length)
+
+		expect(parseMeshInput(view, 'stl').getAttribute('position').count).toBe(3)
+	})
+
+	it.each([[1], [19], [83]])(
+		'returns an empty geometry for a %i byte stl rather than throwing',
+		(length) => {
+			expect(parseMeshInput(new Uint8Array(length), 'stl').getAttribute('position')).toBeUndefined()
+		}
+	)
+
+	// Above the 84-byte guard, so the count at offset 80 is read and believed.
+	it.each([
+		['claims 100 triangles and carries none', 84, 100],
+		['claims 2 triangles and carries 1', 84 + 50, 2],
+	])(
+		'returns an empty geometry for a binary stl that %s rather than throwing',
+		(_label, length, claimed) => {
+			const truncated = new Uint8Array(length)
+			new DataView(truncated.buffer).setUint32(80, claimed, true)
+
+			expect(parseMeshInput(truncated, 'stl').getAttribute('position')).toBeUndefined()
+		}
+	)
+
+	it.each([
+		['an empty string', ''],
+		['a truncated base64 payload', btoa('solid t\nendsolid t\n')],
+		['malformed base64', '!!!not base64!!!'],
+	])('returns an empty geometry for %s rather than throwing', (_label, encoded) => {
+		expect(parseMeshInput(encoded, 'stl').getAttribute('position')).toBeUndefined()
+	})
+})

src/lib/__tests__/meshGeometryTrait.spec.ts

@@ -0,0 +1,62 @@
+import { commonApi, Geometry as ViamGeometry } from '@viamrobotics/sdk'
+import { createWorld, type World } from 'koota'
+import { afterEach, describe, expect, it } from 'vitest'
+
+import { traits } from '$lib/ecs'
+
+const asciiStl = `solid tri
+facet normal 0 0 1
+  outer loop
+    vertex 0 0 0
+    vertex 1 0 0
+    vertex 0 1 0
+  endloop
+endfacet
+endsolid tri
+`
+
+const stlGeometry = (): ViamGeometry =>
+	new ViamGeometry({
+		geometryType: {
+			case: 'mesh',
+			value: new commonApi.Mesh({
+				contentType: 'stl',
+				mesh: new TextEncoder().encode(asciiStl),
+			}),
+		},
+	})
+
+/**
+ * Not covered by `mesh.spec.ts`: a regression to `parsePlyInput` here still puts an entity in the
+ * world, since `PLYLoader` answers STL bytes with an empty geometry rather than throwing.
+ */
+describe('mesh geometry reaches the trait layer', () => {
+	let world: World
+	afterEach(() => world?.destroy())
+
+	it('Geometry parses an stl mesh into real vertices', () => {
+		world = createWorld()
+		const entity = world.spawn(traits.Geometry(stlGeometry()))
+
+		expect(entity.get(traits.BufferGeometry)!.getAttribute('position').count).toBe(3)
+	})
+
+	it('updateGeometryTrait parses an stl mesh for an entity with no prior BufferGeometry', () => {
+		world = createWorld()
+		const entity = world.spawn(traits.Box())
+
+		traits.updateGeometryTrait(entity, stlGeometry())
+
+		expect(entity.has(traits.Box)).toBe(false)
+		expect(entity.get(traits.BufferGeometry)!.getAttribute('position').count).toBe(3)
+	})
+
+	it('updateGeometryTrait re-parses an stl mesh for an entity that already holds a BufferGeometry', () => {
+		world = createWorld()
+		const entity = world.spawn(traits.Geometry(stlGeometry()))
+
+		traits.updateGeometryTrait(entity, stlGeometry())
+
+		expect(entity.get(traits.BufferGeometry)!.getAttribute('position').count).toBe(3)
+	})
+})

src/lib/ecs/traits.ts

@@ -9,7 +9,7 @@ import { ColorFormat } from '$lib/buf/draw/v1/metadata_pb'
 import { createBox, createCapsule, createSphere } from '$lib/geometry'
 import { parsePcdInWorker } from '$lib/loaders/pcd'
 import { Pose, type PosePatch } from '$lib/math'
-import { parsePlyInput } from '$lib/ply'
+import { parseMeshInput } from '$lib/mesh'
 
 export const Name = trait(() => '')
 export const UUID = trait(() => '')
@@ -268,7 +268,8 @@ export const Geometry = (geometry: ViamGeometry) => {
 	} else if (geometry.geometryType.case === 'sphere') {
 		return Sphere(createSphere(geometry.geometryType.value))
 	} else if (geometry.geometryType.case === 'mesh') {
-		return BufferGeometry(parsePlyInput(geometry.geometryType.value.mesh))
+		const { mesh, contentType } = geometry.geometryType.value
+		return BufferGeometry(parseMeshInput(mesh, contentType))
 	}
 
 	return ReferenceFrame
@@ -308,13 +309,14 @@ export const updateGeometryTrait = (entity: Entity, geometry?: ViamGeometry) =>
 			entity.add(Sphere(next))
 		}
 	} else if (geometry.geometryType.case === 'mesh') {
+		const { mesh, contentType } = geometry.geometryType.value
 		if (entity.has(BufferGeometry)) {
 			const old = entity.get(BufferGeometry)
-			entity.set(BufferGeometry, parsePlyInput(geometry.geometryType.value.mesh))
+			entity.set(BufferGeometry, parseMeshInput(mesh, contentType))
 			old?.dispose()
 		} else {
 			entity.remove(Box, Sphere, Capsule)
-			entity.add(BufferGeometry(parsePlyInput(geometry.geometryType.value.mesh)))
+			entity.add(BufferGeometry(parseMeshInput(mesh, contentType)))
 		}
 	} else if (geometry.geometryType.case === 'pointcloud') {
 		updatePointCloud(entity, geometry.geometryType.value.pointCloud)
@@ -363,10 +365,9 @@ const updatePointCloud = (entity: Entity, pointCloud: Uint8Array): void => {
 					}
 				}
 
-				// When the point count changes, attributes must be reallocated. An
-				// entity can hold an attribute-less geometry (`parsePlyInput` returns
-				// one for empty mesh bytes), so treat a missing attribute as a count
-				// of zero rather than reading `.count` off undefined.
+				// Attributes must be reallocated when the point count changes, and an
+				// entity can hold an attribute-less geometry: `parseMeshInput` returns
+				// one for empty or truncated bytes.
 				const oldCount = buffer.getAttribute('position')?.count ?? 0
 				const newCount = parsed.positions.length / 3
 				if (oldCount === newCount) {

src/lib/math/spatialJson.ts

@@ -67,10 +67,11 @@ export const geometryCenterInFrame = (
 
 	const center = new Pose(tmpV.x, tmpV.y, tmpV.z)
 
-	if (quatFromJson(geoOrient, tmpQGeo)) {
-		tmpQLocal.copy(tmpQInv).multiply(tmpQGeo)
-		center.setFromQuaternion(tmpQLocal)
-	}
+	// Unconditional: an absent orientation means identity in the parent's frame, which is still
+	// R_frame⁻¹ once expressed locally. `quatFromJson` writes identity when it finds nothing.
+	quatFromJson(geoOrient, tmpQGeo)
+	tmpQLocal.copy(tmpQInv).multiply(tmpQGeo)
+	center.setFromQuaternion(tmpQLocal)
 
 	return center
 }

src/lib/mesh.ts

@@ -0,0 +1,22 @@
+import type { BufferGeometry } from 'three'
+
+import { parsePlyInput } from '$lib/ply'
+import { parseStlInput } from '$lib/stl'
+
+export type MeshContentType = 'ply' | 'stl'
+
+/**
+ * Reads a content type, not a path: `meshes/base.stl` belongs in `mesh_file_path`. RDK writes a bare
+ * `ply` or `stl`, so the folding and trimming are defense on a proto this repo does not own.
+ */
+export const meshContentType = (raw: string | undefined): MeshContentType | undefined => {
+	const value = (raw ?? '').toLowerCase().split(';')[0]?.trim().split('/').at(-1)
+	return value === 'ply' || value === 'stl' ? value : undefined
+}
+
+/**
+ * PLY is the fallback rather than an error: it is the assumption every render path already makes.
+ * The plan parser does not rely on it, gating on `meshContentType` and skipping what it cannot read.
+ */
+export const parseMeshInput = (mesh: string | Uint8Array, contentType?: string): BufferGeometry =>
+	meshContentType(contentType) === 'stl' ? parseStlInput(mesh) : parsePlyInput(mesh)

src/lib/plugins/DrawService/useDrawAPI.svelte.ts

@@ -15,8 +15,8 @@ import { hierarchy, traits, useWorld } from '$lib/ecs'
 import { createBox, createCapsule, createSphere } from '$lib/geometry'
 import { useCameraControls } from '$lib/hooks/useControls.svelte'
 import { Pose } from '$lib/math'
+import { parseMeshInput } from '$lib/mesh'
 import { useLogs } from '$lib/plugins'
-import { parsePlyInput } from '$lib/ply'
 
 import { useDrawConnectionConfig } from './useDrawConnectionConfig.svelte'
 
@@ -223,7 +223,9 @@ export const provideDrawAPI = () => {
 
 		const geometryTrait = () => {
 			if ('mesh' in data) {
-				const geometry = parsePlyInput(data.mesh.mesh)
+				// The draw service speaks proto JSON, so `mesh` is base64 and `content_type` arrives
+				// camel-cased.
+				const geometry = parseMeshInput(data.mesh.mesh, data.mesh.contentType)
 				return traits.BufferGeometry(geometry)
 			} else if ('box' in data) {
 				return traits.Box(createBox(data.box))