diff --git a/.gitignore b/.gitignore index 9ab5b7d2..af05459e 100644 --- a/.gitignore +++ b/.gitignore @@ -32,3 +32,7 @@ vite.config.ts.timestamp-* # Sentry Config File .env.sentry-build-plugin + +# Protobuf +gen +src/lib/gen diff --git a/.prettierignore b/.prettierignore index 8cc52b9a..f88b2e88 100644 --- a/.prettierignore +++ b/.prettierignore @@ -2,4 +2,8 @@ package-lock.json pnpm-lock.yaml yarn.lock -.changeset \ No newline at end of file +.changeset + +# Protobuf +gen +src/lib/gen \ No newline at end of file diff --git a/Makefile b/Makefile index d81e06fe..4953e141 100644 --- a/Makefile +++ b/Makefile @@ -1,4 +1,4 @@ -.PHONY: help setup up +.PHONY: help setup up proto-generate proto-lint proto-format proto-clean # Default target - show help when no target is specified .DEFAULT_GOAL := help @@ -8,12 +8,37 @@ help: @echo 'Usage: make [target]' @echo '' @echo 'Available targets:' - @echo ' setup - Set up development environment (install pnpm, bun, dependencies)' - @echo ' up - Start development server' - @echo ' help - Show this help message' + @echo ' setup - Set up development environment (install pnpm, bun, dependencies)' + @echo ' up - Start development server' + @echo ' proto-generate - Generate Go and TypeScript code from protobuf definitions' + @echo ' proto-lint - Lint protobuf files' + @echo ' proto-format - Format protobuf files' + @echo ' proto-clean - Clean generated protobuf code' + @echo ' help - Show this help message' setup: @./etc/setup.sh up: pnpm dev + +proto-clean: + @echo 'Cleaning generated protobuf code...' + @rm -rf gen src/lib/gen + @echo 'Clean complete!' + +proto-gen: proto-clean + @echo 'Generating protobuf code...' + @echo 'Updating buf dependencies...' + @pnpm exec buf dep update + @echo 'Installing protoc-gen-go...' + @go install google.golang.org/protobuf/cmd/protoc-gen-go@latest + @echo 'Generating code...' + @PATH="$(shell go env GOPATH)/bin:$(shell pnpm bin):$$PATH" pnpm exec buf generate + @echo 'Protobuf code generation complete!' + +proto-lint: + @pnpm exec buf lint + +proto-format: + @pnpm exec buf format -w diff --git a/buf.gen.yaml b/buf.gen.yaml new file mode 100644 index 00000000..f17f9536 --- /dev/null +++ b/buf.gen.yaml @@ -0,0 +1,16 @@ +version: v2 +managed: + enabled: false +plugins: + # Go code generation + - local: protoc-gen-go + out: gen + opt: paths=source_relative + + # TypeScript code generation (v1 - matches Viam SDK) + - remote: buf.build/bufbuild/es:v1.10.0 + out: src/lib/gen + opt: + - target=ts + - import_extension=none + include_imports: true diff --git a/buf.lock b/buf.lock new file mode 100644 index 00000000..32e9b932 --- /dev/null +++ b/buf.lock @@ -0,0 +1,9 @@ +# Generated by buf. DO NOT EDIT. +version: v2 +deps: + - name: buf.build/googleapis/googleapis + commit: 62f35d8aed1149c291d606d958a7ce32 + digest: b5:d66bf04adc77a0870bdc9328aaf887c7188a36fb02b83a480dc45ef9dc031b4d39fc6e9dc6435120ccf4fe5bfd5c6cb6592533c6c316595571f9a31420ab47fe + - name: buf.build/viamrobotics/api + commit: a4b2b3ea4a094ad69b9c26f96d1f43aa + digest: b5:8997d57a7bba43c2c0b8ad1884e1ec43d337bce946fb2750b9324b859a15983271c2db4b0ceae648f929d62768c4839686d1cb8477d323f51c49cb91df73709c diff --git a/buf.yaml b/buf.yaml new file mode 100644 index 00000000..0125b72c --- /dev/null +++ b/buf.yaml @@ -0,0 +1,11 @@ +version: v2 +modules: + - path: protos +deps: + - buf.build/viamrobotics/api +lint: + use: + - STANDARD +breaking: + use: + - FILE diff --git a/client/client/client.go b/client/client/client.go index c1ba5df3..9ed69b24 100644 --- a/client/client/client.go +++ b/client/client/client.go @@ -18,7 +18,7 @@ import ( "github.com/golang/geo/r3" "github.com/viam-labs/motion-tools/client/colorutil" - "github.com/viam-labs/motion-tools/client/shapes" + "github.com/viam-labs/motion-tools/draw" "google.golang.org/protobuf/encoding/protojson" "go.viam.com/rdk/referenceframe" @@ -402,18 +402,9 @@ func DrawPoses(poses []spatialmath.Pose, colors []string, arrowHeadAtPose bool) // - nurbs: A nurbs curve // - color: The color of the line // - name: A unique label for the curve -func DrawNurbs(nurbs shapes.Nurbs, color string, name string) error { - poseData := make([]json.RawMessage, len(nurbs.ControlPts)) - for i, pose := range nurbs.ControlPts { - data, err := protojson.Marshal(spatialmath.PoseToProtobuf(pose)) - if err != nil { - return err - } - poseData[i] = json.RawMessage(data) - } - +func DrawNurbs(nurbs *draw.Nurbs, color string, name string) error { wrappedData := map[string]interface{}{ - "ControlPts": poseData, + "ControlPts": nurbs.ControlPoints, "Degree": nurbs.Degree, "Weights": nurbs.Weights, "Knots": nurbs.Knots, @@ -465,18 +456,8 @@ func RemoveAllSpatialObjects() error { // - lookAt: The direction the camera should look at // - animate: Whether or not to animate to this pose func SetCameraPose(position r3.Vector, lookAt r3.Vector, animate bool) error { - positionM := map[string]interface{}{ - "X": position.X / 1000.0, - "Y": position.Y / 1000.0, - "Z": position.Z / 1000.0, - } - - lookAtM := map[string]interface{}{ - "X": lookAt.X / 1000.0, - "Y": lookAt.Y / 1000.0, - "Z": lookAt.Z / 1000.0, - } - + positionM := position.Mul(0.001) + lookAtM := lookAt.Mul(0.001) data := map[string]interface{}{ "setCameraPose": true, "Position": positionM, diff --git a/client/client/draw_nurbs_test.go b/client/client/draw_nurbs_test.go index 11e1ab88..b11b55a1 100644 --- a/client/client/draw_nurbs_test.go +++ b/client/client/draw_nurbs_test.go @@ -13,6 +13,6 @@ func TestDrawNurbs(t *testing.T) { t.Run("DrawNurbs", func(t *testing.T) { nurbs := shapes.GenerateNURBS(20, 3, r3.Vector{X: 0, Y: 0, Z: 0}) - test.That(t, DrawNurbs(nurbs, "#40E0D0", "nurbs-1"), test.ShouldBeNil) + test.That(t, DrawNurbs(&nurbs, "#40E0D0", "nurbs-1"), test.ShouldBeNil) }) } diff --git a/client/client/draw_pointcloud_test.go b/client/client/draw_pointcloud_test.go index 05cfb2ec..33a3911c 100644 --- a/client/client/draw_pointcloud_test.go +++ b/client/client/draw_pointcloud_test.go @@ -33,8 +33,8 @@ func TestDrawPointCloud(t *testing.T) { pc, err := pointcloud.NewFromFile("../data/Zaghetto.pcd", pointcloud.BasicType) test.That(t, err, test.ShouldBeNil) - for i := 0; i < 10; i++ { - time.Sleep(16 * time.Millisecond) + for i := range 10 { + time.Sleep(100 * time.Millisecond) test.That(t, DrawPointCloud("Zaghetto"+strconv.Itoa(i+1), pc, nil), test.ShouldBeNil) } }) diff --git a/client/shapes/shapes.go b/client/shapes/shapes.go index 8ef2e464..be7a83e7 100644 --- a/client/shapes/shapes.go +++ b/client/shapes/shapes.go @@ -4,18 +4,12 @@ import ( "math/rand" "github.com/golang/geo/r3" + "github.com/viam-labs/motion-tools/draw" "go.viam.com/rdk/spatialmath" ) -type Nurbs struct { - ControlPts []spatialmath.Pose - Degree int - Weights []float64 - Knots []float64 -} - // Generate a NURBS structure similar to the Three.js version -func GenerateNURBS(numControlPoints int, degree int, offset r3.Vector) Nurbs { +func GenerateNURBS(numControlPoints int, degree int, offset r3.Vector) draw.Nurbs { controlPts := make([]spatialmath.Pose, numControlPoints) weights := make([]float64, numControlPoints) knots := make([]float64, numControlPoints+degree+1) @@ -42,11 +36,11 @@ func GenerateNURBS(numControlPoints int, degree int, offset r3.Vector) Nurbs { knots[i+degree+1] = clamp(knot, 0, 1) } - return Nurbs{ - ControlPts: controlPts, - Degree: degree, - Weights: weights, - Knots: knots, + return draw.Nurbs{ + ControlPoints: controlPts, + Degree: int32(degree), + Weights: weights, + Knots: knots, } } diff --git a/draw/.gitignore b/draw/.gitignore new file mode 100644 index 00000000..6e118ec4 --- /dev/null +++ b/draw/.gitignore @@ -0,0 +1 @@ +__snapshots__ \ No newline at end of file diff --git a/draw/README.md b/draw/README.md new file mode 100644 index 00000000..e69de29b diff --git a/draw/__fixtures__/BoxAnimated.glb b/draw/__fixtures__/BoxAnimated.glb new file mode 100644 index 00000000..69481ec3 Binary files /dev/null and b/draw/__fixtures__/BoxAnimated.glb differ diff --git a/draw/__fixtures__/CesiumMilkTruck.glb b/draw/__fixtures__/CesiumMilkTruck.glb new file mode 100644 index 00000000..d5c7cf5a Binary files /dev/null and b/draw/__fixtures__/CesiumMilkTruck.glb differ diff --git a/draw/__fixtures__/Fox.glb b/draw/__fixtures__/Fox.glb new file mode 100644 index 00000000..1ef5c0d0 Binary files /dev/null and b/draw/__fixtures__/Fox.glb differ diff --git a/draw/__fixtures__/scene_fixture.json b/draw/__fixtures__/scene_fixture.json new file mode 100644 index 00000000..768cdff1 --- /dev/null +++ b/draw/__fixtures__/scene_fixture.json @@ -0,0 +1,1189 @@ +{ + "frame_system": { + "name": "", + "world": { + "frame_type": "static", + "frame": { + "id": "world", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "frames": { + "base": { + "frame_type": "model", + "frame": { + "name": "base", + "model": { + "name": "base", + "links": [ + { + "id": "", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "box", + "x": 2000, + "y": 1300, + "z": 100, + "r": 0, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "" + }, + "parent": "world" + } + ], + "OriginalFile": { + "Bytes": "eyJPcmlnaW5hbEZpbGUiOnt9LCJsaW5rcyI6W3siZ2VvbWV0cnkiOnsiTGFiZWwiOiIiLCJsIjowLCJvcmllbnRhdGlvbiI6eyJ0eXBlIjoicXVhdGVybmlvbiIsInZhbHVlIjp7IlciOjEsIlgiOjAsIlkiOjAsIloiOjB9fSwiciI6MCwidHJhbnNsYXRpb24iOnsiWCI6MCwiWSI6MCwiWiI6MH0sInR5cGUiOiJib3giLCJ4IjoyMDAwLCJ5IjoxMzAwLCJ6IjoxMDB9LCJpZCI6IiIsIm9yaWVudGF0aW9uIjp7InR5cGUiOiJxdWF0ZXJuaW9uIiwidmFsdWUiOnsiVyI6MSwiWCI6MCwiWSI6MCwiWiI6MH19LCJwYXJlbnQiOiJ3b3JsZCIsInRyYW5zbGF0aW9uIjp7IlgiOjAsIlkiOjAsIloiOjB9fV0sIm5hbWUiOiJiYXNlIn0=", + "Extension": "json" + } + }, + "limits": [] + } + }, + "base_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "base_origin", + "translation": { + "X": 0, + "Y": 0, + "Z": -1200 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "compliance": { + "frame_type": "static", + "frame": { + "id": "compliance", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "compliance_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "compliance_origin", + "translation": { + "X": 0, + "Y": 0, + "Z": 8 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "box", + "x": 330, + "y": 97.7, + "z": 10, + "r": 0, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": -3 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "compliance_origin" + } + } + }, + "computers": { + "frame_type": "model", + "frame": { + "name": "computers", + "model": { + "name": "computers", + "links": [ + { + "id": "", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "box", + "x": 2200, + "y": 650, + "z": 1200, + "r": 0, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "" + }, + "parent": "world" + } + ], + "OriginalFile": { + "Bytes": "eyJPcmlnaW5hbEZpbGUiOnt9LCJsaW5rcyI6W3siZ2VvbWV0cnkiOnsiTGFiZWwiOiIiLCJsIjowLCJvcmllbnRhdGlvbiI6eyJ0eXBlIjoicXVhdGVybmlvbiIsInZhbHVlIjp7IlciOjEsIlgiOjAsIlkiOjAsIloiOjB9fSwiciI6MCwidHJhbnNsYXRpb24iOnsiWCI6MCwiWSI6MCwiWiI6MH0sInR5cGUiOiJib3giLCJ4IjoyMjAwLCJ5Ijo2NTAsInoiOjEyMDB9LCJpZCI6IiIsIm9yaWVudGF0aW9uIjp7InR5cGUiOiJxdWF0ZXJuaW9uIiwidmFsdWUiOnsiVyI6MSwiWCI6MCwiWSI6MCwiWiI6MH19LCJwYXJlbnQiOiJ3b3JsZCIsInRyYW5zbGF0aW9uIjp7IlgiOjAsIlkiOjAsIloiOjB9fV0sIm5hbWUiOiJjb21wdXRlcnMifQ==", + "Extension": "json" + } + }, + "limits": [] + } + }, + "computers_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "computers_origin", + "translation": { + "X": 0, + "Y": -325, + "Z": -600 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "floor": { + "frame_type": "model", + "frame": { + "name": "floor", + "model": { + "name": "floor", + "links": [ + { + "id": "", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "box", + "x": 10000, + "y": 10000, + "z": 10, + "r": 0, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "" + }, + "parent": "world" + } + ], + "OriginalFile": { + "Bytes": "eyJPcmlnaW5hbEZpbGUiOnt9LCJsaW5rcyI6W3siZ2VvbWV0cnkiOnsiTGFiZWwiOiIiLCJsIjowLCJvcmllbnRhdGlvbiI6eyJ0eXBlIjoicXVhdGVybmlvbiIsInZhbHVlIjp7IlciOjEsIlgiOjAsIlkiOjAsIloiOjB9fSwiciI6MCwidHJhbnNsYXRpb24iOnsiWCI6MCwiWSI6MCwiWiI6MH0sInR5cGUiOiJib3giLCJ4IjoxMDAwMCwieSI6MTAwMDAsInoiOjEwfSwiaWQiOiIiLCJvcmllbnRhdGlvbiI6eyJ0eXBlIjoicXVhdGVybmlvbiIsInZhbHVlIjp7IlciOjEsIlgiOjAsIlkiOjAsIloiOjB9fSwicGFyZW50Ijoid29ybGQiLCJ0cmFuc2xhdGlvbiI6eyJYIjowLCJZIjowLCJaIjowfX1dLCJuYW1lIjoiZmxvb3IifQ==", + "Extension": "json" + } + }, + "limits": [] + } + }, + "floor_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "floor_origin", + "translation": { + "X": 0, + "Y": 0, + "Z": -1360 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "hardtop-middle-wall": { + "frame_type": "model", + "frame": { + "name": "hardtop-middle-wall", + "model": { + "name": "hardtop-middle-wall", + "links": [ + { + "id": "", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "box", + "x": 5000, + "y": 50, + "z": 2500, + "r": 0, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "" + }, + "parent": "world" + } + ], + "OriginalFile": { + "Bytes": "eyJPcmlnaW5hbEZpbGUiOnt9LCJsaW5rcyI6W3siZ2VvbWV0cnkiOnsiTGFiZWwiOiIiLCJsIjowLCJvcmllbnRhdGlvbiI6eyJ0eXBlIjoicXVhdGVybmlvbiIsInZhbHVlIjp7IlciOjEsIlgiOjAsIlkiOjAsIloiOjB9fSwiciI6MCwidHJhbnNsYXRpb24iOnsiWCI6MCwiWSI6MCwiWiI6MH0sInR5cGUiOiJib3giLCJ4Ijo1MDAwLCJ5Ijo1MCwieiI6MjUwMH0sImlkIjoiIiwib3JpZW50YXRpb24iOnsidHlwZSI6InF1YXRlcm5pb24iLCJ2YWx1ZSI6eyJXIjoxLCJYIjowLCJZIjowLCJaIjowfX0sInBhcmVudCI6IndvcmxkIiwidHJhbnNsYXRpb24iOnsiWCI6MCwiWSI6MCwiWiI6MH19XSwibmFtZSI6ImhhcmR0b3AtbWlkZGxlLXdhbGwifQ==", + "Extension": "json" + } + }, + "limits": [] + } + }, + "hardtop-middle-wall_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "hardtop-middle-wall_origin", + "translation": { + "X": 0, + "Y": 1710, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "hose-guide": { + "frame_type": "model", + "frame": { + "name": "hose-guide", + "model": { + "name": "hose-guide", + "links": [ + { + "id": "", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "box", + "x": 70, + "y": 70, + "z": 90, + "r": 0, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "" + }, + "parent": "world" + } + ], + "OriginalFile": { + "Bytes": "eyJPcmlnaW5hbEZpbGUiOnt9LCJsaW5rcyI6W3siZ2VvbWV0cnkiOnsiTGFiZWwiOiIiLCJsIjowLCJvcmllbnRhdGlvbiI6eyJ0eXBlIjoicXVhdGVybmlvbiIsInZhbHVlIjp7IlciOjEsIlgiOjAsIlkiOjAsIloiOjB9fSwiciI6MCwidHJhbnNsYXRpb24iOnsiWCI6MCwiWSI6MCwiWiI6MH0sInR5cGUiOiJib3giLCJ4Ijo3MCwieSI6NzAsInoiOjkwfSwiaWQiOiIiLCJvcmllbnRhdGlvbiI6eyJ0eXBlIjoicXVhdGVybmlvbiIsInZhbHVlIjp7IlciOjEsIlgiOjAsIlkiOjAsIloiOjB9fSwicGFyZW50Ijoid29ybGQiLCJ0cmFuc2xhdGlvbiI6eyJYIjowLCJZIjowLCJaIjowfX1dLCJuYW1lIjoiaG9zZS1ndWlkZSJ9", + "Extension": "json" + } + }, + "limits": [] + } + }, + "hose-guide_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "hose-guide_origin", + "translation": { + "X": -5.1045213210695614e-15, + "Y": -90.00000000000003, + "Z": -108.00000000000003 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 0.7071067811865477, + "X": 0, + "Y": 0.7071067811865475, + "Z": 0 + } + } + } + }, + "pedestal": { + "frame_type": "model", + "frame": { + "name": "pedestal", + "model": { + "name": "pedestal", + "links": [ + { + "id": "", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "box", + "x": 280, + "y": 280, + "z": 1200, + "r": 0, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "" + }, + "parent": "world" + } + ], + "OriginalFile": { + "Bytes": "eyJPcmlnaW5hbEZpbGUiOnt9LCJsaW5rcyI6W3siZ2VvbWV0cnkiOnsiTGFiZWwiOiIiLCJsIjowLCJvcmllbnRhdGlvbiI6eyJ0eXBlIjoicXVhdGVybmlvbiIsInZhbHVlIjp7IlciOjEsIlgiOjAsIlkiOjAsIloiOjB9fSwiciI6MCwidHJhbnNsYXRpb24iOnsiWCI6MCwiWSI6MCwiWiI6MH0sInR5cGUiOiJib3giLCJ4IjoyODAsInkiOjI4MCwieiI6MTIwMH0sImlkIjoiIiwib3JpZW50YXRpb24iOnsidHlwZSI6InF1YXRlcm5pb24iLCJ2YWx1ZSI6eyJXIjoxLCJYIjowLCJZIjowLCJaIjowfX0sInBhcmVudCI6IndvcmxkIiwidHJhbnNsYXRpb24iOnsiWCI6MCwiWSI6MCwiWiI6MH19XSwibmFtZSI6InBlZGVzdGFsIn0=", + "Extension": "json" + } + }, + "limits": [] + } + }, + "pedestal_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "pedestal_origin", + "translation": { + "X": 0, + "Y": 0, + "Z": -600 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "sander": { + "frame_type": "static", + "frame": { + "id": "sander", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "sander_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "sander_origin", + "translation": { + "X": 0, + "Y": 0, + "Z": 110.00000000000001 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 0.7071067811865477, + "X": 0, + "Y": 0, + "Z": 0.7071067811865475 + } + }, + "geometry": { + "type": "box", + "x": 405, + "y": 101, + "z": 110, + "r": 0, + "l": 0, + "translation": { + "X": -37.5, + "Y": 0, + "Z": -55 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "sander_origin" + } + } + }, + "sensing-camera": { + "frame_type": "static", + "frame": { + "id": "sensing-camera", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "sensing-camera_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "sensing-camera_origin", + "translation": { + "X": 80.5307840170718, + "Y": -18.728006594276273, + "Z": 47.02169680974533 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 0.7068856386356402, + "X": 0.002966872111487073, + "Y": 0.002965069380928958, + "Z": 0.7073154175642742 + } + }, + "geometry": { + "type": "box", + "x": 160, + "y": 65, + "z": 65, + "r": 0, + "l": 0, + "translation": { + "X": -5, + "Y": 10, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "sensing-camera_origin" + } + } + }, + "ur20-modular": { + "frame_type": "model", + "frame": { + "name": "ur20-modular", + "model": { + "name": "UR20", + "kinematic_param_type": "SVA", + "links": [ + { + "id": "base_link", + "translation": { + "X": 0, + "Y": 0, + "Z": 236.3 + }, + "orientation": { + "type": "euler_angles", + "value": { + "pitch": 0, + "roll": 0, + "yaw": 0 + } + }, + "parent": "world" + }, + { + "id": "shoulder_link", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "euler_angles", + "value": { + "pitch": 0, + "roll": 1.570796327, + "yaw": 0 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 122.5, + "l": 333, + "translation": { + "X": 0, + "Y": 0, + "Z": -100 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 0, + "X": 0, + "Y": 0, + "Z": 1 + } + }, + "Label": "" + }, + "parent": "shoulder_pan_joint" + }, + { + "id": "upper_arm_link", + "translation": { + "X": -862, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "" + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 90, + "l": 1032, + "translation": { + "X": -421.6, + "Y": 0, + "Z": 260 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 0.5, + "X": 0.5, + "Y": -0.5, + "Z": -0.5 + } + }, + "Label": "" + }, + "parent": "shoulder_lift_joint" + }, + { + "id": "forearm_link", + "translation": { + "X": -728.7, + "Y": 0, + "Z": 201 + }, + "orientation": { + "type": "" + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 75, + "l": 858, + "translation": { + "X": -360, + "Y": 0, + "Z": 43 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 0.5, + "X": 0.5, + "Y": -0.5, + "Z": -0.5 + } + }, + "Label": "" + }, + "parent": "elbow_joint" + }, + { + "id": "wrist_1_link", + "translation": { + "X": 0, + "Y": -159.3, + "Z": 0 + }, + "orientation": { + "type": "euler_angles", + "value": { + "pitch": 0, + "roll": 1.570796327, + "yaw": 0 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 48.5, + "l": 262, + "translation": { + "X": 0, + "Y": 0, + "Z": -77.5 + }, + "orientation": { + "type": "" + }, + "Label": "" + }, + "parent": "wrist_1_joint" + }, + { + "id": "wrist_2_link", + "translation": { + "X": 0, + "Y": 154.29999999999998, + "Z": 0 + }, + "orientation": { + "type": "euler_angles", + "value": { + "pitch": 3.141592653589793, + "roll": 1.570796326589793, + "yaw": 3.141592653589793 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 48.5, + "l": 260, + "translation": { + "X": 0, + "Y": 0, + "Z": -74.89999999999999 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "" + }, + "parent": "wrist_2_joint" + }, + { + "id": "wrist_3_link", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "" + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 48.5, + "l": 204, + "translation": { + "X": 0, + "Y": 0, + "Z": -70 + }, + "orientation": { + "type": "" + }, + "Label": "" + }, + "parent": "wrist_3_joint" + } + ], + "joints": [ + { + "id": "shoulder_pan_joint", + "type": "revolute", + "parent": "base_link", + "axis": { + "X": 0, + "Y": 0, + "Z": 1 + }, + "max": 360, + "min": -360 + }, + { + "id": "shoulder_lift_joint", + "type": "revolute", + "parent": "shoulder_link", + "axis": { + "X": 0, + "Y": 0, + "Z": 1 + }, + "max": 360, + "min": -360 + }, + { + "id": "elbow_joint", + "type": "revolute", + "parent": "upper_arm_link", + "axis": { + "X": 0, + "Y": 0, + "Z": 1 + }, + "max": 180, + "min": -180 + }, + { + "id": "wrist_1_joint", + "type": "revolute", + "parent": "forearm_link", + "axis": { + "X": 0, + "Y": 0, + "Z": 1 + }, + "max": 360, + "min": -360 + }, + { + "id": "wrist_2_joint", + "type": "revolute", + "parent": "wrist_1_link", + "axis": { + "X": 0, + "Y": 0, + "Z": 1 + }, + "max": 360, + "min": -360 + }, + { + "id": "wrist_3_joint", + "type": "revolute", + "parent": "wrist_2_link", + "axis": { + "X": 0, + "Y": 0, + "Z": 1 + }, + "max": 360, + "min": -360 + } + ], + "OriginalFile": { + "Bytes": "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", + "Extension": "json" + } + }, + "limits": [ + { + "Min": -6.283185307179586, + "Max": 6.283185307179586 + }, + { + "Min": -6.283185307179586, + "Max": 6.283185307179586 + }, + { + "Min": -3.141592653589793, + "Max": 3.141592653589793 + }, + { + "Min": -6.283185307179586, + "Max": 6.283185307179586 + }, + { + "Min": -6.283185307179586, + "Max": 6.283185307179586 + }, + { + "Min": 0.85, + "Max": 4.384 + } + ] + } + }, + "ur20-modular_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "ur20-modular_origin", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "wall": { + "frame_type": "model", + "frame": { + "name": "wall", + "model": { + "name": "wall", + "links": [ + { + "id": "", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "box", + "x": 10000, + "y": 10, + "z": 10000, + "r": 0, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "" + }, + "parent": "world" + } + ], + "OriginalFile": { + "Bytes": "eyJPcmlnaW5hbEZpbGUiOnt9LCJsaW5rcyI6W3siZ2VvbWV0cnkiOnsiTGFiZWwiOiIiLCJsIjowLCJvcmllbnRhdGlvbiI6eyJ0eXBlIjoicXVhdGVybmlvbiIsInZhbHVlIjp7IlciOjEsIlgiOjAsIlkiOjAsIloiOjB9fSwiciI6MCwidHJhbnNsYXRpb24iOnsiWCI6MCwiWSI6MCwiWiI6MH0sInR5cGUiOiJib3giLCJ4IjoxMDAwMCwieSI6MTAsInoiOjEwMDAwfSwiaWQiOiIiLCJvcmllbnRhdGlvbiI6eyJ0eXBlIjoicXVhdGVybmlvbiIsInZhbHVlIjp7IlciOjEsIlgiOjAsIlkiOjAsIloiOjB9fSwicGFyZW50Ijoid29ybGQiLCJ0cmFuc2xhdGlvbiI6eyJYIjowLCJZIjowLCJaIjowfX1dLCJuYW1lIjoid2FsbCJ9", + "Extension": "json" + } + }, + "limits": [] + } + }, + "wall_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "wall_origin", + "translation": { + "X": 0, + "Y": -530, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + } + }, + "parents": { + "base": "base_origin", + "base_origin": "world", + "compliance": "compliance_origin", + "compliance_origin": "sander", + "computers": "computers_origin", + "computers_origin": "world", + "floor": "floor_origin", + "floor_origin": "world", + "hardtop-middle-wall": "hardtop-middle-wall_origin", + "hardtop-middle-wall_origin": "world", + "hose-guide": "hose-guide_origin", + "hose-guide_origin": "sander", + "pedestal": "pedestal_origin", + "pedestal_origin": "world", + "sander": "sander_origin", + "sander_origin": "ur20-modular", + "sensing-camera": "sensing-camera_origin", + "sensing-camera_origin": "ur20-modular", + "ur20-modular": "ur20-modular_origin", + "ur20-modular_origin": "world", + "wall": "wall_origin", + "wall_origin": "world" + } + }, + "frame_system_inputs": { + "base": [], + "base_origin": [], + "compliance": [], + "compliance_origin": [], + "computers": [], + "computers_origin": [], + "floor": [], + "floor_origin": [], + "hardtop-middle-wall": [], + "hardtop-middle-wall_origin": [], + "hose-guide": [], + "hose-guide_origin": [], + "pedestal": [], + "pedestal_origin": [], + "sander": [], + "sander_origin": [], + "sensing-camera": [], + "sensing-camera_origin": [], + "ur20-modular": [ + -3.6784356276141565, -3.103138109246725, -0.7698498368263245, -0.8671763700297851, + 1.594351887702942, 2.5805976390838623 + ], + "ur20-modular_origin": [], + "wall": [], + "wall_origin": [] + }, + "scene_config": { + "arm": "ur20-modular", + "camera": "sensing-camera", + "vacuum": "vacuum-fake" + }, + "scene_camera": { + "position": { + "X": 0, + "Y": 0, + "Z": 3000 + }, + "look_at": { + "X": 0, + "Y": 0, + "Z": 0 + } + } +} diff --git a/draw/arrow.go b/draw/arrow.go new file mode 100644 index 00000000..a54087b3 --- /dev/null +++ b/draw/arrow.go @@ -0,0 +1,60 @@ +package draw + +import ( + "fmt" + + "go.viam.com/rdk/spatialmath" +) + +// Arrows represents a set of poses in 3D space +type Arrows struct { + // The Poses to render + Poses []spatialmath.Pose + + // Either a single color or a color per pose + Colors []Color +} + +type drawArrowsConfig struct { + DrawColorsConfig +} + +func newDrawArrowsConfig() *drawArrowsConfig { + return &drawArrowsConfig{ + DrawColorsConfig: NewDrawColorsConfig(DefaultArrowColor), + } +} + +type drawArrowsOption func(*drawArrowsConfig) + +// WithArrowsColors sets the colors for the arrows +// - defaultColor is the default color for all arrows, or the first color if perPoseColors is provided +// - perPoseColors are the colors for each pose +func WithArrowsColors(defaultColor Color, perPoseColors ...Color) drawArrowsOption { + colors := []Color{defaultColor} + colors = append(colors, perPoseColors...) + return WithColors[*drawArrowsConfig](colors) +} + +// NewArrows creates a new Arrows object +// - Returns an error if the colors are not valid +func NewArrows(poses []spatialmath.Pose, options ...drawArrowsOption) (*Arrows, error) { + config := newDrawArrowsConfig() + for _, option := range options { + option(config) + } + + if len(config.colors) != 1 && len(config.colors) != len(poses) { + return nil, fmt.Errorf("colors must have length 1 (single color) or %d (per-arrow colors), got %d", len(poses), len(config.colors)) + } + + return &Arrows{Poses: poses, Colors: config.colors}, nil +} + +// Draw draws an arrow from a list of poses and colors +// - Defaults to green arrows if no colors are provided +func (arrows Arrows) Draw(name string, parent string, pose spatialmath.Pose) *Drawing { + shape := NewShape(pose, name, WithArrows(arrows)) + drawing := NewDrawing(name, parent, pose, shape, NewMetadata(WithMetadataColors(arrows.Colors...))) + return drawing +} diff --git a/draw/buffer_packer.go b/draw/buffer_packer.go new file mode 100644 index 00000000..15043b22 --- /dev/null +++ b/draw/buffer_packer.go @@ -0,0 +1,124 @@ +package draw + +import ( + "encoding/binary" + "fmt" + "math" + + "github.com/golang/geo/r3" + "go.viam.com/rdk/spatialmath" +) + +// BufferPacker provides efficient direct buffer writing +// T must be a float32 or uint8 +type BufferPacker[T any] struct { + buffer []T + offset int +} + +// NewBufferPacker creates a new packer with pre-allocated capacity. +// - elementCount is the number of elements +// - fieldsPerElement is the number of fields per element +func NewBufferPacker[T any](elementCount, fieldsPerElement int) *BufferPacker[T] { + return &BufferPacker[T]{ + buffer: make([]T, elementCount*fieldsPerElement), + offset: 0, + } +} + +// Write appends float32 values directly to the buffer +// - values are the values to write +func (packer *BufferPacker[T]) Write(values ...T) { + copy(packer.buffer[packer.offset:], values) + packer.offset += len(values) +} + +// Read returns the packed buffer as little-endian bytes +func (packer *BufferPacker[T]) Read() []byte { + var bytesPerElement int + if len(packer.buffer) > 0 { + switch any(packer.buffer[0]).(type) { + case float32: + bytesPerElement = 4 + case uint8: + bytesPerElement = 1 + default: + panic(fmt.Sprintf("unsupported type: %T", packer.buffer[0])) + } + } else { + return []byte{} + } + + bytes := make([]byte, len(packer.buffer)*bytesPerElement) + for i, v := range packer.buffer { + switch v := any(v).(type) { + case float32: + binary.LittleEndian.PutUint32(bytes[i*4:], math.Float32bits(v)) + case uint8: + bytes[i] = v + default: + panic(fmt.Sprintf("unsupported type: %T", v)) + } + } + return bytes +} + +// packFloats packs a slice of float64 values into a Float32Array byte representation +// - floats are the values to pack +func packFloats(floats []float64) []byte { + packer := NewBufferPacker[float32](len(floats), 1) + for _, f := range floats { + packer.Write(float32(f)) + } + return packer.Read() +} + +// packPoints packs a slice of 3D points into a Float32Array byte representation +// - dots are the points to pack: [x, y, z] +func packPoints(dots []r3.Vector) []byte { + packer := NewBufferPacker[float32](len(dots), 3) + + for _, dot := range dots { + packer.Write(float32(dot.X), float32(dot.Y), float32(dot.Z)) + } + + return packer.Read() +} + +// packPoses packs a slice of 3D poses into a Float32Array byte representation +// - poses are the poses to pack: [x, y, z, ox, oy, oz, theta (if theta is true)] +// - theta is whether to include the theta value +func packPoses(poses []spatialmath.Pose, theta bool) []byte { + fields := 6 + if theta { + fields = 7 + } + + packer := NewBufferPacker[float32](len(poses), fields) + + for _, pose := range poses { + point := pose.Point() + ov := pose.Orientation().OrientationVectorDegrees() + packer.Write( + float32(point.X), float32(point.Y), float32(point.Z), + float32(ov.OX), float32(ov.OY), float32(ov.OZ), + ) + if theta { + packer.Write(float32(ov.Theta)) + } + } + + return packer.Read() +} + +// packColors packs a slice of Color values into a []uint8 byte representation +// - colors are the colors to pack: [r, g, b, a] +func packColors(colors []Color) []byte { + packer := NewBufferPacker[uint8](len(colors), 4) + + for _, rgba := range colors { + packer.Write(rgba.R, rgba.G, rgba.B, rgba.A) + } + + return packer.Read() +} diff --git a/draw/color.go b/draw/color.go new file mode 100644 index 00000000..1b7e7ccf --- /dev/null +++ b/draw/color.go @@ -0,0 +1,192 @@ +package draw + +import ( + "image/color" + + "golang.org/x/image/colornames" +) + +// DefaultAlpha is the default alpha value, defaults to 1.0 +var DefaultAlpha = uint8(255) + +type Color struct { + R uint8 `json:"r"` + G uint8 `json:"g"` + B uint8 `json:"b"` + A uint8 `json:"a"` +} + +// Default colors as specified in drawing.proto +var ( + // DefaultArrowColor is the default color of an arrow, defaults to green + DefaultArrowColor = NewColor(WithName("green")) + + // DefaultLineColor is the default color of a line, defaults to blue + DefaultLineColor = NewColor(WithName("blue")) + + // DefaultLinePointColor is the default color of a line point, defaults to darker blue + DefaultLinePointColor = NewColor(WithName("darkblue")) + + // DefaultPointColor is the default color of a point, defaults to gray + DefaultPointColor = NewColor(WithName("gray")) +) + +type colorConfig struct { + r uint8 + g uint8 + b uint8 + a uint8 +} + +func newColorConfig() *colorConfig { + return &colorConfig{ + r: 0, + g: 0, + b: 0, + a: DefaultAlpha, + } +} + +type colorOption func(*colorConfig) + +// WithRGB sets the color from RGB values (with an alpha of 1.0) +func WithRGB(r, g, b uint8) colorOption { + return func(config *colorConfig) { + config.r = r + config.g = g + config.b = b + } +} + +// WithRGBA sets the color from RGBA values +func WithRGBA(rgba color.RGBA) colorOption { + return func(config *colorConfig) { + config.r = rgba.R + config.g = rgba.G + config.b = rgba.B + config.a = rgba.A + } +} + +// WithName sets the color from a named color +// +// See: https://www.w3.org/TR/SVG11/types.html#ColorKeywords +func WithName(name string) colorOption { + return func(config *colorConfig) { + color := colornames.Map[name] + WithRGBA(color)(config) + } +} + +// WithHSV sets the color from HSV values (with an alpha of 255) +// h, s, v are expected in the 0-1 range +func WithHSV(h, s, v float32) colorOption { + return func(config *colorConfig) { + i := int(h * 6) + f := h*6 - float32(i) + p := v * (1 - s) + q := v * (1 - f*s) + t := v * (1 - (1-f)*s) + + var r, g, b float32 + switch i % 6 { + case 0: + r, g, b = v, t, p + case 1: + r, g, b = q, v, p + case 2: + r, g, b = p, v, t + case 3: + r, g, b = p, q, v + case 4: + r, g, b = t, p, v + case 5: + r, g, b = v, p, q + } + + config.r = uint8(r * 255) + config.g = uint8(g * 255) + config.b = uint8(b * 255) + config.a = 255 + } +} + +// NewColor creates a new color, defaults to black with an alpha of 1.0 +func NewColor(options ...colorOption) Color { + config := newColorConfig() + for _, option := range options { + option(config) + } + + return Color{ + R: config.r, + G: config.g, + B: config.b, + A: config.a, + } +} + +// SetRGB sets the color RGB values +func (color Color) SetRGB(r, g, b uint8) Color { + color.R = r + color.G = g + color.B = b + return color +} + +// SetRGBA sets the color from RGBA values +func (color Color) SetRGBA(r, g, b, a uint8) Color { + color.SetRGB(r, g, b) + color.A = a + return color +} + +// SetAlpha sets the alpha value of the color +func (color Color) SetAlpha(alpha uint8) Color { + color.A = alpha + return color +} + +type ColorChooser struct { + count int + colors []Color +} + +func (chooser ColorChooser) Next() Color { + color := chooser.colors[chooser.count%len(chooser.colors)] + chooser.count++ + return color +} + +func NewDefaultColorChooser() ColorChooser { + colors := make([]Color, len(colornames.Map)) + for _, rgba := range colornames.Map { + colors = append(colors, NewColor(WithRGBA(rgba))) + } + + return ColorChooser{colors: colors} +} + +type ConfigurableColors interface { + SetColors([]Color) +} + +type DrawColorsConfig struct { + colors []Color +} + +func NewDrawColorsConfig(colors ...Color) DrawColorsConfig { + return DrawColorsConfig{ + colors: colors, + } +} + +func (config *DrawColorsConfig) SetColors(colors []Color) { + config.colors = colors +} + +func WithColors[T ConfigurableColors](colors []Color) func(T) { + return func(config T) { + config.SetColors(colors) + } +} diff --git a/draw/drawing.go b/draw/drawing.go new file mode 100644 index 00000000..7d0274bf --- /dev/null +++ b/draw/drawing.go @@ -0,0 +1,261 @@ +package draw + +import ( + "github.com/google/uuid" + drawv1 "github.com/viam-labs/motion-tools/gen/draw/v1" + commonv1 "go.viam.com/api/common/v1" + "go.viam.com/rdk/spatialmath" +) + +// Shape represents a Shape in 3D space +type Shape struct { + Center spatialmath.Pose + Label string + Arrows *Arrows + Line *Line + Points *Points + Model *Model + Nurbs *Nurbs +} + +type drawShapeConfig struct { + arrows *Arrows + line *Line + points *Points + model *Model + nurbs *Nurbs +} + +func newDrawShapeConfig() *drawShapeConfig { + return &drawShapeConfig{ + arrows: nil, + line: nil, + points: nil, + model: nil, + nurbs: nil, + } +} + +type drawShapeOption func(*drawShapeConfig) + +func WithArrows(arrows Arrows) drawShapeOption { + return func(config *drawShapeConfig) { + config.arrows = &arrows + } +} + +// WithLine adds a Line to the Shape +func WithLine(line Line) drawShapeOption { + return func(config *drawShapeConfig) { + config.line = &line + } +} + +// WithPoints adds a Points to the Shape +func WithPoints(points Points) drawShapeOption { + return func(config *drawShapeConfig) { + config.points = &points + } +} + +// WithModel adds a Model to the Shape +func WithModel(model Model) drawShapeOption { + return func(config *drawShapeConfig) { + config.model = &model + } +} + +// WithNurbs adds a Nurbs to the Shape +func WithNurbs(nurbs Nurbs) drawShapeOption { + return func(config *drawShapeConfig) { + config.nurbs = &nurbs + } +} + +// NewShape creates a new Shape message +func NewShape(center spatialmath.Pose, label string, option drawShapeOption) Shape { + config := newDrawShapeConfig() + option(config) + + return Shape{ + Center: center, + Label: label, + Arrows: config.arrows, + Line: config.line, + Points: config.points, + Model: config.model, + Nurbs: config.nurbs, + } +} + +// toProto converts the Shape to a drawv1.Shape message +func (shape Shape) toProto() *drawv1.Shape { + switch { + case shape.Arrows != nil: + return &drawv1.Shape{ + Label: shape.Label, + Center: poseToProtobuf(shape.Center), + GeometryType: &drawv1.Shape_Arrows{ + Arrows: &drawv1.Arrows{ + Poses: packPoses(shape.Arrows.Poses, false), + }, + }, + } + case shape.Line != nil: + lineWidth := shape.Line.LineWidth + pointSize := shape.Line.PointSize + return &drawv1.Shape{ + Label: shape.Label, + Center: poseToProtobuf(shape.Center), + GeometryType: &drawv1.Shape_Line{ + Line: &drawv1.Line{ + Positions: packPoints(shape.Line.Positions), + LineWidth: &lineWidth, + PointSize: &pointSize, + }, + }, + } + case shape.Points != nil: + pointSize := shape.Points.PointSize + return &drawv1.Shape{ + Label: shape.Label, + Center: poseToProtobuf(shape.Center), + GeometryType: &drawv1.Shape_Points{ + Points: &drawv1.Points{ + Positions: packPoints(shape.Points.Positions), + PointSize: &pointSize, + }, + }, + } + case shape.Model != nil: + proto := &drawv1.Model{ + Assets: []*drawv1.ModelAsset{}, + Scale: &commonv1.Vector3{ + X: shape.Model.Scale.X, + Y: shape.Model.Scale.Y, + Z: shape.Model.Scale.Z, + }, + AnimationName: &shape.Model.AnimationName, + } + + for _, asset := range shape.Model.Assets { + switch { + case asset.URLContent != nil: + proto.Assets = append(proto.Assets, &drawv1.ModelAsset{ + MimeType: asset.MimeType, + SizeBytes: asset.SizeBytes, + Content: &drawv1.ModelAsset_Url{ + Url: *asset.URLContent, + }, + }) + case asset.BinaryContent != nil: + proto.Assets = append(proto.Assets, &drawv1.ModelAsset{ + MimeType: asset.MimeType, + SizeBytes: asset.SizeBytes, + Content: &drawv1.ModelAsset_Binary{ + Binary: *asset.BinaryContent, + }, + }) + default: + return nil + } + } + + return &drawv1.Shape{ + Label: shape.Label, + Center: poseToProtobuf(shape.Center), + GeometryType: &drawv1.Shape_Model{ + Model: proto, + }, + } + case shape.Nurbs != nil: + return &drawv1.Shape{ + Label: shape.Label, + Center: poseToProtobuf(shape.Center), + GeometryType: &drawv1.Shape_Nurbs{ + Nurbs: &drawv1.Nurbs{ + ControlPoints: packPoses(shape.Nurbs.ControlPoints, true), + Degree: &shape.Nurbs.Degree, + Weights: packFloats(shape.Nurbs.Weights), + Knots: packFloats(shape.Nurbs.Knots), + }, + }, + } + default: + return nil + } +} + +// Drawing represents a drawing in 3D space +type Drawing struct { + Name string + Parent string + Pose spatialmath.Pose + Shape Shape + Metadata Metadata +} + +// Drawable represents a shape that can be drawn +type Drawable interface { + Draw(name string, parent string, pose spatialmath.Pose) (*Drawing, error) +} + +// NewDrawing creates a new Drawing message +func NewDrawing( + name string, + parent string, + pose spatialmath.Pose, + shape Shape, + metadata Metadata, +) *Drawing { + return &Drawing{Name: name, Parent: parent, Pose: pose, Shape: shape, Metadata: metadata} +} + +// toProto converts the Drawing to a drawv1.Drawing message with unit conversion +func (drawing Drawing) toProto() *drawv1.Drawing { + pose := poseInFrameToProtobuf(drawing.Pose, drawing.Parent) + uuidBytes := uuid.New() + return &drawv1.Drawing{ + ReferenceFrame: drawing.Name, + PoseInObserverFrame: pose, + PhysicalObject: drawing.Shape.toProto(), + Uuid: uuidBytes[:], + Metadata: drawing.Metadata.ToProto(), + } +} + +// Metadata represents the metadata of a drawing +type Metadata struct { + Colors []Color +} + +type drawMetadataConfig struct { + DrawColorsConfig +} + +type drawMetadataOption func(*drawMetadataConfig) + +func newDrawMetadataConfig() *drawMetadataConfig { + return &drawMetadataConfig{ + DrawColorsConfig: NewDrawColorsConfig(), + } +} + +func WithMetadataColors(colors ...Color) drawMetadataOption { + return WithColors[*drawMetadataConfig](colors) +} + +// NewMetadata creates a new Metadata message +func NewMetadata(options ...drawMetadataOption) Metadata { + config := newDrawMetadataConfig() + for _, option := range options { + option(config) + } + + return Metadata{Colors: config.colors} +} + +// ToProto converts the Metadata to a drawv1.Metadata message +func (metadata Metadata) ToProto() *drawv1.Metadata { + return &drawv1.Metadata{Colors: packColors(metadata.Colors)} +} diff --git a/draw/frame_system.go b/draw/frame_system.go new file mode 100644 index 00000000..94b2ab9c --- /dev/null +++ b/draw/frame_system.go @@ -0,0 +1,70 @@ +package draw + +import ( + "maps" + "slices" + + "github.com/google/uuid" + drawv1 "github.com/viam-labs/motion-tools/gen/draw/v1" + commonv1 "go.viam.com/api/common/v1" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +// DrawFrameSystemGeometries draws a frame system's geometries in the world frame +// Colors are mapped by frame name +// Returns the transforms that were drawn +func DrawFrameSystemGeometries( + frameSystem *referenceframe.FrameSystem, + inputs referenceframe.FrameSystemInputs, + colors map[string]Color, +) (*drawv1.Transforms, error) { + transforms := &drawv1.Transforms{ + Transforms: make([]*commonv1.Transform, 0), + } + + frameMap, err := referenceframe.FrameSystemGeometries(frameSystem, inputs) + if err != nil { + return nil, err + } + + for _, frameName := range slices.Sorted(maps.Keys(frameMap)) { + geometries := frameMap[frameName] + color := getFrameColor(frameName, colors, frameSystem) + + for _, geometry := range geometries.Geometries() { + label := geometry.Label() + pose := spatialmath.NewZeroPose() + metadata := NewMetadata(WithMetadataColors(color)) + metadataStruct, err := MetadataToStruct(metadata) + if err != nil { + return nil, err + } + + transform, err := NewTransform(uuid.New().String(), label, referenceframe.World, pose, geometry, metadataStruct) + if err != nil { + return nil, err + } + + transforms.Transforms = append(transforms.Transforms, transform) + } + } + + return transforms, nil +} + +func getFrameColor(frameName string, colors map[string]Color, frameSystem *referenceframe.FrameSystem) Color { + if color, ok := colors[frameName]; ok { + return color + } + + frame := frameSystem.Frame(frameName) + if frame != nil { + parent, err := frameSystem.Parent(frame) + if err == nil && parent != nil { + return getFrameColor(parent.Name(), colors, frameSystem) + } + } + + return NewColor(WithName("magenta")) +} diff --git a/draw/geometry.go b/draw/geometry.go new file mode 100644 index 00000000..3d0e59e5 --- /dev/null +++ b/draw/geometry.go @@ -0,0 +1,44 @@ +package draw + +import ( + drawv1 "github.com/viam-labs/motion-tools/gen/draw/v1" + commonv1 "go.viam.com/api/common/v1" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +// DrawGeometry draws a geometry +func DrawGeometry( + id string, + geometry spatialmath.Geometry, + pose spatialmath.Pose, + parent string, + color Color, +) (*commonv1.Transform, error) { + label := geometry.Label() + metadata := NewMetadata(WithMetadataColors(color)) + metadataStruct, err := MetadataToStruct(metadata) + if err != nil { + return nil, err + } + + return NewTransform(id, label, parent, pose, geometry, metadataStruct) +} + +func DrawGeometries(geometries *referenceframe.GeometriesInFrame, colors []Color) (*drawv1.Transforms, error) { + geos := geometries.Geometries() + transforms := &drawv1.Transforms{ + Transforms: make([]*commonv1.Transform, len(geos)), + } + + for i, geometry := range geos { + transform, err := DrawGeometry("", geometry, geometry.Pose(), geometries.Parent(), colors[i]) + if err != nil { + return nil, err + } + + transforms.Transforms[i] = transform + } + + return transforms, nil +} diff --git a/draw/line.go b/draw/line.go new file mode 100644 index 00000000..f6636a93 --- /dev/null +++ b/draw/line.go @@ -0,0 +1,112 @@ +package draw + +import ( + "fmt" + + "github.com/golang/geo/r3" + "go.viam.com/rdk/spatialmath" +) + +var ( + // DefaultLineWidth is the default width of a line in millimeters + DefaultLineWidth float32 = 5.0 +) + +// Line represents a Line in 3D space +type Line struct { + // The positions of the line points to render + Positions []r3.Vector + + // The width of the line, defaults to 5mm + LineWidth float32 + + // The size of the points, defaults to 10mm + PointSize float32 + + // The color of the line, defaults to blue + LineColor Color + + // The color of the points, defaults to darker blue + PointColor Color +} + +// drawLineConfig holds configuration options for creating a Line. +type drawLineConfig struct { + lineWidth float32 + pointSize float32 + DrawColorsConfig +} + +func newDrawLineConfig(lineWidth float32, pointSize float32, colors ...Color) *drawLineConfig { + return &drawLineConfig{ + lineWidth: lineWidth, + pointSize: pointSize, + DrawColorsConfig: NewDrawColorsConfig(colors...), + } +} + +// drawLineOption is a functional option for configuring a Line +type drawLineOption func(*drawLineConfig) + +// WithLineWidth sets the width of the line in millimeters +func WithLineWidth(width float32) drawLineOption { + return func(config *drawLineConfig) { + config.lineWidth = width + } +} + +// WithPointSize sets the size of the points/dots at line vertices +func WithPointSize(size float32) drawLineOption { + return func(config *drawLineConfig) { + config.pointSize = size + } +} + +// WithLineColors sets the color of the line and points +// If pointColor is nil, uses the line color for points +func WithLineColors(lineColor Color, pointColor *Color) drawLineOption { + colors := []Color{lineColor, lineColor} + if pointColor != nil { + colors[1] = *pointColor + } + + return WithColors[*drawLineConfig](colors) +} + +// NewLine creates a new Line object +func NewLine(positions []r3.Vector, options ...drawLineOption) (*Line, error) { + config := newDrawLineConfig(DefaultLineWidth, DefaultPointSize, DefaultLineColor, DefaultLinePointColor) + for _, option := range options { + option(config) + } + + if len(positions) < 2 { + return nil, fmt.Errorf("line must have at least 2 positions, got %d", len(positions)) + } + + if config.pointSize <= 0 { + return nil, fmt.Errorf("point size must be greater than 0, got %f", config.pointSize) + } + + return &Line{ + Positions: positions, + LineWidth: config.lineWidth, + PointSize: config.pointSize, + LineColor: config.colors[0], + PointColor: config.colors[1], + }, nil +} + +// Draw draws a line from a list of points, colors, and optional styling +// If colors is nil or empty, uses DefaultLineColor (blue) for both line and points +// If colors has 1 element, uses that color for both line and points +// If colors has 2 elements, first is for line, second is for points +func (line Line) Draw( + name string, + parent string, + pose spatialmath.Pose, +) *Drawing { + shape := NewShape(pose, name, WithLine(line)) + drawing := NewDrawing(name, parent, pose, shape, NewMetadata(WithMetadataColors(line.LineColor, line.PointColor))) + return drawing +} diff --git a/draw/model.go b/draw/model.go new file mode 100644 index 00000000..575f837d --- /dev/null +++ b/draw/model.go @@ -0,0 +1,153 @@ +package draw + +import ( + "fmt" + + "github.com/golang/geo/r3" + "go.viam.com/rdk/spatialmath" +) + +var ( + // DefaultModelScale is the default scale of a model, defaults to 1.0 + DefaultModelScale = r3.Vector{X: 1.0, Y: 1.0, Z: 1.0} + + // DefaultModelAnimationName is the default animation name of a model, defaults to empty string (no animation) + DefaultModelAnimationName = "" +) + +// ModelAsset represents a model asset +type ModelAsset struct { + MimeType string + SizeBytes *uint64 + URLContent *string + BinaryContent *[]byte +} +type drawModelAssetConfig struct { + sizeBytes *uint64 + urlContent *string + binaryContent *[]byte +} + +func newDrawModelAssetConfig() *drawModelAssetConfig { + return &drawModelAssetConfig{ + sizeBytes: nil, + urlContent: nil, + binaryContent: nil, + } +} + +type drawModelAssetOption func(*drawModelAssetConfig) + +func WithModelAssetSizeBytes(sizeBytes uint64) drawModelAssetOption { + return func(config *drawModelAssetConfig) { + config.sizeBytes = &sizeBytes + } +} + +func NewURLModelAsset(mimeType string, url string, options ...drawModelAssetOption) (*ModelAsset, error) { + if url == "" { + return nil, fmt.Errorf("url cannot be empty") + } + + config := newDrawModelAssetConfig() + for _, option := range options { + option(config) + } + + return &ModelAsset{ + MimeType: mimeType, + SizeBytes: config.sizeBytes, + URLContent: &url, + }, nil +} + +func NewBinaryModelAsset(mimeType string, binaryContent []byte, options ...drawModelAssetOption) (*ModelAsset, error) { + if len(binaryContent) == 0 { + return nil, fmt.Errorf("binary content cannot be empty") + } + + config := newDrawModelAssetConfig() + for _, option := range options { + option(config) + } + + return &ModelAsset{ + MimeType: mimeType, + SizeBytes: config.sizeBytes, + BinaryContent: &binaryContent, + }, nil +} + +// Model represents a 3D Model in various formats +type Model struct { + // The list of assets that make up the model + Assets []*ModelAsset + + // The Scale of the model, defaults to [1.0, 1.0, 1.0] + Scale r3.Vector + + // Name of the animation to play, defaults to empty string (no animation) + AnimationName string +} + +type drawModelConfig struct { + assets []*ModelAsset + scale r3.Vector + animationName string +} + +func newDrawModelConfig() *drawModelConfig { + return &drawModelConfig{ + assets: []*ModelAsset{}, + scale: DefaultModelScale, + animationName: DefaultModelAnimationName, + } +} + +type drawModelOption func(*drawModelConfig) + +func WithModelAssets(assets ...*ModelAsset) drawModelOption { + return func(config *drawModelConfig) { + config.assets = append(config.assets, assets...) + } +} + +func WithModelScale(scale r3.Vector) drawModelOption { + return func(config *drawModelConfig) { + config.scale = scale + } +} + +func WithModelAnimationName(animationName string) drawModelOption { + return func(config *drawModelConfig) { + config.animationName = animationName + } +} + +func NewModel(options ...drawModelOption) (*Model, error) { + config := newDrawModelConfig() + for _, option := range options { + option(config) + } + + if len(config.assets) == 0 { + return nil, fmt.Errorf("model must have at least one asset") + } + + if config.scale.X <= 0 || config.scale.Y <= 0 || config.scale.Z <= 0 { + return nil, fmt.Errorf("scale must be positive, got %v", config.scale) + } + + return &Model{ + Assets: config.assets, + Scale: config.scale, + AnimationName: config.animationName, + }, nil +} + +// Draw draws a model from a URL or GLB bytes +func (model Model) Draw(name string, parent string, pose spatialmath.Pose) *Drawing { + shape := NewShape(pose, name, WithModel(model)) + drawing := NewDrawing(name, parent, pose, shape, NewMetadata()) + return drawing +} diff --git a/draw/nurbs.go b/draw/nurbs.go new file mode 100644 index 00000000..b5f24785 --- /dev/null +++ b/draw/nurbs.go @@ -0,0 +1,133 @@ +package draw + +import ( + "fmt" + + "go.viam.com/rdk/spatialmath" +) + +var ( + // DefaultNurbsDegree is the default degree of a NURBS curve + DefaultNurbsDegree int32 = 3 + + // DefaultNurbsWeights is the default weights of a NURBS curve, defaults to [1, ...] + DefaultNurbsWeight = 1.0 + + // DefaultNurbsColor is the default color of a NURBS curve, defaults to [0, 1, 1, 0.7] (cyan) + DefaultNurbsColor = NewColor(WithName("cyan")) +) + +// Nurbs represents a NURBS curve in 3D space +type Nurbs struct { + // The control points of the NURBS + ControlPoints []spatialmath.Pose + + // The knot vector of the NURBS + Knots []float64 + + // The Degree of the NURBS, defaults to 3 + Degree int32 + + // The Weights of the NURBS, defaults to [1, ...] + Weights []float64 + + // Either a single color or a color per control point + Color Color +} + +type drawNurbsConfig struct { + degree int32 + weights []float64 + DrawColorsConfig +} + +func newDrawNurbsConfig() *drawNurbsConfig { + return &drawNurbsConfig{ + degree: DefaultNurbsDegree, + weights: []float64{}, + DrawColorsConfig: NewDrawColorsConfig(DefaultNurbsColor), + } +} + +type drawNurbsOption func(*drawNurbsConfig) + +// WithNurbsDegree sets the degree of the NURBS curve +func WithNurbsDegree(degree int32) drawNurbsOption { + return func(config *drawNurbsConfig) { + config.degree = degree + } +} + +// WithNurbsWeights sets the weights of the NURBS curve control points +func WithNurbsWeights(weights []float64) drawNurbsOption { + return func(config *drawNurbsConfig) { + config.weights = weights + } +} + +// WithNurbsColors sets the colors of the NURBS curve +// Can be a single color or a color per control point +func WithNurbsColors(defaultColor Color, perPointColors ...Color) drawNurbsOption { + colors := []Color{defaultColor} + colors = append(colors, perPointColors...) + return WithColors[*drawNurbsConfig](colors) +} + +// NewNurbs creates a new Nurbs object with functional options +func NewNurbs(controlPoints []spatialmath.Pose, knots []float64, options ...drawNurbsOption) (*Nurbs, error) { + if len(controlPoints) == 0 { + return nil, fmt.Errorf("control points cannot be empty") + } + + if len(knots) == 0 { + return nil, fmt.Errorf("knots cannot be empty") + } + + config := newDrawNurbsConfig() + for _, option := range options { + option(config) + } + + if config.degree <= 0 { + return nil, fmt.Errorf("degree must be greater than 0, got %d", config.degree) + } + + // Default weights to 1.0 for each control point if not provided + weights := config.weights + if len(weights) == 0 { + weights = make([]float64, len(controlPoints)) + for i := range weights { + weights[i] = 1.0 + } + } else if len(weights) != len(controlPoints) { + return nil, fmt.Errorf("weights must have length %d, got %d", len(controlPoints), len(weights)) + } + + if len(config.colors) != 1 && len(config.colors) != len(controlPoints) { + return nil, fmt.Errorf("colors must have length 1 (single color) or %d (per-control-point colors), got %d", len(controlPoints), len(config.colors)) + } + + if len(knots) != len(controlPoints)+int(config.degree)+1 { + return nil, fmt.Errorf("knots must have length %d, got %d", len(controlPoints)+int(config.degree)+1, len(knots)) + } + + return &Nurbs{ + ControlPoints: controlPoints, + Degree: config.degree, + Weights: weights, + Knots: knots, + Color: config.colors[0], + }, nil +} + +// Draw draws a NURBS curve from a list of control points, weights, knots, and colors +// If colors is nil or empty, uses DefaultNurbsColor (cyan) +func (nurbs Nurbs) Draw( + name string, + parent string, + pose spatialmath.Pose, +) *Drawing { + shape := NewShape(pose, name, WithNurbs(nurbs)) + drawing := NewDrawing(name, parent, pose, shape, NewMetadata(WithMetadataColors(nurbs.Color))) + return drawing +} diff --git a/draw/points.go b/draw/points.go new file mode 100644 index 00000000..e9d36ce2 --- /dev/null +++ b/draw/points.go @@ -0,0 +1,92 @@ +package draw + +import ( + "fmt" + + "github.com/golang/geo/r3" + "go.viam.com/rdk/spatialmath" +) + +var ( + // DefaultPointSize is the default size of a point in millimeters + DefaultPointSize float32 = 10.0 +) + +// Points represents a set of Points in 3D space +type Points struct { + // The Positions to render + Positions []r3.Vector + + // The size of the points, defaults to 10mm + PointSize float32 + + // Either a single color or a color per point + Colors []Color +} + +type drawPointsConfig struct { + pointSize float32 + DrawColorsConfig +} + +func newDrawPointsConfig() *drawPointsConfig { + return &drawPointsConfig{ + pointSize: DefaultPointSize, + DrawColorsConfig: NewDrawColorsConfig(DefaultPointColor), + } +} + +type drawPointsOption func(*drawPointsConfig) + +// WithPointsSize sets the size of the points in millimeters +func WithPointsSize(size float32) drawPointsOption { + return func(config *drawPointsConfig) { + config.pointSize = size + } +} + +// WithPointsColors sets the colors of the points +// Can be a single color or a color per point +func WithPointsColors(defaultColor Color, perPointColors ...Color) drawPointsOption { + colors := []Color{defaultColor} + colors = append(colors, perPointColors...) + return WithColors[*drawPointsConfig](colors) +} + +// NewPoints creates a new Points object with functional options +func NewPoints(positions []r3.Vector, options ...drawPointsOption) (*Points, error) { + if len(positions) == 0 { + return nil, fmt.Errorf("positions cannot be empty") + } + + config := newDrawPointsConfig() + for _, option := range options { + option(config) + } + + if config.pointSize <= 0 { + return nil, fmt.Errorf("point size must be greater than 0, got %f", config.pointSize) + } + + if len(config.colors) != 1 && len(config.colors) != len(positions) { + return nil, fmt.Errorf("colors must have length 1 (single color) or %d (per-point colors), got %d", len(positions), len(config.colors)) + } + + return &Points{ + Positions: positions, + PointSize: config.pointSize, + Colors: config.colors, + }, nil +} + +// Draw draws a set of points from a list of positions and colors +// If colors is nil or empty, uses DefaultPointColor (gray) +func (points Points) Draw( + name string, + parent string, + pose spatialmath.Pose, +) *Drawing { + shape := NewShape(pose, name, WithPoints(points)) + drawing := NewDrawing(name, parent, pose, shape, NewMetadata(WithMetadataColors(points.Colors...))) + return drawing +} diff --git a/draw/protos.go b/draw/protos.go new file mode 100644 index 00000000..0aab9695 --- /dev/null +++ b/draw/protos.go @@ -0,0 +1,49 @@ +package draw + +import ( + commonv1 "go.viam.com/api/common/v1" + "go.viam.com/rdk/spatialmath" +) + +func poseToProtobuf(pose spatialmath.Pose) *commonv1.Pose { + poseProto := spatialmath.PoseToProtobuf(pose) + return poseProto +} + +func poseInFrameToProtobuf(pose spatialmath.Pose, parent string) *commonv1.PoseInFrame { + + return &commonv1.PoseInFrame{ + ReferenceFrame: parent, + Pose: poseToProtobuf(pose), + } +} + +func geometryToProtobuf(geometry spatialmath.Geometry) *commonv1.Geometry { + geometryProto := geometry.ToProtobuf() + sphere := geometryProto.GetSphere() + if sphere != nil { + geometryProto.GeometryType = &commonv1.Geometry_Sphere{ + Sphere: sphere, + } + + return geometryProto + } + + box := geometryProto.GetBox() + if box != nil { + geometryProto.GeometryType = &commonv1.Geometry_Box{ + Box: box, + } + return geometryProto + } + + capsule := geometryProto.GetCapsule() + if capsule != nil { + geometryProto.GeometryType = &commonv1.Geometry_Capsule{ + Capsule: capsule, + } + return geometryProto + } + + return geometryProto +} diff --git a/draw/scene.go b/draw/scene.go new file mode 100644 index 00000000..e2786e3a --- /dev/null +++ b/draw/scene.go @@ -0,0 +1,310 @@ +package draw + +import ( + "fmt" + + "github.com/golang/geo/r3" + drawv1 "github.com/viam-labs/motion-tools/gen/draw/v1" + commonv1 "go.viam.com/api/common/v1" +) + +var ( + // DefaultSceneCamera is the default scene camera, defaults to a perspective camera with a position of [3000, 3000, 3000] and a look_at of [0, 0, 0] + DefaultSceneCamera = SceneCamera{ + // Top-down view: directly above origin (in mm) + Position: r3.Vector{X: 3000, Y: 3000, Z: 3000}, + // Look at the origin + LookAt: r3.Vector{X: 0, Y: 0, Z: 0}, + Animated: false, + PerspectiveCamera: &drawv1.PerspectiveCamera{}, + } + + // DefaultGridEnabled is the default enabled state of the grid, defaults to true + DefaultGridEnabled = true + // DefaultGridCellSize is the default cell size of the grid in millimeters, defaults to 0.5 + DefaultGridCellSize float32 = 500.0 + // DefaultGridSectionSize is the default section size of the grid in millimeters, defaults to 10.0 + DefaultGridSectionSize float32 = 10000.0 + // DefaultGridFadeDistance is the default fade distance of the grid in millimeters, defaults to 25.0 + DefaultGridFadeDistance float32 = 25000.0 +) + +// SceneCamera represents a scene camera +type SceneCamera struct { + // The position of the camera in millimeters + Position r3.Vector + // The look at point of the camera in millimeters + LookAt r3.Vector + // Whether to animate the camera + Animated bool + // The perspective camera configuration + PerspectiveCamera *drawv1.PerspectiveCamera + // The orthographic camera configuration + OrthographicCamera *drawv1.OrthographicCamera +} + +type sceneCameraConfig struct { + animated bool + perspectiveCamera *drawv1.PerspectiveCamera + orthographicCamera *drawv1.OrthographicCamera +} + +func newSceneCameraConfig() *sceneCameraConfig { + return &sceneCameraConfig{ + perspectiveCamera: &drawv1.PerspectiveCamera{}, + } +} + +type sceneCameraOption func(*sceneCameraConfig) + +func WithPerspectiveCamera(perspectiveCamera *drawv1.PerspectiveCamera) sceneCameraOption { + return func(config *sceneCameraConfig) { + config.perspectiveCamera = perspectiveCamera + } +} + +func WithOrthographicCamera(orthographicCamera *drawv1.OrthographicCamera) sceneCameraOption { + return func(config *sceneCameraConfig) { + config.orthographicCamera = orthographicCamera + } +} + +func WithAnimated(animated bool) sceneCameraOption { + return func(config *sceneCameraConfig) { + config.animated = animated + } +} + +// NewSceneCamera creates a new SceneCamera +func NewSceneCamera(position r3.Vector, lookAt r3.Vector, options ...sceneCameraOption) SceneCamera { + config := newSceneCameraConfig() + for _, option := range options { + option(config) + } + + return SceneCamera{ + Position: position, + LookAt: lookAt, + Animated: config.animated, + PerspectiveCamera: config.perspectiveCamera, + OrthographicCamera: config.orthographicCamera, + } +} + +// ToProto converts a SceneCamera to a protobuf SceneCamera +func (camera *SceneCamera) ToProto() *drawv1.SceneCamera { + position := &commonv1.Vector3{X: camera.Position.X, Y: camera.Position.Y, Z: camera.Position.Z} + lookAt := &commonv1.Vector3{X: camera.LookAt.X, Y: camera.LookAt.Y, Z: camera.LookAt.Z} + + if camera.PerspectiveCamera != nil { + return &drawv1.SceneCamera{ + Position: position, + LookAt: lookAt, + Animated: &camera.Animated, + CameraType: &drawv1.SceneCamera_PerspectiveCamera{}, + } + } + + return &drawv1.SceneCamera{ + Position: position, + LookAt: lookAt, + Animated: &camera.Animated, + CameraType: &drawv1.SceneCamera_OrthographicCamera{}, + } +} + +// SceneMetadata represents the metadata of a scene +type SceneMetadata struct { + SceneCamera SceneCamera + Grid bool + GridCellSize float32 + GridSectionSize float32 + GridFadeDistance float32 + PointSize float32 + PointColor Color + LineWidth float32 + LinePointSize float32 + RenderArmModels drawv1.RenderArmModels + RenderShapes []drawv1.RenderShapes +} + +type sceneMetadataConfig struct { + sceneCamera SceneCamera + grid bool + gridCellSize float32 + gridSectionSize float32 + gridFadeDistance float32 + pointSize float32 + pointColor Color + lineWidth float32 + linePointSize float32 + renderArmModels drawv1.RenderArmModels + renderShapes []drawv1.RenderShapes +} + +func newSceneMetadataConfig() *sceneMetadataConfig { + return &sceneMetadataConfig{ + sceneCamera: DefaultSceneCamera, + grid: DefaultGridEnabled, + gridCellSize: DefaultGridCellSize, + gridSectionSize: DefaultGridSectionSize, + gridFadeDistance: DefaultGridFadeDistance, + pointSize: DefaultPointSize, + pointColor: DefaultPointColor, + lineWidth: DefaultLineWidth, + linePointSize: DefaultPointSize, + renderArmModels: drawv1.RenderArmModels_RENDER_ARM_MODELS_COLLIDERS_AND_MODEL, + renderShapes: []drawv1.RenderShapes{drawv1.RenderShapes_RENDER_SHAPES_ARROWS, drawv1.RenderShapes_RENDER_SHAPES_POINTS, drawv1.RenderShapes_RENDER_SHAPES_LINES, drawv1.RenderShapes_RENDER_SHAPES_MODEL}, + } +} + +type sceneMetadataOption func(*sceneMetadataConfig) + +func WithSceneCamera(sceneCamera SceneCamera) sceneMetadataOption { + return func(config *sceneMetadataConfig) { + config.sceneCamera = sceneCamera + } +} + +func WithGrid(grid bool) sceneMetadataOption { + return func(config *sceneMetadataConfig) { + config.grid = grid + } +} + +func WithGridCellSize(gridCellSize float32) sceneMetadataOption { + return func(config *sceneMetadataConfig) { + config.gridCellSize = gridCellSize + } +} + +func WithGridSectionSize(gridSectionSize float32) sceneMetadataOption { + return func(config *sceneMetadataConfig) { + config.gridSectionSize = gridSectionSize + } +} + +func WithGridFadeDistance(gridFadeDistance float32) sceneMetadataOption { + return func(config *sceneMetadataConfig) { + config.gridFadeDistance = gridFadeDistance + } +} + +func WithScenePointSize(pointSize float32) sceneMetadataOption { + return func(config *sceneMetadataConfig) { + config.pointSize = pointSize + } +} + +func WithScenePointColor(pointColor Color) sceneMetadataOption { + return func(config *sceneMetadataConfig) { + config.pointColor = pointColor + } +} + +func WithSceneLineWidth(lineWidth float32) sceneMetadataOption { + return func(config *sceneMetadataConfig) { + config.lineWidth = lineWidth + } +} + +func WithSceneLinePointSize(linePointSize float32) sceneMetadataOption { + return func(config *sceneMetadataConfig) { + config.linePointSize = linePointSize + } +} + +func WithRenderArmModels(renderArmModels drawv1.RenderArmModels) sceneMetadataOption { + return func(config *sceneMetadataConfig) { + config.renderArmModels = renderArmModels + } +} + +func WithRenderShapes(renderShapes []drawv1.RenderShapes) sceneMetadataOption { + return func(config *sceneMetadataConfig) { + config.renderShapes = renderShapes + } +} + +// NewSceneMetadata creates a new SceneMetadata +func NewSceneMetadata(options ...sceneMetadataOption) SceneMetadata { + config := newSceneMetadataConfig() + for _, option := range options { + option(config) + } + + return SceneMetadata{ + SceneCamera: config.sceneCamera, + Grid: config.grid, + GridCellSize: config.gridCellSize, + GridSectionSize: config.gridSectionSize, + GridFadeDistance: config.gridFadeDistance, + PointSize: config.pointSize, + PointColor: config.pointColor, + LineWidth: config.lineWidth, + LinePointSize: config.linePointSize, + RenderArmModels: config.renderArmModels, + RenderShapes: config.renderShapes, + } +} + +// ToProto converts a SceneMetadata to a protobuf SceneMetadata +func (metadata *SceneMetadata) ToProto() *drawv1.SceneMetadata { + return &drawv1.SceneMetadata{ + SceneCamera: metadata.SceneCamera.ToProto(), + Grid: &metadata.Grid, + GridCellSize: &metadata.GridCellSize, + GridSectionSize: &metadata.GridSectionSize, + GridFadeDistance: &metadata.GridFadeDistance, + PointSize: &metadata.PointSize, + PointColor: packColors([]Color{metadata.PointColor}), + LineWidth: &metadata.LineWidth, + LinePointSize: &metadata.LinePointSize, + RenderArmModels: &metadata.RenderArmModels, + RenderShapes: metadata.RenderShapes, + } +} + +// Validate validates scene metadata +func (metadata *SceneMetadata) Validate() error { + if metadata.SceneCamera.PerspectiveCamera == nil && metadata.SceneCamera.OrthographicCamera == nil { + return fmt.Errorf("scene camera type is nil") + } + + // Validate positive values for size-related fields + if metadata.GridCellSize <= 0 { + return fmt.Errorf("grid cell size must be positive, got %f", metadata.GridCellSize) + } + if metadata.GridSectionSize <= 0 { + return fmt.Errorf("grid section size must be positive, got %f", metadata.GridSectionSize) + } + if metadata.GridFadeDistance < 0 { + return fmt.Errorf("grid fade distance must be non-negative, got %f", metadata.GridFadeDistance) + } + if metadata.PointSize <= 0 { + return fmt.Errorf("point size must be positive, got %f", metadata.PointSize) + } + if metadata.LineWidth <= 0 { + return fmt.Errorf("line width must be positive, got %f", metadata.LineWidth) + } + if metadata.LinePointSize <= 0 { + return fmt.Errorf("line dot size must be positive, got %f", metadata.LinePointSize) + } + + if metadata.RenderArmModels != drawv1.RenderArmModels_RENDER_ARM_MODELS_COLLIDERS_AND_MODEL && + metadata.RenderArmModels != drawv1.RenderArmModels_RENDER_ARM_MODELS_COLLIDERS && + metadata.RenderArmModels != drawv1.RenderArmModels_RENDER_ARM_MODELS_MODEL { + return fmt.Errorf("invalid render arm models value: %s", metadata.RenderArmModels) + } + + for _, shape := range metadata.RenderShapes { + if shape != drawv1.RenderShapes_RENDER_SHAPES_ARROWS && + shape != drawv1.RenderShapes_RENDER_SHAPES_POINTS && + shape != drawv1.RenderShapes_RENDER_SHAPES_LINES && + shape != drawv1.RenderShapes_RENDER_SHAPES_MODEL { + return fmt.Errorf("invalid render shapes value: %s", shape) + } + } + + return nil +} diff --git a/draw/snapshot.go b/draw/snapshot.go new file mode 100644 index 00000000..260be613 --- /dev/null +++ b/draw/snapshot.go @@ -0,0 +1,316 @@ +package draw + +import ( + "bytes" + "compress/gzip" + "fmt" + + "github.com/golang/geo/r3" + drawv1 "github.com/viam-labs/motion-tools/gen/draw/v1" + commonv1 "go.viam.com/api/common/v1" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" + + "github.com/google/uuid" + "google.golang.org/protobuf/encoding/protojson" + "google.golang.org/protobuf/proto" + "google.golang.org/protobuf/types/known/structpb" +) + +// Snapshot represents a snapshot of a world state +type Snapshot struct { + uuid []byte + transforms []*commonv1.Transform + drawings []*Drawing + sceneMetadata SceneMetadata +} + +// UUID returns the UUID of the snapshot +func (snapshot *Snapshot) UUID() []byte { + return snapshot.uuid +} + +// Transforms returns the transforms of the snapshot +func (snapshot *Snapshot) Transforms() []*commonv1.Transform { + return snapshot.transforms +} + +// Drawings returns the drawings of the snapshot +func (snapshot *Snapshot) Drawings() []*Drawing { + return snapshot.drawings +} + +// SceneMetadata returns the scene metadata of the snapshot +func (snapshot *Snapshot) SceneMetadata() SceneMetadata { + return snapshot.sceneMetadata +} + +// ToProto converts the snapshot to a protobuf message +func (snapshot *Snapshot) ToProto() *drawv1.Snapshot { + drawingProtos := make([]*drawv1.Drawing, len(snapshot.drawings)) + for i, drawing := range snapshot.drawings { + drawingProtos[i] = drawing.toProto() + } + + return &drawv1.Snapshot{ + Transforms: snapshot.transforms, + Drawings: drawingProtos, + Uuid: snapshot.uuid, + SceneMetadata: snapshot.sceneMetadata.ToProto(), + } +} + +// MarshalJSON marshals a snapshot to JSON +func (snapshot *Snapshot) MarshalJSON() ([]byte, error) { + marshaler := protojson.MarshalOptions{ + EmitUnpopulated: true, + } + + return marshaler.Marshal(snapshot.ToProto()) +} + +// MarshalBinary marshals a snapshot to binary protobuf format +func (snapshot *Snapshot) MarshalBinary() ([]byte, error) { + return proto.Marshal(snapshot.ToProto()) +} + +// MarshalBinaryGzip marshals a snapshot to gzip-compressed binary protobuf format +func (snapshot *Snapshot) MarshalBinaryGzip() ([]byte, error) { + binaryData, err := snapshot.MarshalBinary() + if err != nil { + return nil, fmt.Errorf("failed to marshal binary: %w", err) + } + + var buf bytes.Buffer + gz := gzip.NewWriter(&buf) + if _, err := gz.Write(binaryData); err != nil { + return nil, fmt.Errorf("failed to write gzip data: %w", err) + } + if err := gz.Close(); err != nil { + return nil, fmt.Errorf("failed to close gzip writer: %w", err) + } + + return buf.Bytes(), nil +} + +// NewSnapshot creates a new snapshot with a unique UUID +func NewSnapshot(sceneOptions ...sceneMetadataOption) *Snapshot { + uuidBytes := uuid.New() + return &Snapshot{ + uuid: uuidBytes[:], + transforms: []*commonv1.Transform{}, + drawings: []*Drawing{}, + sceneMetadata: NewSceneMetadata(sceneOptions...), + } +} + +// Validate validates a snapshot +func (snapshot *Snapshot) Validate() error { + if snapshot == nil { + return fmt.Errorf("snapshot is nil") + } + + if len(snapshot.uuid) == 0 { + return fmt.Errorf("snapshot UUID is empty") + } + + if len(snapshot.uuid) != 16 { + return fmt.Errorf("snapshot UUID must be 16 bytes, got %d", len(snapshot.uuid)) + } + + if snapshot.transforms == nil { + return fmt.Errorf("snapshot transforms is nil") + } + + if snapshot.drawings == nil { + return fmt.Errorf("snapshot drawings is nil") + } + + for i, transform := range snapshot.transforms { + if transform == nil { + return fmt.Errorf("transform at index %d is nil", i) + } + if transform.ReferenceFrame == "" { + return fmt.Errorf("transform at index %d has empty reference frame", i) + } + if transform.PoseInObserverFrame == nil { + return fmt.Errorf("transform at index %d has nil pose in observer frame", i) + } + } + + for i, drawing := range snapshot.drawings { + if drawing == nil { + return fmt.Errorf("drawing at index %d is nil", i) + } + if drawing.Name == "" { + return fmt.Errorf("drawing at index %d has empty name", i) + } + if drawing.Pose == nil { + return fmt.Errorf("drawing at index %d has nil pose in observer frame", i) + } + } + + if err := snapshot.sceneMetadata.Validate(); err != nil { + return fmt.Errorf("invalid scene metadata: %w", err) + } + + return nil +} + +// DrawFrameSystemGeometries draws the geometries of a frame system in the world frame to the snapshot +// - frameSystem is the frame system to draw +// - inputs are the inputs to the frame system +// - colors are the colors to use for the frame system geometries, mapped by frame name +// - Returns an error if the frame system geometries cannot be drawn +func (snapshot *Snapshot) DrawFrameSystemGeometries( + frameSystem *referenceframe.FrameSystem, + inputs referenceframe.FrameSystemInputs, + colors map[string]Color, +) error { + transforms, err := DrawFrameSystemGeometries(frameSystem, inputs, colors) + if err != nil { + return err + } + + snapshot.transforms = append(snapshot.transforms, transforms.Transforms...) + return nil +} + +// DrawFrame draws a frame transform to the snapshot +// - id is the ID of the frame +// - name is the name of the frame +// - parent is the parent of the frame +// - pose is the pose of the frame +// - geometry is the geometry of the frame +// - metadata is visualizer metadata for the frame +// - Returns an error if the frame transform cannot be drawn +func (snapshot *Snapshot) DrawFrame( + id string, + name string, + parent string, + pose spatialmath.Pose, + geometry spatialmath.Geometry, + metadata *structpb.Struct, +) error { + transform, err := NewTransform(id, name, parent, pose, geometry, metadata) + if err != nil { + return err + } + snapshot.transforms = append(snapshot.transforms, transform) + return nil +} + +// DrawGeometry draws a geometry to the snapshot +// - geometry is the geometry to draw +// - pose is the pose of the geometry +// - parent is the parent of the geometry +// - color is the color of the geometry +// - Returns an error if the geometry cannot be drawn +func (snapshot *Snapshot) DrawGeometry( + geometry spatialmath.Geometry, + pose spatialmath.Pose, + parent string, + color Color, +) error { + transform, err := DrawGeometry("", geometry, pose, parent, color) + if err != nil { + return err + } + + snapshot.transforms = append(snapshot.transforms, transform) + return nil +} + +// DrawArrows draws arrows to the snapshot +// - name is the name of the arrows +// - parent is the parent of the arrows +// - pose is the pose of the arrows +// - poses are the poses of the arrows +// - options are the options for the arrows +// - Returns an error if the arrows cannot be drawn +func (snapshot *Snapshot) DrawArrows( + name string, + parent string, + pose spatialmath.Pose, + poses []spatialmath.Pose, + options ...drawArrowsOption, +) error { + arrows, err := NewArrows(poses, options...) + if err != nil { + return err + } + + drawing := arrows.Draw(name, parent, pose) + snapshot.drawings = append(snapshot.drawings, drawing) + return nil +} + +// DrawLine draws a line to the snapshot +// - name is the name of the line +// - parent is the parent of the line +// - pose is the pose of the line +// - points are the points of the line +// - options are the options for the line +// - Returns an error if the line cannot be drawn +func (snapshot *Snapshot) DrawLine( + name string, + parent string, + pose spatialmath.Pose, + points []r3.Vector, + options ...drawLineOption, +) error { + line, err := NewLine(points, options...) + if err != nil { + return err + } + + drawing := line.Draw(name, parent, pose) + snapshot.drawings = append(snapshot.drawings, drawing) + return nil +} + +// DrawModelFromURL draws a model from a URL to the snapshot +// - name is the name of the model +// - parent is the parent of the model +// - pose is the pose of the model +// - options are the options for the model +// - Returns an error if the model cannot be drawn +func (snapshot *Snapshot) DrawModel( + name string, + parent string, + pose spatialmath.Pose, + options ...drawModelOption, +) error { + model, err := NewModel(options...) + if err != nil { + return err + } + + drawing := model.Draw(name, parent, pose) + snapshot.drawings = append(snapshot.drawings, drawing) + return nil +} + +// DrawPoints draws a set of points to the snapshot +// - name is the name of the points +// - parent is the parent of the points +// - pose is the pose of the points +// - positions are the positions of the points +// - options are the options for the points +// - Returns an error if the points cannot be drawn +func (snapshot *Snapshot) DrawPoints( + name string, + parent string, + pose spatialmath.Pose, + positions []r3.Vector, + options ...drawPointsOption, +) error { + points, err := NewPoints(positions, options...) + if err != nil { + return err + } + + drawing := points.Draw(name, parent, pose) + snapshot.drawings = append(snapshot.drawings, drawing) + return nil +} diff --git a/draw/snapshot_test.go b/draw/snapshot_test.go new file mode 100644 index 00000000..dfda2bfa --- /dev/null +++ b/draw/snapshot_test.go @@ -0,0 +1,1272 @@ +package draw + +import ( + "encoding/json" + "fmt" + "math" + "math/rand" + "os" + "path/filepath" + "strings" + "testing" + "time" + + "github.com/golang/geo/r3" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) + +const snapshotDir = "__snapshots__" +const fixturesDir = "__fixtures__" + +func init() { + rand.Seed(time.Now().UnixNano()) +} + +// TestSnapshotBox generates a snapshot showcasing a voxel cabin built with boxes using a frame system +func TestSnapshotBox(t *testing.T) { + snapshot := NewSnapshot( + WithSceneCamera( + NewSceneCamera( + r3.Vector{X: 4000, Y: -4000, Z: 3000}, + r3.Vector{X: 0, Y: 0, Z: 1000}, + ), + ), + ) + + voxelScale := 500.0 + voxelSize := r3.Vector{X: voxelScale, Y: voxelScale, Z: voxelScale} + + woodColor := NewColor(WithName("saddlebrown")) // wood + roofColor := NewColor(WithName("maroon")) // roof + windowColor := NewColor(WithName("blue")).SetAlpha(uint8(128)) // window + doorColor := NewColor(WithName("sienna")) // door + chimneyColor := NewColor(WithName("darkgray")) // chimney + benchColor := NewColor(WithName("lightgray")) // bench + chestColor := NewColor(WithName("darkgoldenrod")) // chest + furnitureColor := NewColor(WithName("peru")) // furniture + + // Create frame system and house root frame + fs := referenceframe.NewEmptyFrameSystem("world") + houseFrame, _ := referenceframe.NewStaticFrame("house", spatialmath.NewZeroPose()) + fs.AddFrame(houseFrame, fs.World()) + + // Helper to add a voxel frame + // Per RDK's FrameSystemGeometries design: the geometry's pose defines its position + // relative to the parent frame. The frame itself is at zero pose. + addVoxelFrame := func(parent referenceframe.Frame, name string, x, y, z int) { + // Geometry pose relative to parent frame (this is what FrameSystemGeometries uses) + geomPose := spatialmath.NewPoseFromPoint(r3.Vector{ + X: float64(x) * voxelScale, + Y: float64(y) * voxelScale, + Z: float64(z) * voxelScale, + }) + voxel, err := spatialmath.NewBox( + geomPose, + voxelSize, + name, + ) + if err != nil { + t.Fatal(err) + } + + // Frame at zero pose - geometry carries the offset + frame, err := referenceframe.NewStaticFrameWithGeometry(name, spatialmath.NewZeroPose(), voxel) + if err != nil { + t.Fatal(err) + } + + fs.AddFrame(frame, parent) + } + + // Create floor parent frame (origin at the corner of the floor) + floorOrigin := spatialmath.NewPoseFromPoint(r3.Vector{X: -4 * voxelScale, Y: -3 * voxelScale, Z: 0}) + floorParent, _ := referenceframe.NewStaticFrame("floor", floorOrigin) + fs.AddFrame(floorParent, houseFrame) + + // Floor voxels (positioned relative to floor origin) + for x := range 8 { + for y := range 6 { + addVoxelFrame(floorParent, fmt.Sprintf("floor_voxel_%d_%d", x, y), x, y, 0) + } + } + + // Create walls parent frame (origin at corner of the walls at ground level) + wallsOrigin := spatialmath.NewPoseFromPoint(r3.Vector{X: -4 * voxelScale, Y: -3 * voxelScale, Z: 1 * voxelScale}) + wallsParent, _ := referenceframe.NewStaticFrame("walls", wallsOrigin) + fs.AddFrame(wallsParent, houseFrame) + + // Front wall (Y = 0 relative to walls origin) with door opening + for x := 0; x < 8; x++ { + for z := 0; z < 4; z++ { + // Skip door opening (originally at x=-1 to 0, z=1-2 in world, now x=3-4, z=0-1 in walls frame) + if !(z <= 1 && x >= 3 && x <= 4) { + addVoxelFrame(wallsParent, fmt.Sprintf("front_wall_%d_%d", x, z), x, 0, z) + } + } + } + + // Back wall (Y = 5 relative to walls origin) - skip window positions + for x := 0; x < 8; x++ { + for z := 0; z < 4; z++ { + // Skip window positions (originally at x=-1 to 0, z=2 in world, now x=3-4, z=1 in walls frame) + if !(z == 1 && x >= 3 && x <= 4) { + addVoxelFrame(wallsParent, fmt.Sprintf("back_wall_%d_%d", x, z), x, 5, z) + } + } + } + + // Left wall (X = 0 relative to walls origin) - skip window position + for y := 0; y < 6; y++ { + for z := 0; z < 4; z++ { + // Skip window position (originally at y=0, z=2 in world, now y=3, z=1 in walls frame) + if !(z == 1 && y == 3) { + addVoxelFrame(wallsParent, fmt.Sprintf("left_wall_%d_%d", y, z), 0, y, z) + } + } + } + + // Right wall (X = 7 relative to walls origin) + for y := 0; y < 6; y++ { + for z := 0; z < 4; z++ { + addVoxelFrame(wallsParent, fmt.Sprintf("right_wall_%d_%d", y, z), 7, y, z) + } + } + + // Create windows parent frame (origin at first window position) + windowsOrigin := spatialmath.NewPoseFromPoint(r3.Vector{X: -1 * 500, Y: 2 * 500, Z: 2 * 500}) + windowsParent, _ := referenceframe.NewStaticFrame("windows", windowsOrigin) + fs.AddFrame(windowsParent, houseFrame) + + // Back wall windows (relative to windows origin) + addVoxelFrame(windowsParent, "back_window_1", 0, 0, 0) + addVoxelFrame(windowsParent, "back_window_2", 1, 0, 0) + + // Left wall window (relative to windows origin) + addVoxelFrame(windowsParent, "left_window", -3, -2, 0) + + // Create roof parent frame (origin at corner of the roof base) + roofOrigin := spatialmath.NewPoseFromPoint(r3.Vector{X: -5 * 500, Y: -4 * 500, Z: 5 * 500}) + roofParent, _ := referenceframe.NewStaticFrame("roof", roofOrigin) + fs.AddFrame(roofParent, houseFrame) + + // Roof layer 1 (base layer, relative to roof origin) + for x := 0; x < 10; x++ { + for y := 0; y < 8; y++ { + addVoxelFrame(roofParent, fmt.Sprintf("roof_1_%d_%d", x, y), x, y, 0) + } + } + // Roof layer 2 + for x := 1; x < 9; x++ { + for y := 1; y < 7; y++ { + addVoxelFrame(roofParent, fmt.Sprintf("roof_2_%d_%d", x, y), x, y, 1) + } + } + // Roof layer 3 + for x := 2; x < 8; x++ { + for y := 2; y < 6; y++ { + addVoxelFrame(roofParent, fmt.Sprintf("roof_3_%d_%d", x, y), x, y, 2) + } + } + // Roof peak + for x := 3; x < 7; x++ { + for y := 3; y < 5; y++ { + addVoxelFrame(roofParent, fmt.Sprintf("roof_peak_%d_%d", x, y), x, y, 3) + } + } + + // Create door parent frame (origin at door position) + doorOrigin := spatialmath.NewPoseFromPoint(r3.Vector{X: 1 * 500, Y: -5 * 500, Z: 1 * 500}) + doorParent, _ := referenceframe.NewStaticFrame("door", doorOrigin) + fs.AddFrame(doorParent, houseFrame) + + // Door voxels (1x2x2 grid, relative to door origin) + addVoxelFrame(doorParent, "door_1", 0, 0, 0) + addVoxelFrame(doorParent, "door_2", 0, 1, 0) + addVoxelFrame(doorParent, "door_3", 0, 0, 1) + addVoxelFrame(doorParent, "door_4", 0, 1, 1) + + // Create chimney parent frame (origin at chimney base) + chimneyOrigin := spatialmath.NewPoseFromPoint(r3.Vector{X: 2 * 500, Y: 1 * 500, Z: 6 * 500}) + chimneyParent, _ := referenceframe.NewStaticFrame("chimney", chimneyOrigin) + fs.AddFrame(chimneyParent, houseFrame) + + // Chimney voxels (vertical stack, relative to chimney origin) + addVoxelFrame(chimneyParent, "chimney_1", 0, 0, 0) + addVoxelFrame(chimneyParent, "chimney_2", 0, 0, 1) + addVoxelFrame(chimneyParent, "chimney_3", 0, 0, 2) + addVoxelFrame(chimneyParent, "chimney_4", 0, 0, 3) + + // Create table parent frame (origin at table position) + tableOrigin := spatialmath.NewPoseFromPoint(r3.Vector{X: -1 * 500, Y: 0, Z: 1 * 500}) + tableParent, _ := referenceframe.NewStaticFrame("table", tableOrigin) + fs.AddFrame(tableParent, houseFrame) + + // Table voxels (2 blocks side by side, relative to table origin) + addVoxelFrame(tableParent, "table_1", 0, 0, 0) + addVoxelFrame(tableParent, "table_2", 1, 0, 0) + + // Create bench parent frame (origin at bench position) + benchOrigin := spatialmath.NewPoseFromPoint(r3.Vector{X: -3 * 500, Y: -2 * 500, Z: 1 * 500}) + benchParent, _ := referenceframe.NewStaticFrame("bench", benchOrigin) + fs.AddFrame(benchParent, houseFrame) + + // Bench voxels (2 blocks stacked vertically, relative to bench origin) + addVoxelFrame(benchParent, "bench_1", 0, 0, 0) + addVoxelFrame(benchParent, "bench_2", 0, 0, 1) + + // Chest frame (single geometry at origin) + chestOrigin := spatialmath.NewPoseFromPoint(r3.Vector{X: 2 * 500, Y: 1 * 500, Z: 1 * 500}) + chestVoxel, _ := spatialmath.NewBox( + spatialmath.NewZeroPose(), + voxelSize, + "chest", + ) + chestFrame, _ := referenceframe.NewStaticFrameWithGeometry("chest", chestOrigin, chestVoxel) + fs.AddFrame(chestFrame, houseFrame) + + // Create color map for the frame system + // Include "house" so the entire frame hierarchy is rendered + frameColors := map[string]Color{ + "house": woodColor, // Root frame needs a color for hierarchy + "floor": woodColor, + "walls": woodColor, + "windows": windowColor, + "roof": roofColor, + "door": doorColor, + "chimney": chimneyColor, + "table": furnitureColor, + "bench": benchColor, + "chest": chestColor, + } + + // Draw the frame system + inputs := referenceframe.NewZeroInputs(fs) + err := snapshot.DrawFrameSystemGeometries(fs, inputs, frameColors) + if err != nil { + t.Fatal(err) + } + + writeSnapshot(t, snapshot, "snapshot_box.json") +} + +func createBody( + t *testing.T, + snapshot *Snapshot, + name string, + parent string, + orbitRadius float64, + orbitAngle float64, + bodyRadius float64, + color Color, +) string { + t.Helper() + + angleRad := orbitAngle * math.Pi / 180.0 + x := orbitRadius * math.Cos(angleRad) + y := orbitRadius * math.Sin(angleRad) + + orbitFrameName := name + "-orbit" + err := snapshot.DrawFrame("", orbitFrameName, parent, spatialmath.NewPoseFromPoint(r3.Vector{X: x, Y: y, Z: 0}), nil, nil) + if err != nil { + t.Fatal(err) + } + + geometry, err := spatialmath.NewSphere(spatialmath.NewZeroPose(), bodyRadius, name) + if err != nil { + t.Fatal(err) + } + + err = snapshot.DrawGeometry(geometry, spatialmath.NewZeroPose(), orbitFrameName, color) + if err != nil { + t.Fatal(err) + } + + return orbitFrameName +} + +func createRings(t *testing.T, snapshot *Snapshot, parent string, radius float64, color Color) { + t.Helper() + + diameter := radius * 2.0 + box, err := spatialmath.NewBox( + spatialmath.NewZeroPose(), + r3.Vector{X: diameter, Y: diameter, Z: 10}, + parent+"-rings", + ) + if err != nil { + t.Fatal(err) + } + + pose := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 0}) + err = snapshot.DrawGeometry(box, pose, parent, color) + if err != nil { + t.Fatal(err) + } +} + +// TestSnapshotSphere generates a snapshot showcasing a complete solar system with all planets and moons +func TestSnapshotSphere(t *testing.T) { + snapshot := NewSnapshot( + WithSceneCamera( + NewSceneCamera( + r3.Vector{X: 40000, Y: 40000, Z: 30000}, + r3.Vector{X: 0, Y: 0, Z: 0}, + ), + ), + ) + + sunOrbitFrame := "sun-orbit" + err := snapshot.DrawFrame("", sunOrbitFrame, "world", spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 2000}), nil, nil) + if err != nil { + t.Fatal(err) + } + + sunGeometry, err := spatialmath.NewSphere(spatialmath.NewZeroPose(), 2000, "sun") + if err != nil { + t.Fatal(err) + } + + err = snapshot.DrawGeometry(sunGeometry, spatialmath.NewZeroPose(), sunOrbitFrame, NewColor(WithName("orange"))) + if err != nil { + t.Fatal(err) + } + + createBody(t, snapshot, "mercury", sunOrbitFrame, 4000, 10, 350, NewColor(WithName("dimgray"))) + createBody(t, snapshot, "venus", sunOrbitFrame, 6000, 50, 700, NewColor(WithName("wheat"))) + + earthOrbit := createBody(t, snapshot, "earth", sunOrbitFrame, 8000, 95, 750, NewColor(WithName("dodgerblue"))) + createBody(t, snapshot, "moon", earthOrbit, 1400, 135, 200, NewColor(WithName("silver"))) + + marsOrbit := createBody(t, snapshot, "mars", sunOrbitFrame, 10000, 175, 500, NewColor(WithName("orangered"))) + createBody(t, snapshot, "phobos", marsOrbit, 900, 45, 75, NewColor(WithName("dimgray"))) + createBody(t, snapshot, "deimos", marsOrbit, 1200, 225, 60, NewColor(WithName("darkgray"))) + + jupiterOrbit := createBody(t, snapshot, "jupiter", sunOrbitFrame, 14000, 250, 1500, NewColor(WithName("tan"))) + createBody(t, snapshot, "io", jupiterOrbit, 2100, 0, 200, NewColor(WithName("orange"))) + createBody(t, snapshot, "europa", jupiterOrbit, 2400, 90, 175, NewColor(WithName("azure"))) + createBody(t, snapshot, "ganymede", jupiterOrbit, 2700, 180, 250, NewColor(WithName("slategray"))) + createBody(t, snapshot, "callisto", jupiterOrbit, 3000, 270, 225, NewColor(WithName("darkslategray"))) + + saturnOrbit := createBody(t, snapshot, "saturn", sunOrbitFrame, 18000, 315, 1300, NewColor(WithName("palegoldenrod"))) + createRings(t, snapshot, saturnOrbit, 1600, NewColor(WithName("palegoldenrod")).SetAlpha(uint8(128))) + createBody(t, snapshot, "rhea", saturnOrbit, 2100, 110, 125, NewColor(WithName("gainsboro"))) + createBody(t, snapshot, "enceladus", saturnOrbit, 1850, 200, 100, NewColor(WithName("snow"))) + createBody(t, snapshot, "mimas", saturnOrbit, 1700, 290, 90, NewColor(WithName("silver"))) + + uranusOrbit := createBody(t, snapshot, "uranus", sunOrbitFrame, 22000, 25, 900, NewColor(WithName("paleturquoise"))) + createRings(t, snapshot, uranusOrbit, 1100, NewColor(WithName("paleturquoise")).SetAlpha(uint8(128))) + createBody(t, snapshot, "titania", uranusOrbit, 1900, 0, 150, NewColor(WithName("lightslategray"))) + createBody(t, snapshot, "oberon", uranusOrbit, 2100, 90, 140, NewColor(WithName("gray"))) + createBody(t, snapshot, "umbriel", uranusOrbit, 1700, 180, 125, NewColor(WithName("dimgray"))) + createBody(t, snapshot, "ariel", uranusOrbit, 1550, 270, 130, NewColor(WithName("darkgray"))) + + neptuneOrbit := createBody(t, snapshot, "neptune", sunOrbitFrame, 26000, 110, 850, NewColor(WithName("royalblue"))) + createRings(t, snapshot, neptuneOrbit, 950, NewColor(WithName("royalblue")).SetAlpha(uint8(128))) + createBody(t, snapshot, "triton", neptuneOrbit, 1800, 200, 200, NewColor(WithName("lavender"))) + + plutoOrbit := createBody(t, snapshot, "pluto", sunOrbitFrame, 30000, 195, 225, NewColor(WithName("tan"))) + createBody(t, snapshot, "charon", plutoOrbit, 750, 90, 125, NewColor(WithName("gray"))) + + createBody(t, snapshot, "ceres", sunOrbitFrame, 11500, 140, 175, NewColor(WithName("rosybrown"))) + + writeSnapshot(t, snapshot, "snapshot_sphere.json") +} + +// TestSnapshotCapsule generates a snapshot of a city simulation with plots, buildings, and citizens +func TestSnapshotCapsule(t *testing.T) { + snapshot := NewSnapshot( + WithSceneCamera( + NewSceneCamera( + r3.Vector{X: 15000, Y: -15000, Z: 12000}, + r3.Vector{X: 0, Y: 0, Z: 2000}, + ), + ), + ) + + roadColor := NewColor(WithName("darkgray")) + grassColor := NewColor(WithName("green")) + concreteColor := NewColor(WithName("lightgray")) + buildingColors := []Color{ + NewColor(WithName("beige")), + NewColor(WithName("lightblue")), + NewColor(WithName("lavender")), + NewColor(WithName("lightyellow")), + } + citizenColor := NewColor(WithName("purple")) + + plotSize := 3000.0 // Each plot is 3m x 3m (scaled up) + plotThickness := 200.0 // Thick enough to cover axes helper gizmo + gridSize := 8 // 8x8 grid + + // Create frame system + fs := referenceframe.NewEmptyFrameSystem("world") + rootFrame, _ := referenceframe.NewStaticFrame("root", spatialmath.NewZeroPose()) + fs.AddFrame(rootFrame, fs.World()) + + // Create color map + frameColors := map[string]Color{ + "root": grassColor, // Default color + "people": citizenColor, + } + + // Create city plots in a grid + for i := 0; i < gridSize; i++ { + for j := 0; j < gridSize; j++ { + x := float64(i-gridSize/2) * plotSize + y := float64(j-gridSize/2) * plotSize + + plotName := fmt.Sprintf("plot_%d_%d", i, j) + + // Determine plot type (roads, grass, concrete) + var plotColor Color + isRoad := (i == gridSize/2 || j == gridSize/2) // Cross roads through center + isConcrete := !isRoad && (i%2 == 0 && j%2 == 0) // Some plots are concrete + + if isRoad { + plotColor = roadColor + } else if isConcrete { + plotColor = concreteColor + } else { + plotColor = grassColor + } + + // Per RDK's FrameSystemGeometries: geometry pose defines position relative to parent + plotGeomPose := spatialmath.NewPoseFromPoint(r3.Vector{X: x, Y: y, Z: plotThickness / 2.0}) + plot, _ := spatialmath.NewBox( + plotGeomPose, + r3.Vector{X: plotSize, Y: plotSize, Z: plotThickness}, + plotName, + ) + plotFrame, _ := referenceframe.NewStaticFrameWithGeometry(plotName, spatialmath.NewZeroPose(), plot) + fs.AddFrame(plotFrame, rootFrame) + frameColors[plotName] = plotColor + + // Add buildings on some non-road plots + if !isRoad && rand.Float64() < 0.4 { // 40% chance of building + buildingHeight := 2000.0 + rand.Float64()*6000.0 // Random height 2-8m + buildingWidth := 2000.0 + rand.Float64()*800.0 // Random width 2-2.8m + buildingDepth := 2000.0 + rand.Float64()*800.0 // Random depth 2-2.8m + buildingColor := buildingColors[rand.Intn(len(buildingColors))] + buildingName := plotName + "_building" + + // Building geometry position relative to root (since plot frame is at root) + // Building sits on top of the plot + buildingGeomPose := spatialmath.NewPoseFromPoint(r3.Vector{X: x, Y: y, Z: plotThickness + buildingHeight/2.0}) + building, _ := spatialmath.NewBox( + buildingGeomPose, + r3.Vector{X: buildingWidth, Y: buildingDepth, Z: buildingHeight}, + buildingName, + ) + buildingFrame, _ := referenceframe.NewStaticFrameWithGeometry(buildingName, spatialmath.NewZeroPose(), building) + fs.AddFrame(buildingFrame, rootFrame) + frameColors[buildingName] = buildingColor + } + } + } + + // Create people parent frame + peopleFrame, _ := referenceframe.NewStaticFrame("people", spatialmath.NewZeroPose()) + fs.AddFrame(peopleFrame, rootFrame) + + numCitizens := 150 // Many citizens walking around + citizenHeight := 200.0 + cityBounds := (float64(gridSize)/2.0 - 0.5) * plotSize + + for i := 0; i < numCitizens; i++ { + cx := (rand.Float64()*2.0 - 1.0) * cityBounds + cy := (rand.Float64()*2.0 - 1.0) * cityBounds + cz := plotThickness + citizenHeight/2.0 // Standing on top of ground plots + + rotation := rand.Float64() * 360.0 + personName := fmt.Sprintf("person_%d", i) + + // Per RDK's FrameSystemGeometries: geometry pose defines position relative to parent + personGeomPose := spatialmath.NewPose( + r3.Vector{X: cx, Y: cy, Z: cz}, + &spatialmath.OrientationVectorDegrees{OX: 0, OY: 0, OZ: 1, Theta: rotation}, + ) + citizen, _ := spatialmath.NewCapsule( + personGeomPose, + 50.0, // Radius (body width) - 100mm diameter + citizenHeight, // Height (person height) - small but visible + personName, + ) + personFrame, _ := referenceframe.NewStaticFrameWithGeometry(personName, spatialmath.NewZeroPose(), citizen) + fs.AddFrame(personFrame, peopleFrame) + frameColors[personName] = citizenColor + } + + // Draw the frame system + inputs := referenceframe.FrameSystemInputs{} + err := snapshot.DrawFrameSystemGeometries(fs, inputs, frameColors) + if err != nil { + t.Fatal(err) + } + + writeSnapshot(t, snapshot, "snapshot_capsule.json") +} + +// TestSnapshotArrows generates a snapshot showcasing arrow drawings +// Creates three geometries (box, sphere, capsule) with arrows pointing to their surfaces +func TestSnapshotArrows(t *testing.T) { + snapshot := NewSnapshot( + WithSceneCamera( + NewSceneCamera( + r3.Vector{X: 2000, Y: 2000, Z: 1500}, + r3.Vector{X: 0, Y: 0, Z: 0}, + WithAnimated(false), + ), + ), + ) + + // 1. Create a box geometry at origin + boxPose := spatialmath.NewPoseFromPoint(r3.Vector{X: -800, Y: 0, Z: 300}) + box, err := spatialmath.NewBox( + spatialmath.NewZeroPose(), + r3.Vector{X: 400, Y: 400, Z: 400}, + "test-box", + ) + if err != nil { + t.Fatal(err) + } + err = snapshot.DrawGeometry(box, boxPose, "world", NewColor(WithName("red"))) + if err != nil { + t.Fatal(err) + } + + // Create thousands of arrows pointing to box faces + var boxArrowPoses []spatialmath.Pose + boxHalfSize := 200.0 // half of 400mm + numArrowsPerFace := 500 // 500 arrows per face = 3000 total arrows + faces := []struct { + normal r3.Vector + offset r3.Vector + }{ + {r3.Vector{X: 0, Y: 0, Z: -1}, r3.Vector{X: 0, Y: 0, Z: boxHalfSize + 150}}, // Top + {r3.Vector{X: 0, Y: 0, Z: 1}, r3.Vector{X: 0, Y: 0, Z: -boxHalfSize - 150}}, // Bottom + {r3.Vector{X: 0, Y: -1, Z: 0}, r3.Vector{X: 0, Y: boxHalfSize + 150, Z: 0}}, // Front (+Y) + {r3.Vector{X: 0, Y: 1, Z: 0}, r3.Vector{X: 0, Y: -boxHalfSize - 150, Z: 0}}, // Back (-Y) + {r3.Vector{X: -1, Y: 0, Z: 0}, r3.Vector{X: boxHalfSize + 150, Y: 0, Z: 0}}, // Right (+X) + {r3.Vector{X: 1, Y: 0, Z: 0}, r3.Vector{X: -boxHalfSize - 150, Y: 0, Z: 0}}, // Left (-X) + } + + for _, face := range faces { + gridSize := int(math.Sqrt(float64(numArrowsPerFace))) + spacing := (boxHalfSize * 2) / float64(gridSize+1) + + for i := 0; i < gridSize; i++ { + for j := 0; j < gridSize; j++ { + var pos r3.Vector + if face.normal.Z != 0 { + pos = r3.Vector{ + X: -boxHalfSize + float64(i+1)*spacing, + Y: -boxHalfSize + float64(j+1)*spacing, + Z: face.offset.Z, + } + } else if face.normal.Y != 0 { + pos = r3.Vector{ + X: -boxHalfSize + float64(i+1)*spacing, + Y: face.offset.Y, + Z: -boxHalfSize + float64(j+1)*spacing, + } + } else { + pos = r3.Vector{ + X: face.offset.X, + Y: -boxHalfSize + float64(i+1)*spacing, + Z: -boxHalfSize + float64(j+1)*spacing, + } + } + + boxArrowPoses = append(boxArrowPoses, spatialmath.NewPose( + boxPose.Point().Add(pos), + &spatialmath.OrientationVectorDegrees{OX: face.normal.X, OY: face.normal.Y, OZ: face.normal.Z}, + )) + } + } + } + + err = snapshot.DrawArrows( + "box-surface-arrows", + "world", + spatialmath.NewZeroPose(), + boxArrowPoses, + WithArrowsColors(NewColor(WithName("red"))), + ) + if err != nil { + t.Fatal(err) + } + + // 2. Create a sphere geometry + spherePose := spatialmath.NewPoseFromPoint(r3.Vector{X: 800, Y: 0, Z: 300}) + sphere, err := spatialmath.NewSphere( + spatialmath.NewZeroPose(), + 250, + "test-sphere", + ) + if err != nil { + t.Fatal(err) + } + err = snapshot.DrawGeometry(sphere, spherePose, "world", NewColor(WithName("green"))) + if err != nil { + t.Fatal(err) + } + + // Create thousands of arrows pointing radially inward to sphere surface + var sphereArrowPoses []spatialmath.Pose + sphereCenter := r3.Vector{X: 800, Y: 0, Z: 300} + sphereRadius := 250.0 + numSphereArrows := 2000 // 2000 arrows around the sphere + + for i := 0; i < numSphereArrows; i++ { + theta := 2 * math.Pi * float64(i) / ((1 + math.Sqrt(5)) / 2) // Golden angle + y := 1 - (2*float64(i)+1)/float64(numSphereArrows) // Distribute from -1 to 1 + radiusAtY := math.Sqrt(1 - y*y) + + x := radiusAtY * math.Cos(theta) + z := radiusAtY * math.Sin(theta) + + arrowDistance := sphereRadius + 150 + arrowPos := r3.Vector{ + X: sphereCenter.X + x*arrowDistance, + Y: sphereCenter.Y + y*arrowDistance, + Z: sphereCenter.Z + z*arrowDistance, + } + + direction := r3.Vector{ + X: sphereCenter.X - arrowPos.X, + Y: sphereCenter.Y - arrowPos.Y, + Z: sphereCenter.Z - arrowPos.Z, + }.Normalize() + + sphereArrowPoses = append(sphereArrowPoses, spatialmath.NewPose( + arrowPos, + &spatialmath.OrientationVectorDegrees{OX: direction.X, OY: direction.Y, OZ: direction.Z}, + )) + } + + err = snapshot.DrawArrows( + "sphere-surface-arrows", + "world", + spatialmath.NewZeroPose(), + sphereArrowPoses, + WithArrowsColors(NewColor(WithName("green"))), + ) + if err != nil { + t.Fatal(err) + } + + // 3. Create a capsule geometry + capsulePose := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 800}) + capsule, err := spatialmath.NewCapsule( + spatialmath.NewZeroPose(), + 150, + 600, + "test-capsule", + ) + if err != nil { + t.Fatal(err) + } + err = snapshot.DrawGeometry(capsule, capsulePose, "world", NewColor(WithName("blue"))) + if err != nil { + t.Fatal(err) + } + + // Create thousands of arrows pointing to capsule surface + var capsuleArrowPoses []spatialmath.Pose + capsuleCenter := r3.Vector{X: 0, Y: 0, Z: 800} + capsuleRadius := 150.0 + capsuleLength := 600.0 + numCapsuleArrows := 3000 // 3000 arrows around the capsule + + numRings := 50 + arrowsPerRing := numCapsuleArrows / numRings + + for ring := 0; ring < numRings; ring++ { + zOffset := -capsuleLength/2 + float64(ring)*(capsuleLength/float64(numRings-1)) + + for i := 0; i < arrowsPerRing; i++ { + angle := float64(i) * 2 * math.Pi / float64(arrowsPerRing) + + var radialDistance float64 + if math.Abs(zOffset) > capsuleLength/2-capsuleRadius { + capZ := math.Abs(zOffset) - (capsuleLength/2 - capsuleRadius) + radialDistance = math.Sqrt(capsuleRadius*capsuleRadius - capZ*capZ) + } else { + radialDistance = capsuleRadius + } + + arrowDistance := radialDistance + 150 + x := capsuleCenter.X + arrowDistance*math.Cos(angle) + y := capsuleCenter.Y + arrowDistance*math.Sin(angle) + z := capsuleCenter.Z + zOffset + + direction := r3.Vector{ + X: capsuleCenter.X - x, + Y: capsuleCenter.Y - y, + Z: 0, + }.Normalize() + + capsuleArrowPoses = append(capsuleArrowPoses, spatialmath.NewPose( + r3.Vector{X: x, Y: y, Z: z}, + &spatialmath.OrientationVectorDegrees{OX: direction.X, OY: direction.Y, OZ: direction.Z}, + )) + } + } + + err = snapshot.DrawArrows( + "capsule-surface-arrows", + "world", + spatialmath.NewZeroPose(), + capsuleArrowPoses, + WithArrowsColors(NewColor(WithName("blue"))), + ) + if err != nil { + t.Fatal(err) + } + + writeSnapshot(t, snapshot, "snapshot_arrows.json") +} + +// TestSnapshotLine generates a snapshot showcasing lines navigating around obstacles +// Creates boxes and spheres scattered around with various paths weaving through them +func TestSnapshotLine(t *testing.T) { + snapshot := NewSnapshot( + WithSceneCamera( + NewSceneCamera( + r3.Vector{X: 8000, Y: 8000, Z: 6000}, + r3.Vector{X: 0, Y: 0, Z: 1000}, + ), + ), + ) + + obstacleColors := []Color{ + NewColor(WithName("red")), + NewColor(WithName("green")), + NewColor(WithName("blue")), + NewColor(WithName("yellow")), + NewColor(WithName("magenta")), + NewColor(WithName("cyan")), + } + + numObstacles := 15 + for i := range numObstacles { + x := (rand.Float64()*2.0 - 1.0) * 3000.0 + y := (rand.Float64()*2.0 - 1.0) * 3000.0 + z := rand.Float64()*1500.0 + 500.0 + + size := 300.0 + rand.Float64()*400.0 + color := obstacleColors[rand.Intn(len(obstacleColors))] + + if rand.Float64() < 0.5 { + box, err := spatialmath.NewBox( + spatialmath.NewZeroPose(), + r3.Vector{X: size, Y: size, Z: size}, + fmt.Sprintf("obstacle-box-%d", i), + ) + if err != nil { + t.Fatal(err) + } + + err = snapshot.DrawGeometry(box, spatialmath.NewPoseFromPoint(r3.Vector{X: x, Y: y, Z: z}), "world", color) + if err != nil { + t.Fatal(err) + } + } else { + sphere, err := spatialmath.NewSphere(spatialmath.NewZeroPose(), size/2.0, fmt.Sprintf("obstacle-sphere-%d", i)) + if err != nil { + t.Fatal(err) + } + err = snapshot.DrawGeometry(sphere, spatialmath.NewPoseFromPoint(r3.Vector{X: x, Y: y, Z: z}), "world", color) + if err != nil { + t.Fatal(err) + } + } + } + + lineColors := []Color{ + NewColor(WithName("red")), + NewColor(WithName("green")), + NewColor(WithName("orange")), + NewColor(WithName("magenta")), + } + + // Path 1: Smooth winding path with many points + var path1 []r3.Vector + numPoints1 := 100 + for i := 0; i < numPoints1; i++ { + t := float64(i) / float64(numPoints1-1) + x := (t*2.0 - 1.0) * 4000.0 + y := math.Sin(t*math.Pi*4) * 2000.0 + z := 500.0 + math.Sin(t*math.Pi*2)*500.0 + path1 = append(path1, r3.Vector{X: x, Y: y, Z: z}) + } + err := snapshot.DrawLine( + "smooth-path", + "world", + spatialmath.NewZeroPose(), + path1, + ) + if err != nil { + t.Fatal(err) + } + + // Path 2: Zigzag path with fewer points (more angular) + var path2 []r3.Vector + numPoints2 := 15 + for i := range numPoints2 { + t := float64(i) / float64(numPoints2-1) + x := math.Cos(t*math.Pi*2) * 3000.0 + y := math.Sin(t*math.Pi*2) * 3000.0 + z := 1000.0 + float64(i%2)*800.0 + path2 = append(path2, r3.Vector{X: x, Y: y, Z: z}) + } + err = snapshot.DrawLine( + "zigzag-path", + "world", + spatialmath.NewZeroPose(), + path2, + WithLineWidth(20.0), + WithPointSize(10.0), + WithLineColors(lineColors[0], nil), + ) + if err != nil { + t.Fatal(err) + } + + // Path 3: Spiral path with medium smoothness + var path3 []r3.Vector + numPoints3 := 50 + for i := range numPoints3 { + t := float64(i) / float64(numPoints3-1) + angle := t * math.Pi * 6 + radius := 3500.0 * (1.0 - t*0.7) + x := math.Cos(angle) * radius + y := math.Sin(angle) * radius + z := 200.0 + t*2000.0 + path3 = append(path3, r3.Vector{X: x, Y: y, Z: z}) + } + err = snapshot.DrawLine( + "spiral-path", + "world", + spatialmath.NewZeroPose(), + path3, + WithLineWidth(12.0), + WithPointSize(10.0), + WithLineColors(lineColors[1], nil), + ) + if err != nil { + t.Fatal(err) + } + + // Path 4: Straight diagonal path with very few points + var path4 []r3.Vector + path4 = append(path4, r3.Vector{X: -3500, Y: -3500, Z: 300}) + path4 = append(path4, r3.Vector{X: -1000, Y: 0, Z: 800}) + path4 = append(path4, r3.Vector{X: 1000, Y: 1500, Z: 1200}) + path4 = append(path4, r3.Vector{X: 3500, Y: 3500, Z: 500}) + err = snapshot.DrawLine( + "straight-path", + "world", + spatialmath.NewZeroPose(), + path4, + WithLineWidth(25.0), + WithPointSize(10.0), + WithLineColors(lineColors[2], nil), + ) + if err != nil { + t.Fatal(err) + } + + // Path 5: Helix path spiraling upward + var path5 []r3.Vector + numPoints5 := 80 + radius := 2000.0 + height := 3000.0 + numTurns := 3.0 + for i := range numPoints5 { + t := float64(i) / float64(numPoints5-1) + angle := t * math.Pi * 2 * numTurns + x := math.Cos(angle) * radius + y := math.Sin(angle) * radius + z := 500.0 + t*height + path5 = append(path5, r3.Vector{X: x, Y: y, Z: z}) + } + err = snapshot.DrawLine( + "helix-path", + "world", + spatialmath.NewZeroPose(), + path5, + WithLineWidth(18.0), + WithPointSize(10.0), + WithLineColors(lineColors[3], nil), + ) + if err != nil { + t.Fatal(err) + } + + writeSnapshot(t, snapshot, "snapshot_line.json") +} + +// TestSnapshotPoints generates a snapshot showcasing points shapes +func TestSnapshotPoints(t *testing.T) { + snapshot := NewSnapshot( + WithSceneCamera( + NewSceneCamera( + r3.Vector{X: 8000, Y: 8000, Z: 6000}, + r3.Vector{X: 0, Y: 0, Z: 1000}, + ), + ), + ) + + pointSize := float32(50.0) + + // 1. Single color point cloud in a grid (100mm spacing between points) + var gridPoints []r3.Vector + for x := -1000.0; x <= 1000.0; x += 100.0 { + for y := -1000.0; y <= 1000.0; y += 100.0 { + for z := 0.0; z <= 2000.0; z += 100.0 { + gridPoints = append(gridPoints, r3.Vector{X: x, Y: y, Z: z}) + } + } + } + + err := snapshot.DrawPoints( + "grid-points", + "world", + spatialmath.NewZeroPose(), + gridPoints, + WithPointsColors(NewColor(WithName("red"))), + WithPointsSize(pointSize), + ) + if err != nil { + t.Fatal(err) + } + + // 2. Per-point colored points in a sphere (1200mm radius, ~100mm average spacing) + var spherePoints []r3.Vector + var sphereColors []Color + numPoints := 10000 + radius := 1200.0 + + for i := 0; i < numPoints; i++ { + theta := rand.Float64() * 2 * math.Pi + phi := math.Acos(2*rand.Float64() - 1) + r := radius * math.Cbrt(rand.Float64()) + + x := r * math.Sin(phi) * math.Cos(theta) + y := r * math.Sin(phi) * math.Sin(theta) + z := r * math.Cos(phi) + + spherePoints = append(spherePoints, r3.Vector{X: x + 4000, Y: y, Z: z + 1200}) + hue := math.Mod(theta/(2*math.Pi), 1.0) + sphereColors = append(sphereColors, NewColor(WithHSV(float32(hue), 1.0, 1.0))) + } + + err = snapshot.DrawPoints( + "sphere-points", + "world", + spatialmath.NewZeroPose(), + spherePoints, + WithPointsColors(sphereColors[0], sphereColors[1:]...), + WithPointsSize(pointSize), + ) + if err != nil { + t.Fatal(err) + } + + // 3. Parametric surface (wave) - 100mm spacing between points\ + var wavePoints []r3.Vector + for x := -5000.0; x <= 5000.0; x += 100.0 { + for y := -5000.0; y <= 5000.0; y += 100.0 { + dist := math.Sqrt(x*x + y*y) + z := 500 * math.Sin(dist/1000) * math.Exp(-dist/10000) + wavePoints = append(wavePoints, r3.Vector{X: x - 8000, Y: y, Z: z + 1000}) + } + } + + err = snapshot.DrawPoints( + "wave-points", + "world", + spatialmath.NewZeroPose(), + wavePoints, + WithPointsColors(NewColor(WithName("blue"))), + WithPointsSize(pointSize), + ) + if err != nil { + t.Fatal(err) + } + + writeSnapshot(t, snapshot, "snapshot_points.json") +} + +// TestSnapshotModel generates a snapshot showcasing model shapes with fun models +func TestSnapshotModel(t *testing.T) { + snapshot := NewSnapshot( + WithSceneCamera( + NewSceneCamera( + r3.Vector{X: 3000, Y: 3000, Z: 2000}, + r3.Vector{X: 0, Y: 0, Z: 300}, + ), + ), + ) + + createPose := func(x, y, z float64) spatialmath.Pose { + orientation := &spatialmath.OrientationVectorDegrees{ + OX: 1, OY: 0, OZ: 0, Theta: 90, + } + return spatialmath.NewPose(r3.Vector{X: x, Y: y, Z: z}, orientation) + } + + // 1. Duck - Classic rubber duck from URL + duckURL := "http://localhost:5173/models/Duck.glb" + duckSize := uint64(120072) + duckAsset, err := NewURLModelAsset("model/gltf-binary", duckURL, WithModelAssetSizeBytes(duckSize)) + if err != nil { + t.Fatal(err) + } + err = snapshot.DrawModel( + "duck", + "world", + createPose(-2000, -2000, 0), + WithModelAssets(duckAsset), + ) + if err != nil { + t.Fatal(err) + } + + // 2. Avocado - Fresh avocado from URL + avocadoURL := "http://localhost:5173/models/Avocado.glb" + avocadoSize := uint64(8110256) + avocadoAsset, err := NewURLModelAsset("model/gltf-binary", avocadoURL, WithModelAssetSizeBytes(avocadoSize)) + if err != nil { + t.Fatal(err) + } + err = snapshot.DrawModel( + "avocado", + "world", + createPose(0, -2000, 0), + WithModelAssets(avocadoAsset), + WithModelScale(r3.Vector{X: 20.0, Y: 20.0, Z: 20.0}), + ) + if err != nil { + t.Fatal(err) + } + + // 3. Lantern - Medieval lantern from URL + lanternURL := "http://localhost:5173/models/Lantern.glb" + lanternSize := uint64(9564180) + lanternAsset, err := NewURLModelAsset("model/gltf-binary", lanternURL, WithModelAssetSizeBytes(lanternSize)) + if err != nil { + t.Fatal(err) + } + err = snapshot.DrawModel( + "lantern", + "world", + createPose(2000, -2000, 0), + WithModelAssets(lanternAsset), + WithModelScale(r3.Vector{X: 0.04, Y: 0.04, Z: 0.04}), + ) + if err != nil { + t.Fatal(err) + } + + // 4. Animated Box - An animated box scene from GLB data + boxData, err := os.ReadFile(filepath.Join(".", "__fixtures__", "BoxAnimated.glb")) + if err != nil { + t.Fatal(err) + } + boxAnimationName := "animation_0" + boxAsset, err := NewBinaryModelAsset("model/gltf-binary", boxData, WithModelAssetSizeBytes(uint64(len(boxData)))) + if err != nil { + t.Fatal(err) + } + err = snapshot.DrawModel( + "box", + "world", + createPose(-2000, 2000, 600), + WithModelAssets(boxAsset), + WithModelAnimationName(boxAnimationName), + ) + if err != nil { + t.Fatal(err) + } + + // 5. Milk Truck - Cesium's iconic milk truck from GLB data + milkTruckData, err := os.ReadFile(filepath.Join(".", "__fixtures__", "CesiumMilkTruck.glb")) + if err != nil { + t.Fatal(err) + } + milkTruckAsset, err := NewBinaryModelAsset("model/gltf-binary", milkTruckData, WithModelAssetSizeBytes(uint64(len(milkTruckData)))) + if err != nil { + t.Fatal(err) + } + err = snapshot.DrawModel( + "milktruck", + "world", + createPose(0, 2000, 0), + WithModelAssets(milkTruckAsset), + WithModelScale(r3.Vector{X: 0.4, Y: 0.4, Z: 0.4}), + ) + if err != nil { + t.Fatal(err) + } + + // 6. Fox - Animated fox character from GLB data + foxData, err := os.ReadFile(filepath.Join(".", "__fixtures__", "Fox.glb")) + if err != nil { + t.Fatal(err) + } + foxAsset, err := NewBinaryModelAsset("model/gltf-binary", foxData, WithModelAssetSizeBytes(uint64(len(foxData)))) + if err != nil { + t.Fatal(err) + } + err = snapshot.DrawModel( + "fox", + "world", + createPose(2000, 2000, 0), + WithModelAssets(foxAsset), + WithModelScale(r3.Vector{X: 0.02, Y: 0.02, Z: 0.02}), + ) + if err != nil { + t.Fatal(err) + } + + writeSnapshot(t, snapshot, "snapshot_model.json") +} + +type sceneFixture struct { + FrameSystem json.RawMessage `json:"frame_system"` + FrameSystemInputs map[string][]float64 `json:"frame_system_inputs"` + SceneConfig sceneConfig `json:"scene_config"` + SceneCamera SceneCamera `json:"scene_camera,omitempty"` +} + +type sceneConfig struct { + Arm string `json:"arm"` + Camera string `json:"camera"` + Vacuum string `json:"vacuum"` +} + +func getColorForFrame(frameName string, config sceneConfig, palette map[string]Color) Color { + if strings.HasPrefix(frameName, config.Arm) || strings.HasPrefix(frameName, "arm") { + return palette["arm"] + } + if strings.HasPrefix(frameName, config.Camera) || strings.HasPrefix(frameName, "camera") { + return palette["camera"] + } + if strings.HasPrefix(frameName, config.Vacuum) || strings.HasPrefix(frameName, "vacuum") { + return palette["vacuum"] + } + if strings.HasPrefix(frameName, "sander") { + return palette["sander"] + } + if strings.HasPrefix(frameName, "wall") { + return palette["wall"] + } + if strings.HasPrefix(frameName, "floor") { + return palette["floor"] + } + if strings.HasPrefix(frameName, "hardtop-middle-wall") { + return palette["hardtop-middle-wall"] + } + return palette["default"] +} + +func TestSnapshotScene(t *testing.T) { + data, err := os.ReadFile(filepath.Join(".", fixturesDir, "scene_fixture.json")) + if err != nil { + t.Fatal(fmt.Errorf("failed to read fixture file: %w", err)) + } + + var fixture sceneFixture + if err := json.Unmarshal(data, &fixture); err != nil { + t.Fatal(fmt.Errorf("failed to unmarshal fixture: %w", err)) + } + + sceneCamera := NewSceneCamera(fixture.SceneCamera.Position, fixture.SceneCamera.LookAt) + snapshot := NewSnapshot(WithSceneCamera(sceneCamera)) + + var frameSystem referenceframe.FrameSystem + if err := json.Unmarshal(fixture.FrameSystem, &frameSystem); err != nil { + t.Fatal(fmt.Errorf("failed to unmarshal frame system: %w", err)) + } + + inputs := make(referenceframe.FrameSystemInputs) + for k, v := range fixture.FrameSystemInputs { + inputs[k] = referenceframe.FloatsToInputs(v) + } + + colors := make(map[string]Color) + colorPalette := map[string]Color{ + "default": NewColor(WithName("red")), + "arm": NewColor(WithName("orange")), + "camera": NewColor(WithName("blue")), + "sander": NewColor(WithName("cyan")), + "vacuum": NewColor(WithName("goldenrod")), + "wall": NewColor(WithName("gray")), + "floor": NewColor(WithName("slategray")), + "hardtop-middle-wall": NewColor(WithName("magenta")), + } + + for _, frame := range frameSystem.FrameNames() { + color := getColorForFrame(frame, fixture.SceneConfig, colorPalette) + colors[frame] = color + } + + if err := snapshot.DrawFrameSystemGeometries(&frameSystem, inputs, colors); err != nil { + t.Fatal(fmt.Errorf("failed to draw frame system geometries: %w", err)) + } + + if err := snapshot.Validate(); err != nil { + t.Fatal(fmt.Errorf("snapshot validation failed: %w", err)) + } + + writeSnapshot(t, snapshot, "snapshot_scene.json") +} + +func writeSnapshot(t *testing.T, snapshot *Snapshot, filename string) { + t.Helper() + + dir := filepath.Join(".", snapshotDir) + if err := os.MkdirAll(dir, 0o755); err != nil { + t.Fatal(err) + } + + // Marshal to JSON + jsonBytes, err := snapshot.MarshalJSON() + if err != nil { + t.Fatal(err) + } + + // Marshal to binary + binaryBytes, err := snapshot.MarshalBinary() + if err != nil { + t.Fatal(err) + } + + // Marshal to gzip-compressed binary + gzipBytes, err := snapshot.MarshalBinaryGzip() + if err != nil { + t.Fatal(err) + } + + // Write JSON file + jsonPath := filepath.Join(dir, filename) + if err := os.WriteFile(jsonPath, jsonBytes, 0o644); err != nil { + t.Fatal(err) + } + + // Write gzip-compressed binary file (replace .json with .pb.gz) + gzipFilename := strings.TrimSuffix(filename, ".json") + ".pb.gz" + gzipPath := filepath.Join(dir, gzipFilename) + if err := os.WriteFile(gzipPath, gzipBytes, 0o644); err != nil { + t.Fatal(err) + } + + // Log sizes for benchmarking + t.Logf("Generated %s: %d transforms, %d drawings", filename, len(snapshot.Transforms()), len(snapshot.Drawings())) + t.Logf(" JSON: %d bytes", len(jsonBytes)) + t.Logf(" Binary: %d bytes (%.1f%% of JSON)", len(binaryBytes), float64(len(binaryBytes))/float64(len(jsonBytes))*100) + t.Logf(" Gzip: %d bytes (%.1f%% of JSON)", len(gzipBytes), float64(len(gzipBytes))/float64(len(jsonBytes))*100) +} diff --git a/draw/transform.go b/draw/transform.go new file mode 100644 index 00000000..9ef85d18 --- /dev/null +++ b/draw/transform.go @@ -0,0 +1,57 @@ +package draw + +import ( + "encoding/base64" + + "github.com/google/uuid" + commonv1 "go.viam.com/api/common/v1" + "go.viam.com/rdk/spatialmath" + "google.golang.org/protobuf/types/known/structpb" +) + +// NewTransform creates a Transform with optional geometry and optional metadata +// If units is nil, uses mm +func NewTransform( + id string, + name string, + parent string, + pose spatialmath.Pose, + geometry spatialmath.Geometry, + metadata *structpb.Struct, +) (*commonv1.Transform, error) { + var idBytes []byte + if id == "" { + newId := uuid.New() + idBytes = newId[:] + } else { + parsedId, err := uuid.Parse(id) + if err != nil { + return nil, err + } + idBytes = parsedId[:] + } + + poseInFrame := poseInFrameToProtobuf(pose, parent) + transform := &commonv1.Transform{ + Uuid: idBytes, + ReferenceFrame: name, + PoseInObserverFrame: poseInFrame, + Metadata: metadata, + } + + // Only set PhysicalObject if geometry is provided + if geometry != nil { + transform.PhysicalObject = geometryToProtobuf(geometry) + } + + return transform, nil +} + +// MetadataToStruct converts drawing metadata (colors/alphas) into a Struct +func MetadataToStruct(metadata Metadata) (*structpb.Struct, error) { + fields := make(map[string]*structpb.Value) + encoded := base64.StdEncoding.EncodeToString(packColors(metadata.Colors)) + fields["colors"] = structpb.NewStringValue(encoded) + + return &structpb.Struct{Fields: fields}, nil +} diff --git a/e2e/edit-frame.test.ts b/e2e/edit-frame.test.ts index 91765eba..3fe1faba 100644 --- a/e2e/edit-frame.test.ts +++ b/e2e/edit-frame.test.ts @@ -1,45 +1,28 @@ import { expect, test } from '@playwright/test' -import { - createViamClient, - JsonValue, - Struct, - ViamClient, - ViamClientOptions, -} from '@viamrobotics/sdk' - -const testConfig = { - host: 'motion-tools-e2e-main.l6j4r7m65g.viam.cloud', - name: 'motion-tools-e2e-main', - partId: '9741704d-ea0e-484c-8cf8-0a849096af1e', - apiKeyId: '76fcaf4b-4e04-4c6b-9665-c9c663ee4fad', - apiKeyValue: 'iz95ie2bz5h617xhs2ko9eov1b5bryas', - signalingAddress: 'https://app.viam.com:443', - organizationId: 'd9fd430a-25ec-47ba-b548-5d1b1b2fc6d1', -} -const fragmentIdsToDelete: string[] = [] - -async function connect(): Promise { - const API_KEY_ID = testConfig.apiKeyId - const API_KEY = testConfig.apiKeyValue - const opts: ViamClientOptions = { - serviceHost: testConfig.signalingAddress, - credentials: { - type: 'api-key', - authEntity: API_KEY_ID, - payload: API_KEY, - }, - } - - const client = await createViamClient(opts) - return client -} +import { JsonValue, Struct, ViamClient } from '@viamrobotics/sdk' +import { createPage } from './helpers/create-page' +import { assertNoFailedScreenshots, takeScreenshot } from './helpers/take-screenshot' +import { connect, testConfig } from './helpers/connect' +const fragmentIdsToDelete: string[] = [] let viamClient: ViamClient test.beforeAll(async () => { viamClient = await connect() }) +test.afterAll('cleanup', async () => { + await viamClient.appClient.updateRobotPart( + testConfig.partId, + testConfig.name, + Struct.fromJson({}) + ) + + for (const fragmentId of fragmentIdsToDelete) { + await viamClient.appClient.deleteFragment(fragmentId) + } +}) + const basicEditFrameConfig = { components: [ { @@ -75,28 +58,15 @@ const basicEditFrameConfig = { test('basic edit frame', async ({ browser }) => { const testPrefix = 'BASIC_EDIT_FRAME' + const { page, failedScreenshots, refresh } = await createPage(browser) + await viamClient.appClient.updateRobotPart( testConfig.partId, testConfig.name, Struct.fromJson(basicEditFrameConfig) ) - const failedScreenshots = [] as string[] - const context = await browser.newContext() - await context.addCookies([ - { - name: 'weblab_experiments', - value: 'MOTION_TOOLS_EDIT_FRAME', - domain: 'localhost', - path: '/', - }, - ]) - let page = await context.newPage() + await page.waitForTimeout(5000) - page.on('console', (message) => { - console.log(`[${message.type()}] ${message.text()}`) - }) - await page.goto('/') - await expect(page.getByText('World', { exact: true })).toBeVisible() // SETUP CONFIG await expect(page.getByTestId('icon-robot-outline')).toBeVisible() @@ -119,8 +89,9 @@ test('basic edit frame', async ({ browser }) => { await page.getByTestId('icon-close').click() // OPEN A WORLD OBJECT AND EDIT THE FRAME - await expect(page.getByText('base-1', { exact: true })).toBeVisible() - await page.getByText('base-1', { exact: true }).click() + const tree = page.locator('[role="tree"]') + await expect(tree.getByText('base-1', { exact: true })).toBeVisible() + await tree.getByText('base-1', { exact: true }).click() await expect(page.getByTestId('details-header')).toBeVisible() @@ -142,73 +113,35 @@ test('basic edit frame', async ({ browser }) => { await page.getByLabel('mutable box dimensions z value').fill('600') await expect(page.getByText('Live updates paused', { exact: true })).toBeVisible() - try { - await expect(page).toHaveScreenshot(`${testPrefix}-0-edited.png`, { - fullPage: true, - threshold: 0.1, - }) - } catch (error) { - console.warn(error) - failedScreenshots.push(`${testPrefix}-0-edited.png`) - } + await takeScreenshot(page, `${testPrefix}-0-edited`, failedScreenshots, { fullPage: true }) // SAVE THE CHANGES await page.getByText('Save', { exact: true }).click() await expect(page.getByText('Live updates paused', { exact: true })).toBeHidden() - try { - await expect(page).toHaveScreenshot(`${testPrefix}-1-saved.png`, { - fullPage: true, - threshold: 0.1, - }) - } catch (error) { - console.warn(error) - failedScreenshots.push(`${testPrefix}-1-saved.png`) - } + await takeScreenshot(page, `${testPrefix}-1-saved`, failedScreenshots, { fullPage: true }) // give network some time to sync the config await page.waitForTimeout(5000) // RELOAD THE PAGE - page = await context.newPage() - page.on('console', (message) => { - console.log(`[${message.type()}] ${message.text()}`) - }) - await page.goto('/') - await expect(page.getByText('base-1', { exact: true })).toBeVisible() - await page.getByText('base-1', { exact: true }).click() + await refresh() + await expect(tree.getByText('base-1', { exact: true })).toBeVisible() + await tree.getByText('base-1', { exact: true }).click() await expect(page.getByTestId('details-header')).toBeVisible() // give page time to laod up frame details - try { - await expect(page).toHaveScreenshot(`${testPrefix}-2-reloaded.png`, { - fullPage: true, - threshold: 0.1, - }) - } catch (error) { - console.warn(error) - failedScreenshots.push(`${testPrefix}-2-reloaded.png`) - } + await takeScreenshot(page, `${testPrefix}-2-reloaded`, failedScreenshots, { fullPage: true }) // REPARENT THE OBJECT await expect(page.getByLabel('dropdown parent frame name')).toBeVisible() await page.getByLabel('dropdown parent frame name').click() await page.getByLabel('dropdown parent frame name').selectOption('parent') - try { - await expect(page).toHaveScreenshot(`${testPrefix}-3-parented.png`, { fullPage: true }) - } catch (error) { - console.warn(error) - failedScreenshots.push(`${testPrefix}-3-parented.png`) - } + await takeScreenshot(page, `${testPrefix}-3-parented`, failedScreenshots, { fullPage: true }) // DISCARD CHANGES await expect(page.getByText('Live updates paused', { exact: true })).toBeVisible() await page.getByText('Discard', { exact: true }).click() await expect(page.getByText('Live updates paused', { exact: true })).toBeHidden() - try { - await expect(page).toHaveScreenshot(`${testPrefix}-4-discarded.png`, { fullPage: true }) - } catch (error) { - console.warn(error) - failedScreenshots.push(`${testPrefix}-4-discarded.png`) - } + await takeScreenshot(page, `${testPrefix}-4-discarded`, failedScreenshots, { fullPage: true }) // RESTORE THE ORIGINAL FRAME await expect(page.getByText('None', { exact: true })).toBeVisible() @@ -225,17 +158,9 @@ test('basic edit frame', async ({ browser }) => { await expect(page.getByText('Live updates paused', { exact: true })).toBeVisible() await page.getByText('Save', { exact: true }).click() await expect(page.getByText('Live updates paused', { exact: true })).toBeHidden() - try { - await expect(page).toHaveScreenshot(`${testPrefix}-5-restored.png`, { fullPage: true }) - } catch (error) { - console.warn(error) - failedScreenshots.push(`${testPrefix}-5-restored.png`) - } + await takeScreenshot(page, `${testPrefix}-5-restored`, failedScreenshots, { fullPage: true }) - if (failedScreenshots.length > 0) { - console.log(`Failed screenshots: ${failedScreenshots.join(', ')}`) - throw new Error(`Failed screenshots: ${failedScreenshots.join(', ')}`) - } + assertNoFailedScreenshots(failedScreenshots) }) const createDeleteFrameConfig = { @@ -265,28 +190,13 @@ const createDeleteFrameConfig = { test('create and delete frame', async ({ browser }) => { const testPrefix = 'CREATE_DELETE' + const { page, failedScreenshots, refresh } = await createPage(browser) + await viamClient.appClient.updateRobotPart( testConfig.partId, testConfig.name, Struct.fromJson(createDeleteFrameConfig as unknown as JsonValue) ) - const failedScreenshots = [] as string[] - const context = await browser.newContext() - await context.addCookies([ - { - name: 'weblab_experiments', - value: 'MOTION_TOOLS_EDIT_FRAME', - domain: 'localhost', - path: '/', - }, - ]) - const page = await context.newPage() - await page.waitForTimeout(5000) - page.on('console', (message) => { - console.log(`[${message.type()}] ${message.text()}`) - }) - await page.goto('/') - await expect(page.getByText('World', { exact: true })).toBeVisible() // SETUP CONFIG await expect(page.getByTestId('icon-robot-outline')).toBeVisible() @@ -318,12 +228,7 @@ test('create and delete frame', async ({ browser }) => { await expect(page.getByTestId('icon-plus')).toBeVisible() page.getByTestId('icon-plus').click() - try { - await expect(page).toHaveScreenshot(`${testPrefix}-0-added.png`, { fullPage: true }) - } catch (error) { - console.warn(error) - failedScreenshots.push(`${testPrefix}-0-added.png`) - } + await takeScreenshot(page, `${testPrefix}-0-added`, failedScreenshots, { fullPage: true }) await expect(page.getByText('Live updates paused', { exact: true })).toBeVisible() await page.getByText('Save', { exact: true }).click() @@ -338,28 +243,15 @@ test('create and delete frame', async ({ browser }) => { await expect(page.getByText('Delete frame', { exact: true })).toBeVisible() page.getByText('Delete frame', { exact: true }).click() - try { - await expect(page).toHaveScreenshot(`${testPrefix}-1-deleted.png`, { fullPage: true }) - } catch (error) { - console.warn(error) - failedScreenshots.push(`${testPrefix}-1-deleted.png`) - } + await takeScreenshot(page, `${testPrefix}-1-deleted`, failedScreenshots, { fullPage: true }) // DISCARD CHANGES await expect(page.getByText('Live updates paused', { exact: true })).toBeVisible() await page.getByText('Discard', { exact: true }).click() await expect(page.getByText('Live updates paused', { exact: true })).toBeHidden() - try { - await expect(page).toHaveScreenshot(`${testPrefix}-2-discarded.png`, { fullPage: true }) - } catch (error) { - console.warn(error) - failedScreenshots.push(`${testPrefix}-2-discarded.png`) - } + await takeScreenshot(page, `${testPrefix}-2-discarded`, failedScreenshots, { fullPage: true }) - if (failedScreenshots.length > 0) { - console.log(`Failed screenshots: ${failedScreenshots.join(', ')}`) - throw new Error(`Failed screenshots: ${failedScreenshots.join(', ')}`) - } + assertNoFailedScreenshots(failedScreenshots) }) const fragmentConfig = { components: [ @@ -422,7 +314,8 @@ const fragmentUsingConfig = (fragmentId: string) => { test('fragement edit frame', async ({ browser }) => { const testPrefix = 'FRAGMENT_EDIT_FRAME' - const failedScreenshots = [] as string[] + const { page, failedScreenshots } = await createPage(browser) + const resp = await viamClient.appClient.createFragment( testConfig.organizationId, 'TEMP_FRAGMENT', @@ -439,24 +332,6 @@ test('fragement edit frame', async ({ browser }) => { Struct.fromJson(fragmentUsingConfig(resp.id) as unknown as JsonValue) ) - // WAIT FOR THE TREE DRAWER TO LOAD - const context = await browser.newContext() - await context.addCookies([ - { - name: 'weblab_experiments', - value: 'MOTION_TOOLS_EDIT_FRAME', - domain: 'localhost', - path: '/', - }, - ]) - const page = await context.newPage() - await page.waitForTimeout(5000) - page.on('console', (message) => { - console.log(`[${message.type()}] ${message.text()}`) - }) - await page.goto('/') - await expect(page.getByText('World', { exact: true })).toBeVisible() - // SETUP CONFIG await expect(page.getByTestId('icon-robot-outline')).toBeVisible() await page.getByTestId('icon-robot-outline').click() @@ -480,12 +355,7 @@ test('fragement edit frame', async ({ browser }) => { // WAIT FOR THE TREE DRAWER TO LOAD await expect(page.getByText('frag-base-1', { exact: true })).toBeVisible() - try { - await expect(page).toHaveScreenshot(`${testPrefix}-0-setup.png`, { fullPage: true }) - } catch (error) { - console.warn(error) - failedScreenshots.push(`${testPrefix}-0-setup.png`) - } + await takeScreenshot(page, `${testPrefix}-0-setup`, failedScreenshots, { fullPage: true }) await expect(page.getByText('frag-base-1', { exact: true })).toBeVisible() await page.getByText('frag-base-1', { exact: true }).click() @@ -510,29 +380,7 @@ test('fragement edit frame', async ({ browser }) => { await page.getByText('Save', { exact: true }).click() await expect(page.getByText('Live updates paused', { exact: true })).toBeHidden() - try { - await expect(page).toHaveScreenshot(`${testPrefix}-1-saved.png`, { - fullPage: true, - threshold: 0.1, - }) - } catch (error) { - console.warn(error) - failedScreenshots.push(`${testPrefix}-1-saved.png`) - } - - if (failedScreenshots.length > 0) { - console.log(`Failed screenshots: ${failedScreenshots.join(', ')}`) - throw new Error(`Failed screenshots: ${failedScreenshots.join(', ')}`) - } -}) + await takeScreenshot(page, `${testPrefix}-1-saved`, failedScreenshots, { fullPage: true }) -test.afterAll('cleanup', async () => { - await viamClient.appClient.updateRobotPart( - testConfig.partId, - testConfig.name, - Struct.fromJson({}) - ) - for (const fragmentId of fragmentIdsToDelete) { - await viamClient.appClient.deleteFragment(fragmentId) - } + assertNoFailedScreenshots(failedScreenshots) }) diff --git a/e2e/go-client.test.ts b/e2e/go-client.test.ts index 9ccc8e84..8c9d49a6 100644 --- a/e2e/go-client.test.ts +++ b/e2e/go-client.test.ts @@ -1,48 +1,10 @@ -import { Browser, expect, Page, test } from '@playwright/test' +import { expect, test } from '@playwright/test' import { execSync } from 'child_process' - -const createPage = async (browser: Browser): Promise => { - const context = await browser.newContext() - await context.addCookies([ - { - name: 'weblab_experiments', - value: 'MOTION_TOOLS_EDIT_FRAME', - domain: 'localhost', - path: '/', - }, - ]) - const page = await context.newPage() - page.on('console', (message) => { - console.log(`[${message.type()}] ${message.text()}`) - }) - await page.goto('/') - await expect(page.getByText('World', { exact: true })).toBeVisible({ timeout: 10000 }) - return page -} - -const takeScreenshot = async (page: Page, testPrefix: string, failedScreenshots: string[]) => { - try { - await expect(page).toHaveScreenshot(`${testPrefix}.png`, { - fullPage: true, - threshold: 0.1, - }) - } catch (error) { - console.warn(error) - failedScreenshots.push(`${testPrefix}.png`) - } -} - -const assertNoFailedScreenshots = (failedScreenshots: string[]) => { - if (failedScreenshots.length > 0) { - console.log(`Failed screenshots: ${failedScreenshots.join(', ')}`) - throw new Error(`Failed screenshots: ${failedScreenshots.join(', ')}`) - } -} +import { createPage } from './helpers/create-page' +import { assertNoFailedScreenshots, takeScreenshot } from './helpers/take-screenshot' test('draw frame system', async ({ browser }) => { - const testPrefix = 'DRAW_FRAME_SYSTEM' - const failedScreenshots = [] as string[] - const page = await createPage(browser) + const { page, failedScreenshots } = await createPage(browser) execSync( 'go test -run ^TestDrawFrameSystem$/DrawFrameSystem github.com/viam-labs/motion-tools/client/client -count=1', @@ -51,15 +13,13 @@ test('draw frame system', async ({ browser }) => { } ) - await takeScreenshot(page, testPrefix, failedScreenshots) + await takeScreenshot(page, 'DRAW_FRAME_SYSTEM', failedScreenshots) assertNoFailedScreenshots(failedScreenshots) }) test('draw frames', async ({ browser }) => { - const testPrefix = 'DRAW_FRAMES' - const failedScreenshots = [] as string[] - const page = await createPage(browser) + const { page, failedScreenshots } = await createPage(browser) execSync( 'go test -run ^TestDrawFrames$/DrawFrames github.com/viam-labs/motion-tools/client/client -count=1', @@ -67,15 +27,14 @@ test('draw frames', async ({ browser }) => { encoding: 'utf-8', } ) - await takeScreenshot(page, testPrefix, failedScreenshots) + + await takeScreenshot(page, 'DRAW_FRAMES', failedScreenshots) assertNoFailedScreenshots(failedScreenshots) }) test('draw geometries', async ({ browser }) => { - const testPrefix = 'DRAW_GEOMETRIES' - const failedScreenshots = [] as string[] - const page = await createPage(browser) + const { page, failedScreenshots } = await createPage(browser) execSync( 'go test -run ^TestDrawGeometries$/DrawGeometries github.com/viam-labs/motion-tools/client/client -count=1', @@ -83,15 +42,14 @@ test('draw geometries', async ({ browser }) => { encoding: 'utf-8', } ) - await takeScreenshot(page, testPrefix, failedScreenshots) + + await takeScreenshot(page, 'DRAW_GEOMETRIES', failedScreenshots) assertNoFailedScreenshots(failedScreenshots) }) test('draw geometries updating', async ({ browser }) => { - const testPrefix = 'DRAW_GEOMETRIES_UPDATING' - const failedScreenshots = [] as string[] - const page = await createPage(browser) + const { page, failedScreenshots } = await createPage(browser) execSync( 'go test -run ^TestDrawGeometriesUpdating$/DrawGeometriesUpdating github.com/viam-labs/motion-tools/client/client -count=1', @@ -99,15 +57,14 @@ test('draw geometries updating', async ({ browser }) => { encoding: 'utf-8', } ) - await takeScreenshot(page, testPrefix, failedScreenshots) + + await takeScreenshot(page, 'DRAW_GEOMETRIES_UPDATING', failedScreenshots) assertNoFailedScreenshots(failedScreenshots) }) test('draw gltf', async ({ browser }) => { - const testPrefix = 'DRAW_GLTF' - const failedScreenshots = [] as string[] - const page = await createPage(browser) + const { page, failedScreenshots } = await createPage(browser) execSync( 'go test -run ^TestDrawGLTF$/DrawGLTF github.com/viam-labs/motion-tools/client/client -count=1', @@ -115,15 +72,18 @@ test('draw gltf', async ({ browser }) => { encoding: 'utf-8', } ) - await takeScreenshot(page, testPrefix, failedScreenshots) + + await expect(page.locator('[role="tree"]').getByText(/Scene|model/)).toBeVisible({ + timeout: 30000, + }) + + await takeScreenshot(page, 'DRAW_GLTF', failedScreenshots) assertNoFailedScreenshots(failedScreenshots) }) test('draw lines', async ({ browser }) => { - const testPrefix = 'DRAW_LINES' - const failedScreenshots = [] as string[] - const page = await createPage(browser) + const { page, failedScreenshots } = await createPage(browser) execSync( 'go test -run ^TestDrawLines$/DrawLine github.com/viam-labs/motion-tools/client/client -count=1', @@ -131,15 +91,14 @@ test('draw lines', async ({ browser }) => { encoding: 'utf-8', } ) - await takeScreenshot(page, testPrefix, failedScreenshots) + + await takeScreenshot(page, 'DRAW_LINES', failedScreenshots) assertNoFailedScreenshots(failedScreenshots) }) test('draw point cloud', async ({ browser }) => { - const testPrefix = 'DRAW_POINT_CLOUD' - const failedScreenshots = [] as string[] - const page = await createPage(browser) + const { page, failedScreenshots } = await createPage(browser) execSync( 'go test -run ^TestDrawPointCloud$/DrawPointCloud github.com/viam-labs/motion-tools/client/client -count=1', @@ -147,15 +106,20 @@ test('draw point cloud', async ({ browser }) => { encoding: 'utf-8', } ) - await takeScreenshot(page, testPrefix, failedScreenshots) + + await expect(page.getByText('Zaghetto', { exact: true })).toBeVisible({ timeout: 15000 }) + await expect(page.getByText('Zaghetto1', { exact: true })).toBeVisible({ timeout: 15000 }) + await expect(page.getByText('Zaghetto2', { exact: true })).toBeVisible({ timeout: 15000 }) + await expect(page.getByText('Zaghetto3', { exact: true })).toBeVisible({ timeout: 15000 }) + await expect(page.getByText('Zaghetto4', { exact: true })).toBeVisible({ timeout: 15000 }) + + await takeScreenshot(page, 'DRAW_POINT_CLOUD', failedScreenshots) assertNoFailedScreenshots(failedScreenshots) }) test('draw points', async ({ browser }) => { - const testPrefix = 'DRAW_POINTS' - const failedScreenshots = [] as string[] - const page = await createPage(browser) + const { page, failedScreenshots } = await createPage(browser) execSync( 'go test -run ^TestDrawPoints$/DrawPoints github.com/viam-labs/motion-tools/client/client -count=1', @@ -163,15 +127,14 @@ test('draw points', async ({ browser }) => { encoding: 'utf-8', } ) - await takeScreenshot(page, testPrefix, failedScreenshots) + + await takeScreenshot(page, 'DRAW_POINTS', failedScreenshots) assertNoFailedScreenshots(failedScreenshots) }) test('draw poses', async ({ browser }) => { - const testPrefix = 'DRAW_POSES' - const failedScreenshots = [] as string[] - const page = await createPage(browser) + const { page, failedScreenshots } = await createPage(browser) execSync( 'go test -run ^TestDrawPoses$/DrawPoses github.com/viam-labs/motion-tools/client/client -count=1', @@ -179,15 +142,14 @@ test('draw poses', async ({ browser }) => { encoding: 'utf-8', } ) - await takeScreenshot(page, testPrefix, failedScreenshots) + + await takeScreenshot(page, 'DRAW_POSES', failedScreenshots) assertNoFailedScreenshots(failedScreenshots) }) test('draw world state', async ({ browser }) => { - const testPrefix = 'DRAW_WORLD_STATE' - const failedScreenshots = [] as string[] - const page = await createPage(browser) + const { page, failedScreenshots } = await createPage(browser) execSync( 'go test -run ^TestDrawWorldState$/DrawWorldState github.com/viam-labs/motion-tools/client/client -count=1', @@ -195,15 +157,13 @@ test('draw world state', async ({ browser }) => { encoding: 'utf-8', } ) - await takeScreenshot(page, testPrefix, failedScreenshots) + await takeScreenshot(page, 'DRAW_WORLD_STATE', failedScreenshots) assertNoFailedScreenshots(failedScreenshots) }) test('remove spatial objects', async ({ browser }) => { - const testPrefix = 'REMOVE_SPATIAL_OBJECTS' - const failedScreenshots = [] as string[] - const page = await createPage(browser) + const { page, failedScreenshots } = await createPage(browser) execSync( 'go test -run ^TestRemoveSpatialObjects$/RemoveSpatialObjects github.com/viam-labs/motion-tools/client/client -count=1', @@ -211,15 +171,13 @@ test('remove spatial objects', async ({ browser }) => { encoding: 'utf-8', } ) - await takeScreenshot(page, testPrefix, failedScreenshots) + await takeScreenshot(page, 'REMOVE_SPATIAL_OBJECTS', failedScreenshots) assertNoFailedScreenshots(failedScreenshots) }) test('set camera pose', async ({ browser }) => { - const testPrefix = 'SET_CAMERA_POSE' - const failedScreenshots = [] as string[] - const page = await createPage(browser) + const { page, failedScreenshots } = await createPage(browser) execSync( 'go test -run ^TestSetCameraPose$/SetCameraPose github.com/viam-labs/motion-tools/client/client -count=1', @@ -227,7 +185,12 @@ test('set camera pose', async ({ browser }) => { encoding: 'utf-8', } ) - await takeScreenshot(page, testPrefix, failedScreenshots) + + // Camera animations are client-side and can't be detected via DOM + // Wait for a reasonable duration for the animation to complete (typical animations are 500-1000ms) + await page.waitForTimeout(3000) + + await takeScreenshot(page, 'SET_CAMERA_POSE', failedScreenshots) assertNoFailedScreenshots(failedScreenshots) }) diff --git a/e2e/go-client.test.ts-snapshots/DRAW-FRAME-SYSTEM-darwin.png b/e2e/go-client.test.ts-snapshots/DRAW-FRAME-SYSTEM-darwin.png index 28c16f79..db6888af 100644 Binary files a/e2e/go-client.test.ts-snapshots/DRAW-FRAME-SYSTEM-darwin.png and b/e2e/go-client.test.ts-snapshots/DRAW-FRAME-SYSTEM-darwin.png differ diff --git a/e2e/go-client.test.ts-snapshots/DRAW-FRAMES-darwin.png b/e2e/go-client.test.ts-snapshots/DRAW-FRAMES-darwin.png index dbf2fae1..4531eeef 100644 Binary files a/e2e/go-client.test.ts-snapshots/DRAW-FRAMES-darwin.png and b/e2e/go-client.test.ts-snapshots/DRAW-FRAMES-darwin.png differ diff --git a/e2e/go-client.test.ts-snapshots/DRAW-GEOMETRIES-darwin.png b/e2e/go-client.test.ts-snapshots/DRAW-GEOMETRIES-darwin.png index c09b534f..1051e321 100644 Binary files a/e2e/go-client.test.ts-snapshots/DRAW-GEOMETRIES-darwin.png and b/e2e/go-client.test.ts-snapshots/DRAW-GEOMETRIES-darwin.png differ diff --git a/e2e/go-client.test.ts-snapshots/DRAW-POINT-CLOUD-darwin.png b/e2e/go-client.test.ts-snapshots/DRAW-POINT-CLOUD-darwin.png index 60b444a1..8f3770bb 100644 Binary files a/e2e/go-client.test.ts-snapshots/DRAW-POINT-CLOUD-darwin.png and b/e2e/go-client.test.ts-snapshots/DRAW-POINT-CLOUD-darwin.png differ diff --git a/e2e/helpers/cleanup/cleanup.go b/e2e/helpers/cleanup/cleanup.go new file mode 100644 index 00000000..9490beca --- /dev/null +++ b/e2e/helpers/cleanup/cleanup.go @@ -0,0 +1,15 @@ +package main + +import ( + "fmt" + "os" + + "github.com/viam-labs/motion-tools/client/client" +) + +func main() { + if err := client.RemoveAllSpatialObjects(); err != nil { + fmt.Fprintf(os.Stderr, "Failed to remove all spatial objects: %v\n", err) + os.Exit(1) + } +} diff --git a/e2e/helpers/cleanup/cleanup.ts b/e2e/helpers/cleanup/cleanup.ts new file mode 100644 index 00000000..58dd9a89 --- /dev/null +++ b/e2e/helpers/cleanup/cleanup.ts @@ -0,0 +1,7 @@ +import { execSync } from 'child_process' +import { expect, Page } from '@playwright/test' + +export const cleanup = async (page: Page) => { + execSync('go run e2e/helpers/cleanup/cleanup.go', { encoding: 'utf-8' }) + await expect(page.getByText('No objects displayed')).toBeVisible() +} diff --git a/e2e/helpers/connect.ts b/e2e/helpers/connect.ts new file mode 100644 index 00000000..b3dca8e5 --- /dev/null +++ b/e2e/helpers/connect.ts @@ -0,0 +1,24 @@ +import { createViamClient, ViamClient, ViamClientOptions } from '@viamrobotics/sdk' + +export const testConfig = { + host: 'motion-tools-e2e-main.l6j4r7m65g.viam.cloud', + name: 'motion-tools-e2e-main', + partId: '9741704d-ea0e-484c-8cf8-0a849096af1e', + apiKeyId: '76fcaf4b-4e04-4c6b-9665-c9c663ee4fad', + apiKeyValue: 'iz95ie2bz5h617xhs2ko9eov1b5bryas', + signalingAddress: 'https://app.viam.com:443', + organizationId: 'd9fd430a-25ec-47ba-b548-5d1b1b2fc6d1', +} + +export const connect = async (): Promise => { + const opts: ViamClientOptions = { + serviceHost: testConfig.signalingAddress, + credentials: { + type: 'api-key', + authEntity: testConfig.apiKeyId, + payload: testConfig.apiKeyValue, + }, + } + + return createViamClient(opts) +} diff --git a/e2e/helpers/create-page.ts b/e2e/helpers/create-page.ts new file mode 100644 index 00000000..faa72f50 --- /dev/null +++ b/e2e/helpers/create-page.ts @@ -0,0 +1,42 @@ +import { Browser, expect } from '@playwright/test' +import { cleanup } from './cleanup/cleanup' + +export const createPage = async (browser: Browser) => { + const failedScreenshots = [] as string[] + const context = await browser.newContext() + await context.addCookies([ + { + name: 'weblab_experiments', + value: 'MOTION_TOOLS_EDIT_FRAME', + domain: 'localhost', + path: '/', + }, + ]) + + let page = await context.newPage() + + // Add console logging for debugging + page.on('console', (message) => { + console.log(`[${message.type()}] ${message.text()}`) + }) + + const load = async () => { + await page.goto('/') + await expect(page.getByRole('heading', { name: 'World' })).toBeVisible({ timeout: 15000 }) + } + + const refresh = async () => { + page = await context.newPage() + // Re-attach console logging on refresh + page.on('console', (message) => { + console.log(`[${message.type()}] ${message.text()}`) + }) + await load() + } + + await load() + await page.waitForTimeout(5000) + await cleanup(page) + + return { page, failedScreenshots, refresh } +} diff --git a/e2e/helpers/take-screenshot.ts b/e2e/helpers/take-screenshot.ts new file mode 100644 index 00000000..97963ffe --- /dev/null +++ b/e2e/helpers/take-screenshot.ts @@ -0,0 +1,34 @@ +import { Page, PageAssertionsToHaveScreenshotOptions, expect } from '@playwright/test' + +export const takeScreenshot = async ( + page: Page, + testPrefix: string, + failedScreenshots: string[], + options: PageAssertionsToHaveScreenshotOptions = { + fullPage: true, + threshold: 0.1, + } +) => { + // Wait for any rendering to stabilize + await page.evaluate(() => { + return new Promise((resolve) => { + requestAnimationFrame(() => { + resolve() + }) + }) + }) + + try { + await expect(page).toHaveScreenshot(`${testPrefix}.png`, options) + } catch (error) { + console.warn(error) + failedScreenshots.push(`${testPrefix}.png`) + } +} + +export const assertNoFailedScreenshots = (failedScreenshots: string[]) => { + if (failedScreenshots.length > 0) { + console.log(`Failed screenshots: ${failedScreenshots.join(', ')}`) + throw new Error(`Failed screenshots: ${failedScreenshots.join(', ')}`) + } +} diff --git a/go.mod b/go.mod index d6d38982..de483516 100644 --- a/go.mod +++ b/go.mod @@ -6,8 +6,11 @@ toolchain go1.23.11 require ( github.com/golang/geo v0.0.0-20230421003525-6adc56603217 + github.com/google/uuid v1.6.0 + go.viam.com/api v0.1.495 go.viam.com/rdk v0.90.0 go.viam.com/test v1.2.4 + golang.org/x/image v0.25.0 google.golang.org/protobuf v1.36.8 ) @@ -56,7 +59,6 @@ require ( github.com/gonuts/binary v0.2.0 // indirect github.com/google/go-cmp v0.7.0 // indirect github.com/google/s2a-go v0.1.8 // indirect - github.com/google/uuid v1.6.0 // indirect github.com/googleapis/enterprise-certificate-proxy v0.3.3 // indirect github.com/googleapis/gax-go/v2 v2.13.0 // indirect github.com/gorilla/securecookie v1.1.2 // indirect @@ -129,12 +131,10 @@ require ( go.uber.org/goleak v1.3.0 // indirect go.uber.org/multierr v1.11.0 // indirect go.uber.org/zap v1.27.0 // indirect - go.viam.com/api v0.1.473 // indirect go.viam.com/utils v0.1.163 // indirect golang.org/x/arch v0.0.0-20210923205945-b76863e36670 // indirect golang.org/x/crypto v0.41.0 // indirect golang.org/x/exp v0.0.0-20241009180824-f66d83c29e7c // indirect - golang.org/x/image v0.25.0 // indirect golang.org/x/mod v0.26.0 // indirect golang.org/x/net v0.43.0 // indirect golang.org/x/oauth2 v0.30.0 // indirect diff --git a/go.sum b/go.sum index f1986047..5a2ced91 100644 --- a/go.sum +++ b/go.sum @@ -917,8 +917,8 @@ go.uber.org/zap v1.18.1/go.mod h1:xg/QME4nWcxGxrpdeYfq7UvYrLh66cuVKdrbD1XF/NI= go.uber.org/zap v1.23.0/go.mod h1:D+nX8jyLsMHMYrln8A0rJjFt/T/9/bGgIhAqxv5URuY= go.uber.org/zap v1.27.0 h1:aJMhYGrd5QSmlpLMr2MftRKl7t8J8PTZPA732ud/XR8= go.uber.org/zap v1.27.0/go.mod h1:GB2qFLM7cTU87MWRP2mPIjqfIDnGu+VIO4V/SdhGo2E= -go.viam.com/api v0.1.473 h1:Hy1JybY6b9lqO2WIfniQN6Mj5+1bmibTzsXkuqos41c= -go.viam.com/api v0.1.473/go.mod h1:p/am76zx8SZ74V/F4rEAYQIpHaaLUwJgY2q3Uw3FIWk= +go.viam.com/api v0.1.495 h1:AbNn4rlPDddNTwtEUef2ed0pihs/9gmr1//F2TQAcHg= +go.viam.com/api v0.1.495/go.mod h1:p/am76zx8SZ74V/F4rEAYQIpHaaLUwJgY2q3Uw3FIWk= go.viam.com/rdk v0.90.0 h1:c0QceDFhfd9+bZEh/prU7biIAhXNdotHZiFTipA8t3E= go.viam.com/rdk v0.90.0/go.mod h1:suncnUTf6T+EHkntDx+p9beHTkxtN9EPO+z9948Eg+w= go.viam.com/test v1.2.4 h1:JYgZhsuGAQ8sL9jWkziAXN9VJJiKbjoi9BsO33TW3ug= diff --git a/package.json b/package.json index ca8955da..ca549c30 100644 --- a/package.json +++ b/package.json @@ -19,6 +19,7 @@ "test": "pnpm test:unit -- --run", "test:coverage": "npx vitest run --coverage", "test:e2e": "playwright test", + "test:e2e-ui": "playwright test --ui", "model-pipeline:run": "node scripts/model-pipeline.js", "release": "changeset publish" }, @@ -26,6 +27,7 @@ "@ag-grid-community/client-side-row-model": "32.3.9", "@ag-grid-community/core": "32.3.9", "@ag-grid-community/styles": "32.3.9", + "@bufbuild/protobuf": "1.10.1", "@changesets/cli": "2.29.6", "@dimforge/rapier3d-compat": "0.18.2", "@eslint/compat": "1.3.2", @@ -84,6 +86,7 @@ "type-fest": "^5.0.1", "typescript": "5.9.2", "typescript-eslint": "8.42.0", + "uuid-tool": "^2.0.3", "vite": "7.1.11", "vite-plugin-devtools-json": "1.0.0", "vite-plugin-mkcert": "1.17.8", diff --git a/pnpm-lock.yaml b/pnpm-lock.yaml index e99d51cd..cfee4d59 100644 --- a/pnpm-lock.yaml +++ b/pnpm-lock.yaml @@ -21,6 +21,9 @@ importers: '@ag-grid-community/styles': specifier: 32.3.9 version: 32.3.9 + '@bufbuild/protobuf': + specifier: 1.10.1 + version: 1.10.1 '@changesets/cli': specifier: 2.29.6 version: 2.29.6(@types/node@24.10.0) @@ -195,6 +198,9 @@ importers: typescript-eslint: specifier: 8.42.0 version: 8.42.0(eslint@9.34.0(jiti@2.6.1))(typescript@5.9.2) + uuid-tool: + specifier: ^2.0.3 + version: 2.0.3 vite: specifier: 7.1.11 version: 7.1.11(@types/node@24.10.0)(jiti@2.6.1)(lightningcss@1.30.1)(tsx@4.20.5)(yaml@2.8.1) @@ -3617,6 +3623,9 @@ packages: util-deprecate@1.0.2: resolution: {integrity: sha512-EPD5q1uXyFxJpCrLnCc1nHnq3gOa6DZBocAIiI2TaSCA7VCJ1UJDMagCzIkXNsUYfD1daK//LTEQ8xiIbrHtcw==} + uuid-tool@2.0.3: + resolution: {integrity: sha512-En9cTJm+bLgjjfoIVEW91f0Irr0HZ9mqzK0BwntzZzOg82bOUEIzIw8pNwvC+dhVnnJ8x+fQV8TQ+q6te1ximA==} + uuid@11.1.0: resolution: {integrity: sha512-0/A9rDy9P7cJ+8w1c9WD9V//9Wj15Ce2MPz8Ri6032usz+NfePxx5AcN3bN+r6ZL6jEo066/yNYB3tn4pQEx+A==} hasBin: true @@ -7445,6 +7454,8 @@ snapshots: util-deprecate@1.0.2: {} + uuid-tool@2.0.3: {} + uuid@11.1.0: {} vite-node@3.2.4(@types/node@24.10.0)(jiti@2.6.1)(lightningcss@1.30.1)(tsx@4.20.5)(yaml@2.8.1): diff --git a/protos/draw/v1/drawing.proto b/protos/draw/v1/drawing.proto new file mode 100644 index 00000000..fde38de1 --- /dev/null +++ b/protos/draw/v1/drawing.proto @@ -0,0 +1,132 @@ +syntax = "proto3"; + +package viam.motion_tools.draw.v1; + +import "common/v1/common.proto"; +import "draw/v1/metadata.proto"; +import "google/protobuf/struct.proto"; + +option go_package = "github.com/viam-labs/motion-tools/gen/draw/v1;drawv1"; + +// Arrows represents a set of arrows in 3D space +// Metadata: +// - colors: []uint8 of a single color: [r, g, b, a] or a color per arrow: [r, g, b, a, ...], defaults to [0, 255, 0, 180] (green) +message Arrows { + // The poses of the arrows + // float32 array of poses: [x, y, z, ox, oy, oz] + bytes poses = 1; +} + +// Line represents a line in 3D space +// Metadata: +// - colors: []uint8 of a single color: [r, g, b, a] or two colors for lines and points: [r, g, b, a, r, g, b, a] +// defaults to [0, 128, 255, 180] (blue) for lines, [0, 77, 204, 180] (darker blue) for points +message Line { + // The positions of the line points + // float32 array of positions: [x, y, z, ...] + bytes positions = 1; + + // optional width of the line in millimeters, defaults to 5 + optional float line_width = 2; + + // optional size of the points in millimeters, defaults to 10 + optional float point_size = 3; +} + +// Points represents a set of points in 3D space +// Metadata: +// - colors: []uint8 of a single color: [r, g, b, a] or a color per point: [r, g, b, a, ...], defaults to [51, 51, 51, 180] (gray) +message Points { + // The positions of the points + // float32 array of positions: [x, y, z, ...] + bytes positions = 1; + + // Size of the points in millimeters, defaults to 10 + optional float point_size = 2; +} + +// ModelAsset represents the type of model +message ModelAsset { + // The mime type of the model asset + string mime_type = 1; + + // Expected file size in bytes (helpful for progress tracking) + optional uint64 size_bytes = 2; + + // The content of the model asset + oneof content { + // The url of the model asset + string url = 3; + // The binary data of the model asset + bytes binary = 4; + } +} + +// Model represents a 3D model in various formats +// Metadata: +// - colors: defaults to []uint8 of no colors +message Model { + // The list of assets that make up the model + repeated ModelAsset assets = 1; + + // Uniform scale factor, defaults to [1.0, 1.0, 1.0] + optional viam.common.v1.Vector3 scale = 2; + + // Name of the animation to play, defaults to empty string (no animation) + optional string animation_name = 3; +} + +// Nurbs represents a NURBS curve in 3D space +// Metadata: +// - colors: []uint8 of a single color: [r, g, b, a], defaults to [0, 255, 255, 180] (cyan) +message Nurbs { + // The control points of the NURBS + // []float32 of poses: [x, y, z, ox, oy, oz, theta, ...] + bytes control_points = 1; + + // The knots of the NURBS + // float32 array of knots: [knot, ...] + bytes knots = 2; + + // The degree of the NURBS, defaults to 3 + optional int32 degree = 3; + + // The weights of the NURBS, defaults to [1, 1, 1, ...] + // float32 array of weights: [weight, ...] + optional bytes weights = 4; +} + +message Shape { + // The center of the shape + viam.common.v1.Pose center = 1; + + // The label of the shape + string label = 2; + + // The type of the shape + oneof geometry_type { + Arrows arrows = 3; + Line line = 4; + Points points = 5; + Model model = 6; + Nurbs nurbs = 7; + } +} + +// For non-physical visualization shapes +message Drawing { + // The name of the shape + string reference_frame = 1; + + // The pose of the shape as observed through it's parent frame/the world + viam.common.v1.PoseInFrame pose_in_observer_frame = 2; + + // The shape of the drawing + optional Shape physical_object = 3; + + // The UUID of the shape + bytes uuid = 4; + + // The metadata of the shape + viam.motion_tools.draw.v1.Metadata metadata = 5; +} diff --git a/protos/draw/v1/metadata.proto b/protos/draw/v1/metadata.proto new file mode 100644 index 00000000..098af3fa --- /dev/null +++ b/protos/draw/v1/metadata.proto @@ -0,0 +1,12 @@ +syntax = "proto3"; + +package viam.motion_tools.draw.v1; + +option go_package = "github.com/viam-labs/motion-tools/gen/draw/v1;drawv1"; + +message Metadata { + // Float32Array of colors: [r, g, b, a, ...] + // For a single shape/geometry, this is a single color of [r, g, b, a] + // For complex shapes/geometries, this is an array of colors of [r, g, b, a, ...] + optional bytes colors = 1; +} diff --git a/protos/draw/v1/scene.proto b/protos/draw/v1/scene.proto new file mode 100644 index 00000000..9e3f17c8 --- /dev/null +++ b/protos/draw/v1/scene.proto @@ -0,0 +1,98 @@ +syntax = "proto3"; + +package viam.motion_tools.draw.v1; + +import "common/v1/common.proto"; + +option go_package = "github.com/viam-labs/motion-tools/gen/draw/v1;drawv1"; + +message OrthographicCamera { + // for orthographic camera props +} + +message PerspectiveCamera { + // for perspective camera props +} + +message SceneCamera { + // The position of the camera in millimeters + // defaults to [0, 0, 3000] + viam.common.v1.Vector3 position = 1; + + // The look at point of the camera in millimeters + // defaults to [0, 0, 0] + viam.common.v1.Vector3 look_at = 2; + + // Whether to animate camera movement + // defaults to false + optional bool animated = 3; + + // The camera type + // defaults to perspective_camera + oneof camera_type { + PerspectiveCamera perspective_camera = 4; + OrthographicCamera orthographic_camera = 5; + } +} + +enum RenderArmModels { + RENDER_ARM_MODELS_UNSPECIFIED = 0; + RENDER_ARM_MODELS_COLLIDERS = 1; + RENDER_ARM_MODELS_COLLIDERS_AND_MODEL = 2; + RENDER_ARM_MODELS_MODEL = 3; +} + +enum RenderShapes { + RENDER_SHAPES_UNSPECIFIED = 0; + RENDER_SHAPES_ARROWS = 1; + RENDER_SHAPES_LINES = 2; + RENDER_SHAPES_POINTS = 3; + RENDER_SHAPES_MODEL = 4; + RENDER_SHAPES_NURBS = 5; +} + +message SceneMetadata { + // The camera to use for the scene + // defaults to a perspective_camera with a position of [3000, 3000, 3000] and a look_at of [0, 0, 0] + optional SceneCamera scene_camera = 1; + + // Whether to render the grid + // defaults to true + optional bool grid = 2; + + // The size of the grid cells + // defaults to 500 (millimeters) + optional float grid_cell_size = 3; + + // The size of the grid sections + // defaults to 10000 (millimeters) + optional float grid_section_size = 4; + + // The fade distance of the grid + // defaults to 25000 (millimeters) + optional float grid_fade_distance = 5; + + // The size of points if not defined by a transform's metadata + // defaults to 10 (millimeters) + optional float point_size = 6; + + // The color of the points if not defined by a transform's metadata + // defaults to [51, 51, 51, 255] (gray) + optional bytes point_color = 7; + + // The width of the lines if not defined by a transform's metadata + // defaults to 5 (millimeters) + optional float line_width = 8; + + // The size of the dots of the lines if not defined by a transform's metadata + // defaults to 10 (millimeters) + optional float line_point_size = 9; + + // The models to render + // defaults to COLLIDERS_AND_MODEL + optional RenderArmModels render_arm_models = 10; + + // The shapes to render + // defaults to [ARROWS, POINTS, LINES, MODEL, NURBS] + repeated RenderShapes render_shapes = 11; +} diff --git a/protos/draw/v1/snapshot.proto b/protos/draw/v1/snapshot.proto new file mode 100644 index 00000000..c03b78ad --- /dev/null +++ b/protos/draw/v1/snapshot.proto @@ -0,0 +1,23 @@ +syntax = "proto3"; + +package viam.motion_tools.draw.v1; + +import "common/v1/common.proto"; +import "draw/v1/drawing.proto"; +import "draw/v1/scene.proto"; + +option go_package = "github.com/viam-labs/motion-tools/gen/draw/v1;drawv1"; + +message Snapshot { + // the list of transforms and shapes in the scene to render + repeated viam.common.v1.Transform transforms = 1; + + // the list of drawings in the scene to render + repeated viam.motion_tools.draw.v1.Drawing drawings = 2; + + // a unique ID for the snapshot, to inform the visualizer if a new snapshot is being rendered + bytes uuid = 3; + + // optional metadata for the scene, matches the visualizer's "settings" + optional viam.motion_tools.draw.v1.SceneMetadata scene_metadata = 4; +} diff --git a/protos/draw/v1/transforms.proto b/protos/draw/v1/transforms.proto new file mode 100644 index 00000000..ec8160db --- /dev/null +++ b/protos/draw/v1/transforms.proto @@ -0,0 +1,13 @@ +syntax = "proto3"; + +package viam.motion_tools.draw.v1; + +import "common/v1/common.proto"; + +option go_package = "github.com/viam-labs/motion-tools/gen/draw/v1;drawv1"; + +// Transforms is a list of transforms +// TODO: Here for backwards compatibility for the draw API, remove in a future release +message Transforms { + repeated viam.common.v1.Transform transforms = 1; +} diff --git a/src/hooks.client.ts b/src/hooks.client.ts index 8e68c03e..53788117 100644 --- a/src/hooks.client.ts +++ b/src/hooks.client.ts @@ -1,22 +1,27 @@ import { handleErrorWithSentry, replayIntegration } from '@sentry/sveltekit' import * as Sentry from '@sentry/sveltekit' +import { noop } from 'lodash-es' -Sentry.init({ - dsn: 'https://221c5ddd7e532dad95be66b8b6fabf2d@o1356192.ingest.us.sentry.io/4509599892897792', +export let handleError: ReturnType = () => noop() - tracesSampleRate: 1.0, +if (import.meta.env.MODE === 'production') { + Sentry.init({ + dsn: 'https://221c5ddd7e532dad95be66b8b6fabf2d@o1356192.ingest.us.sentry.io/4509599892897792', - // This sets the sample rate to be 10%. You may want this to be 100% while - // in development and sample at a lower rate in production - replaysSessionSampleRate: 0.1, + tracesSampleRate: 1.0, - // If the entire session is not sampled, use the below sample rate to sample - // sessions when an error occurs. - replaysOnErrorSampleRate: 1.0, + // This sets the sample rate to be 10%. You may want this to be 100% while + // in development and sample at a lower rate in production + replaysSessionSampleRate: 0.1, - // If you don't want to use Session Replay, just remove the line below: - integrations: [replayIntegration()], -}) + // If the entire session is not sampled, use the below sample rate to sample + // sessions when an error occurs. + replaysOnErrorSampleRate: 1.0, -// If you have a custom error handler, pass it to `handleErrorWithSentry` -export const handleError = handleErrorWithSentry() + // If you don't want to use Session Replay, just remove the line below: + integrations: [replayIntegration()], + }) + + // If you have a custom error handler, pass it to `handleErrorWithSentry` + handleError = handleErrorWithSentry() +} diff --git a/src/hooks.server.ts b/src/hooks.server.ts index 031293d4..7c2cfcb9 100644 --- a/src/hooks.server.ts +++ b/src/hooks.server.ts @@ -1,18 +1,24 @@ import { sequence } from '@sveltejs/kit/hooks' import { handleErrorWithSentry, sentryHandle } from '@sentry/sveltekit' import * as Sentry from '@sentry/sveltekit' +import { noop } from 'lodash-es' -Sentry.init({ - dsn: 'https://221c5ddd7e532dad95be66b8b6fabf2d@o1356192.ingest.us.sentry.io/4509599892897792', +// If you have custom handlers, make sure to place them after `sentryHandle()` in the `sequence` function. +export let handle: ReturnType = async ({ event, resolve }) => resolve(event) +export let handleError: ReturnType = () => noop() - tracesSampleRate: 1.0, +if (import.meta.env.MODE === 'production') { + Sentry.init({ + dsn: 'https://221c5ddd7e532dad95be66b8b6fabf2d@o1356192.ingest.us.sentry.io/4509599892897792', - // uncomment the line below to enable Spotlight (https://spotlightjs.com) - // spotlight: import.meta.env.DEV, -}) + tracesSampleRate: 1.0, -// If you have custom handlers, make sure to place them after `sentryHandle()` in the `sequence` function. -export const handle = sequence(sentryHandle()) + // uncomment the line below to enable Spotlight (https://spotlightjs.com) + // spotlight: import.meta.env.DEV, + }) + + handle = sequence(sentryHandle()) -// If you have a custom error handler, pass it to `handleErrorWithSentry` -export const handleError = handleErrorWithSentry() + // If you have a custom error handler, pass it to `handleErrorWithSentry` + handleError = handleErrorWithSentry() +} diff --git a/src/lib/FrameConfigUpdater.svelte.ts b/src/lib/FrameConfigUpdater.svelte.ts index 44856cac..74aafe8d 100644 --- a/src/lib/FrameConfigUpdater.svelte.ts +++ b/src/lib/FrameConfigUpdater.svelte.ts @@ -1,8 +1,9 @@ import type { Frame } from './frame' import type { WorldObject } from './lib' -import { createPose } from './transform' +import { createPose, matrixToPose, poseToMatrix } from './transform' import type { Geometries } from './WorldObject.svelte' import type { Pose } from '@viamrobotics/sdk' +import { Matrix4 } from 'three' type UpdateFrameCallback = { (componentName: string, referenceFrame: string, pose: Pose, geometry?: Frame['geometry']): void @@ -12,22 +13,29 @@ type RemoveFrameCallback = { (componentName: string): void } +type GetWorldMatrixCallback = { + (frameName: string): Matrix4 +} + export class FrameConfigUpdater { private object: () => WorldObject | undefined private referenceFrame: () => string private updateFrame: UpdateFrameCallback private removeFrame: RemoveFrameCallback + private getWorldMatrix: GetWorldMatrixCallback constructor( object: () => WorldObject | undefined, updateFrame: UpdateFrameCallback, removeFrame: RemoveFrameCallback, - referenceFrame: () => string + referenceFrame: () => string, + getWorldMatrix: GetWorldMatrixCallback ) { this.referenceFrame = referenceFrame this.object = object this.updateFrame = updateFrame this.removeFrame = removeFrame + this.getWorldMatrix = getWorldMatrix } public updateLocalPosition = ({ x, y, z }: { x?: number; y?: number; z?: number }) => { @@ -150,7 +158,21 @@ export class FrameConfigUpdater { public setFrameParent = (parentName: string) => { const object = this.object() if (!object) return - this.updateFrame(object.name ?? '', parentName, createPose(object.localEditedPose)) + + const currentParentName = this.referenceFrame() + const currentLocalPose = object.localEditedPose + const currentLocalMatrix = poseToMatrix(createPose(currentLocalPose)) + + const currentParentMatrix = this.getWorldMatrix(currentParentName) + const currentWorldMatrix = currentParentMatrix.clone().multiply(currentLocalMatrix) + + const newParentMatrix = this.getWorldMatrix(parentName) + const newParentMatrixInverse = newParentMatrix.clone().invert() + + const newLocalMatrix = newParentMatrixInverse.multiply(currentWorldMatrix) + const newLocalPose = matrixToPose(newLocalMatrix) + + this.updateFrame(object.name ?? '', parentName, newLocalPose) } public deleteFrame = () => { diff --git a/src/lib/WorldObject.svelte.ts b/src/lib/WorldObject.svelte.ts index 5b13eb77..17c88dc9 100644 --- a/src/lib/WorldObject.svelte.ts +++ b/src/lib/WorldObject.svelte.ts @@ -1,18 +1,31 @@ -import type { Geometry, PlainMessage, Pose, Struct, TransformWithUUID } from '@viamrobotics/sdk' +import type { Geometry, Pose, Struct, TransformWithUUID } from '@viamrobotics/sdk' import { BatchedMesh, Color, MathUtils, Object3D, Vector3, type BufferGeometry } from 'three' import { createPose, matrixToPose, poseToMatrix } from './transform' import type { ValueOf } from 'type-fest' -import { isColorRepresentation, isRGB, parseColor, parseOpacity, parseRGB } from './color' +import { + isRGB, + parseColor, + parseOpacity, + parseRGB, + parseRGBABuffer, + parseBase64RGBABuffer, + normalizeColorValue, +} from './color' import type { OBB } from 'three/addons/math/OBB.js' +import type { Drawing, Shape } from './gen/draw/v1/drawing_pb' +import type { PlainMessage } from '@bufbuild/protobuf' +import { UuidTool } from 'uuid-tool' -export type PointsGeometry = { +// TODO: here for backwards compatibility, remove when moving to new draw API +export type LegacyPointsGeometry = { center: undefined - geometryType: { case: 'points'; value: Float32Array } + geometryType: { case: 'legacyPoints'; value: Float32Array } } -export type LinesGeometry = { +// TODO: here for backwards compatibility, remove when moving to new draw API +export type LegacyLinesGeometry = { center: undefined - geometryType: { case: 'line'; value: Float32Array } + geometryType: { case: 'legacyLines'; value: Float32Array } } export type ThreeBufferGeometry = { @@ -20,7 +33,12 @@ export type ThreeBufferGeometry = { geometryType: { case: 'bufferGeometry'; value: BufferGeometry } } -export type Geometries = Geometry | PointsGeometry | LinesGeometry | ThreeBufferGeometry +export type Geometries = + | Geometry + | PlainMessage + | LegacyPointsGeometry + | LegacyLinesGeometry + | ThreeBufferGeometry export const SupportedShapes = { points: 'points', @@ -28,8 +46,25 @@ export const SupportedShapes = { arrow: 'arrow', } as const +const SUPPORTED_SHAPES_KEYS = Object.keys(SupportedShapes) as (keyof typeof SupportedShapes)[] + +export interface ShapeMetadata { + type: ValueOf | '' +} + +export const COLOR_SCHEMA = ['r', 'g', 'b'] as const +export const POSE_SCHEMA = ['x', 'y', 'z', 'ox', 'oy', 'oz'] as const + +export interface ArrowShapeMetadata extends ShapeMetadata { + type: 'arrow' + poseCount: number + poses: Uint8Array + colorCount: number + colors: Uint8Array +} + export type Metadata = { - colors?: Float32Array + colors?: Uint8Array | Float32Array color?: Color opacity?: number gltf?: { scene: Object3D } @@ -42,6 +77,8 @@ export type Metadata = { object: BatchedMesh } shape?: ValueOf + // TODO: Add other shape metadata types here + shapeMetadata?: ArrowShapeMetadata getBoundingBoxAt?: (box: OBB) => void } @@ -56,12 +93,41 @@ const METADATA_KEYS = [ 'lineDotColor', 'batched', 'shape', + 'shapeMetadata', ] as const -export const isMetadataKey = (key: string): key is keyof Metadata => { +const isMetadataKey = (key: string): key is keyof Metadata => { return METADATA_KEYS.includes(key as (typeof METADATA_KEYS)[number]) } +const isShapeMetadata = (value: unknown): value is ShapeMetadata => { + return ( + typeof value === 'object' && + value !== null && + 'type' in value && + SUPPORTED_SHAPES_KEYS.includes(value.type as (typeof SUPPORTED_SHAPES_KEYS)[number]) + ) +} + +const isArrowMetadata = (value: unknown): value is ArrowShapeMetadata => { + if (!isShapeMetadata(value)) return false + if (value.type !== 'arrow') return false + if (!('poseCount' in value)) return false + if (typeof value.poseCount !== 'number') return false + if (!('poses' in value)) return false + if (!(value.poses instanceof Uint8Array)) return false + // Each pose is 6 float32 values (x, y, z, ox, oy, oz) = 6 * 4 bytes + if (value.poses.length !== value.poseCount * POSE_SCHEMA.length * 4) return false + if (!('colorCount' in value)) return false + if (typeof value.colorCount !== 'number') return false + if (!('colors' in value)) return false + if (!(value.colors instanceof Uint8Array)) return false + // Each color is 3 float32 values (r, g, b) = 3 * 4 bytes + if (value.colors.length !== value.colorCount * COLOR_SCHEMA.length * 4) return false + + return true +} + export class WorldObject { uuid = MathUtils.generateUUID() name = '' @@ -136,7 +202,28 @@ const unwrapValue = (value: PlainMessage): unknown => { } } -export const parseMetadata = (fields: PlainMessage['fields'] = {}) => { +const EMPTY_SHAPE_METADATA: ShapeMetadata = { type: '' } +const parseShapeMetadata = (value: unknown) => { + if (!isShapeMetadata(value)) return EMPTY_SHAPE_METADATA + switch (value.type) { + case 'arrow': + if ('poses' in value && typeof value.poses === 'string') { + value.poses = Uint8Array.fromBase64(value.poses) + } + + if ('colors' in value && typeof value.colors === 'string') { + value.colors = Uint8Array.fromBase64(value.colors) + } + + if (!isArrowMetadata(value)) return EMPTY_SHAPE_METADATA + + return value as ArrowShapeMetadata + default: + return EMPTY_SHAPE_METADATA + } +} + +const parseMetadata = (fields: PlainMessage['fields'] = {}) => { const json: Metadata = {} for (const [k, v] of Object.entries(fields)) { @@ -146,8 +233,26 @@ export const parseMetadata = (fields: PlainMessage['fields'] = {}) => { switch (k) { case 'color': case 'lineDotColor': - json[k] = readColor(unwrappedValue) + json[k] = isRGB(unwrappedValue) ? parseRGB(unwrappedValue) : parseColor(unwrappedValue) break + case 'colors': { + let colorsArray: Uint8Array | undefined + if (typeof unwrappedValue === 'string') { + colorsArray = parseBase64RGBABuffer(unwrappedValue) as Uint8Array + } else if (unwrappedValue instanceof Uint8Array) { + colorsArray = parseRGBABuffer(unwrappedValue) as Uint8Array + } + if (colorsArray && colorsArray.length >= 4) { + json[k] = colorsArray + json.color = new Color().setRGB( + normalizeColorValue(colorsArray[0]), + normalizeColorValue(colorsArray[1]), + normalizeColorValue(colorsArray[2]) + ) + json.opacity = normalizeColorValue(colorsArray[3]) + } + break + } case 'opacity': json[k] = parseOpacity(unwrappedValue) break @@ -169,21 +274,17 @@ export const parseMetadata = (fields: PlainMessage['fields'] = {}) => { case 'shape': json[k] = unwrappedValue as ValueOf break + case 'shapeMetadata': + json[k] = parseShapeMetadata(unwrappedValue) as ArrowShapeMetadata + break } } return json } -const readColor = (color: unknown): Color => { - if (isColorRepresentation(color)) return parseColor(color) - if (isRGB(color)) return parseRGB(color) - return new Color('black') -} - export const fromTransform = (transform: TransformWithUUID) => { const metadata: Metadata = transform.metadata ? parseMetadata(transform.metadata.fields) : {} - const worldObject = new WorldObject( transform.referenceFrame, transform.poseInObserverFrame?.pose, @@ -191,7 +292,37 @@ export const fromTransform = (transform: TransformWithUUID) => { transform.physicalObject, metadata ) - worldObject.uuid = transform.uuidString + + if (transform.uuidString) { + worldObject.uuid = transform.uuidString + } + + return worldObject +} + +export type DrawingWithUUID = PlainMessage & { uuidString: string } +export const drawingWithUUID = (drawing: PlainMessage) => { + return { + ...drawing, + uuidString: UuidTool.toString([...drawing.uuid]), + } +} + +export const fromDrawing = (drawing: DrawingWithUUID) => { + const colors = parseRGBABuffer(drawing.metadata?.colors ?? new Uint8Array()) as Uint8Array + const metadata: Metadata = { colors: colors } + const worldObject = new WorldObject( + drawing.referenceFrame, + drawing.poseInObserverFrame?.pose, + drawing.poseInObserverFrame?.referenceFrame, + drawing.physicalObject, + metadata + ) + + if (drawing.uuidString) { + worldObject.uuid = drawing.uuidString + } + return worldObject } diff --git a/src/lib/arrows.ts b/src/lib/arrows.ts new file mode 100644 index 00000000..348e52c3 --- /dev/null +++ b/src/lib/arrows.ts @@ -0,0 +1,74 @@ +import { BufferDataType, parseBuffer } from './buffer-metadata' +import type { WorldObject } from './WorldObject.svelte' +import { RGBA_FIELDS, type RGBA } from './color' +import type { TypedArray } from 'three' +import type { ArrowsGeometry } from './shape' + +export const ARROWS_POSE_FIELDS = ['x', 'y', 'z', 'ox', 'oy', 'oz'] +export const ARROWS_POSE_SIZE = [4, 4, 4, 4, 4, 4] +export const ARROWS_POSE_TYPE = [ + BufferDataType.FLOAT, + BufferDataType.FLOAT, + BufferDataType.FLOAT, + BufferDataType.FLOAT, + BufferDataType.FLOAT, + BufferDataType.FLOAT, +] + +export const DEFAULT_ARROW_COLOR: RGBA = [0, 255, 0, 255] // Green in uint8 format + +interface ParsedArrowsBuffer { + poseData: TypedArray + poses: number + colorData: TypedArray + colors: number +} + +export const parseArrowsBuffer = ( + geometry: ArrowsGeometry, + metadata: WorldObject['metadata'] +): ParsedArrowsBuffer => { + const { poses } = geometry.geometryType.value + if (!poses.length) { + console.warn('No poses found for arrows', { + poses: poses, + }) + + return { + poseData: new Float32Array(), + poses: 0, + colorData: new Float32Array(), + colors: 0, + } + } + + const poseDataMm = parseBuffer(poses, { + fields: ARROWS_POSE_FIELDS, + size: ARROWS_POSE_SIZE, + type: ARROWS_POSE_TYPE, + }) + + // Convert position components (x, y, z) from mm to m, keep orientation (ox, oy, oz) unchanged + const poseData = new Float32Array(poseDataMm.length) + const numPoses = poseDataMm.length / ARROWS_POSE_FIELDS.length + for (let i = 0; i < numPoses; i++) { + const idx = i * ARROWS_POSE_FIELDS.length + // Scale position (x, y, z) from mm to m + poseData[idx] = poseDataMm[idx] * 0.001 + poseData[idx + 1] = poseDataMm[idx + 1] * 0.001 + poseData[idx + 2] = poseDataMm[idx + 2] * 0.001 + // Keep orientation (ox, oy, oz) unchanged + poseData[idx + 3] = poseDataMm[idx + 3] + poseData[idx + 4] = poseDataMm[idx + 4] + poseData[idx + 5] = poseDataMm[idx + 5] + } + + const colorData = metadata?.colors ?? new Float32Array([0, 1, 0, 1]) + + return { + poseData, + colorData, + poses: numPoses, + colors: colorData.length / RGBA_FIELDS.length, + } +} diff --git a/src/lib/buffer-metadata.ts b/src/lib/buffer-metadata.ts new file mode 100644 index 00000000..f12247f0 --- /dev/null +++ b/src/lib/buffer-metadata.ts @@ -0,0 +1,119 @@ +import type { TypedArray } from 'three' + +export const BufferDataType = { + INT: 'int', + UINT: 'uint', + FLOAT: 'float', +} as const + +export type BufferDataType = (typeof BufferDataType)[keyof typeof BufferDataType] + +export type BufferMetadata = { + // Describes the dimensions and their order for each buffer, e.g. ["x", "y", "z", "ox", "oy", "oz", "theta", "r", "g", "b", "a"]. + fields: string[] + // The size of each dimension in bytes. For example, for five float32 fields this would be [4, 4, 4, 4, 4]. + size: number[] + // The data type of each dimension, corresponding to the 'fields' list. + type: BufferDataType[] + // For partial updates, this specifies the starting element offset in the client's buffer + start?: number +} + +/** + * Parse buffer data using metadata to create a typed array. + * Returns TypedArray for optimal Three.js compatibility. + * + * @param data - The raw buffer data as Uint8Array + * @param metadata - Optional metadata describing the buffer structure + * @returns TypedArray view of the data + */ +export const parseBuffer = (data: Uint8Array, metadata?: BufferMetadata): TypedArray => { + if (!metadata || metadata.type.length === 0) { + return data + } + + const firstType = metadata.type[0] + const allSameType = metadata.type.every((t) => t === firstType) + const firstSize = metadata.size[0] + const allSameSize = metadata.size.every((s) => s === firstSize) + + if (allSameType && allSameSize) { + return createTypedArray(data, firstType, firstSize) + } + + const stride = metadata.size.reduce((sum, size) => sum + size, 0) + const elementCount = data.byteLength / stride + const fieldCount = metadata.fields.length + const result = new Float32Array(elementCount * fieldCount) + + let resultIdx = 0 + for (let i = 0; i < elementCount; i++) { + let byteOffset = i * stride + for (let j = 0; j < fieldCount; j++) { + const type = metadata.type[j] + const size = metadata.size[j] + result[resultIdx++] = readValue(data, byteOffset, type, size) + byteOffset += size + } + } + + return result +} + +export const packBuffer = (data: TypedArray): Uint8Array => { + return new Uint8Array(data.buffer as ArrayBuffer, data.byteOffset, data.byteLength) +} + +const createTypedArray = (data: Uint8Array, type: BufferDataType, size: number): TypedArray => { + const { buffer, byteOffset, byteLength } = data + const needsAlignment = size > 1 && byteOffset % size !== 0 + const alignedBuffer = needsAlignment ? new Uint8Array(data).buffer : buffer + const alignedOffset = needsAlignment ? 0 : byteOffset + + switch (type) { + case BufferDataType.INT: + if (size === 1) return new Int8Array(buffer, byteOffset, byteLength) + if (size === 2) return new Int16Array(alignedBuffer, alignedOffset, byteLength / 2) + if (size === 4) return new Int32Array(alignedBuffer, alignedOffset, byteLength / 4) + break + case BufferDataType.UINT: + if (size === 1) return new Uint8Array(buffer, byteOffset, byteLength) + if (size === 2) return new Uint16Array(alignedBuffer, alignedOffset, byteLength / 2) + if (size === 4) return new Uint32Array(alignedBuffer, alignedOffset, byteLength / 4) + break + case BufferDataType.FLOAT: + if (size === 4) return new Float32Array(alignedBuffer, alignedOffset, byteLength / 4) + if (size === 8) return new Float64Array(alignedBuffer, alignedOffset, byteLength / 8) + break + } + + return new Float32Array(alignedBuffer, alignedOffset, byteLength / 4) +} + +const readValue = ( + data: Uint8Array, + offset: number, + type: BufferDataType, + size: number +): T[number] => { + const view = new DataView(data.buffer, data.byteOffset + offset, size) + + switch (type) { + case BufferDataType.INT: + if (size === 1) return view.getInt8(0) + if (size === 2) return view.getInt16(0, true) + if (size === 4) return view.getInt32(0, true) + break + case BufferDataType.UINT: + if (size === 1) return view.getUint8(0) + if (size === 2) return view.getUint16(0, true) + if (size === 4) return view.getUint32(0, true) + break + case BufferDataType.FLOAT: + if (size === 4) return view.getFloat32(0, true) + if (size === 8) return view.getFloat64(0, true) + break + } + + return 0 +} diff --git a/src/lib/color.ts b/src/lib/color.ts index 5bc0f4c4..90cfd673 100644 --- a/src/lib/color.ts +++ b/src/lib/color.ts @@ -2,6 +2,20 @@ import { Color, type ColorRepresentation, type RGB } from 'three' import twColors from 'tailwindcss/colors' import { isNumber } from 'lodash-es' import { ResourceName } from '@viamrobotics/sdk' +import { BufferDataType, parseBuffer } from './buffer-metadata' + +export const RGBA_FIELDS = ['r', 'g', 'b', 'a'] +export const RGBA_SIZE = [1, 1, 1, 1] // uint8 values (0-255) +export const RGBA_TYPE = [ + BufferDataType.UINT, + BufferDataType.UINT, + BufferDataType.UINT, + BufferDataType.UINT, +] + +export type RGBA = [number, number, number, number] + +export const normalizeColorValue = (value: number): number => value / 255 // Step 3: linear sRGB → sRGB const linearToSrgb = (x: number) => { @@ -169,3 +183,41 @@ const isColorHex = (color: unknown): color is string => { return false } + +export const parseRGBABuffer = (buffer: Uint8Array) => { + return parseBuffer(buffer, { + fields: RGBA_FIELDS, + size: RGBA_SIZE, + type: RGBA_TYPE, + }) +} + +export const parseBase64RGBABuffer = (base64Str: string) => { + const binary = atob(base64Str) + const bytes = new Uint8Array(binary.length) + for (let i = 0; i < binary.length; i++) { + bytes[i] = binary.charCodeAt(i) + } + return parseRGBABuffer(bytes) +} + +export const rgbaToHex = (rgba: number[]): string => { + const r = Math.round(rgba[0] ?? 0) + .toString(16) + .padStart(2, '0') + const g = Math.round(rgba[1] ?? 0) + .toString(16) + .padStart(2, '0') + const b = Math.round(rgba[2] ?? 0) + .toString(16) + .padStart(2, '0') + return `#${r}${g}${b}` +} + +export const hexToRGBA = (hex: string): [number, number, number, number] => { + const r = parseInt(hex.slice(1, 3), 16) + const g = parseInt(hex.slice(3, 5), 16) + const b = parseInt(hex.slice(5, 7), 16) + const a = 1 + return [r, g, b, a] +} diff --git a/src/lib/components/App.svelte b/src/lib/components/App.svelte index 0a33694a..cdef2268 100644 --- a/src/lib/components/App.svelte +++ b/src/lib/components/App.svelte @@ -13,7 +13,7 @@ import Dashboard from './dashboard/Dashboard.svelte' import { domPortal } from '$lib/portal' import { provideSettings } from '$lib/hooks/useSettings.svelte' - import FileDrop from './FileDrop.svelte' + import FileDrop from './FileDrop/FileDrop.svelte' import { provideWeblabs } from '$lib/hooks/useWeblabs.svelte' import { providePartConfig } from '$lib/hooks/usePartConfig.svelte' import { useViamClient } from '@viamrobotics/svelte-sdk' diff --git a/src/lib/components/Details.svelte b/src/lib/components/Details.svelte index 4d22340d..e218172f 100644 --- a/src/lib/components/Details.svelte +++ b/src/lib/components/Details.svelte @@ -3,7 +3,8 @@ lang="ts" > import { OrientationVector } from '$lib/three/OrientationVector' - import { Quaternion, Vector3, MathUtils } from 'three' + import { Quaternion, Vector3, MathUtils, Matrix4 } from 'three' + import { poseToMatrix, createPose } from '$lib/transform' const vec3 = new Vector3() const quaternion = new Quaternion() @@ -62,11 +63,22 @@ const draggable = useDraggable('details') + const getWorldMatrix = (frameName: string): Matrix4 => { + if (frameName === 'world') return new Matrix4() + const frame = frames.current.find((f) => f.name === frameName) + if (!frame) return new Matrix4() + + const parentMatrix = getWorldMatrix(frame.referenceFrame ?? 'world') + const localMatrix = poseToMatrix(createPose(frame.pose)) + return parentMatrix.multiply(localMatrix) + } + const detailConfigUpdater = new FrameConfigUpdater( () => object, partConfig.updateFrame, partConfig.deleteFrame, - () => referenceFrame + () => referenceFrame, + getWorldMatrix ) const setGeometryType = (type: 'none' | 'box' | 'sphere' | 'capsule') => { @@ -266,20 +278,20 @@ {#if worldPosition}
world position - (m) + (mm)
x - {worldPosition.x.toFixed(2)} + {(worldPosition.x * 1000).toFixed(2)}
y - {worldPosition.y.toFixed(2)} + {(worldPosition.y * 1000).toFixed(2)}
z - {worldPosition.z.toFixed(2)} + {(worldPosition.z * 1000).toFixed(2)}
@@ -329,7 +341,7 @@ {@const PoseAttribute = showEditFrameOptions ? MutableField : ImmutableField}
local position - (m) + (mm)
{@render PoseAttribute({ @@ -427,6 +439,7 @@ }}
dimensions (box) + (mm)
{@render GeometryAttribute({ label: 'x', @@ -459,6 +472,7 @@ }}
dimensions (capsule) + (mm)
{@render GeometryAttribute({ label: 'r', @@ -481,6 +495,7 @@ {@const { radiusMm } = object?.geometry?.geometryType.value as { radiusMm: number }}
dimensions (sphere) + (mm)
{@render GeometryAttribute({ label: 'r', @@ -504,6 +519,7 @@ {@const { dimsMm } = object.geometry.geometryType.value}
dimensions (box) + (mm)
x @@ -523,6 +539,7 @@ {@const { value } = object.geometry.geometryType}
dimensions (capsule) + (mm)
r @@ -538,6 +555,7 @@
dimensions (sphere) + (mm)
r diff --git a/src/lib/components/FileDrop.svelte b/src/lib/components/FileDrop.svelte deleted file mode 100644 index a0fc8c76..00000000 --- a/src/lib/components/FileDrop.svelte +++ /dev/null @@ -1,158 +0,0 @@ - - - - -
diff --git a/src/lib/components/FileDrop/FileDrop.svelte b/src/lib/components/FileDrop/FileDrop.svelte new file mode 100644 index 00000000..f27e6db9 --- /dev/null +++ b/src/lib/components/FileDrop/FileDrop.svelte @@ -0,0 +1,247 @@ + + + + +
diff --git a/src/lib/components/FileDrop/json.ts b/src/lib/components/FileDrop/json.ts new file mode 100644 index 00000000..0be72e27 --- /dev/null +++ b/src/lib/components/FileDrop/json.ts @@ -0,0 +1,13 @@ +export const JSON_PREFIXES = { + SNAPSHOT: 'snapshot', +} + +export const SUPPORTED_JSON_PREFIXES = [JSON_PREFIXES.SNAPSHOT] + +export const onJSONDrop = (result: string | ArrayBuffer | null | undefined) => { + if (!result || typeof result !== 'string') { + return null + } + + return JSON.parse(result) +} diff --git a/src/lib/components/FileDrop/mesh.ts b/src/lib/components/FileDrop/mesh.ts new file mode 100644 index 00000000..f81e105b --- /dev/null +++ b/src/lib/components/FileDrop/mesh.ts @@ -0,0 +1,78 @@ +import type { DropStates } from './useFileDrop.svelte' +import type { ValueOf } from 'type-fest' + +export const MESH_EXTENSIONS = { + PCD: 'pcd', + PLY: 'ply', +} + +export const SUPPORTED_MESH_EXTENSIONS = [MESH_EXTENSIONS.PCD, MESH_EXTENSIONS.PLY] + +export const onMeshDrop = ( + event: DragEvent, + setDropState: (state: DropStates) => void, + onLoad: ( + extension: ValueOf, + result: string | ArrayBuffer | null | undefined + ) => void, + onSuccess: (message: string) => void, + onError: (message: string) => void +) => { + event.preventDefault() + + if (event.dataTransfer === null) { + return + } + + let completed = 0 + + const { files } = event.dataTransfer + + for (const file of files) { + const ext = file.name.split('.').at(-1) + + if (!ext) { + onError(`Could not determine file extension.`) + continue + } + + if (!SUPPORTED_MESH_EXTENSIONS.includes(ext)) { + onError( + `Only ${SUPPORTED_MESH_EXTENSIONS.map((file) => `.${file}`).join(', ')} files are supported.` + ) + continue + } + + const reader = new FileReader() + + reader.addEventListener('loadend', () => { + completed += 1 + + if (completed === files.length) { + setDropState('inactive') + onSuccess( + `${files.length === 1 ? file.name : `${files.length} files`} loaded successfully.` + ) + } + }) + + reader.addEventListener('error', () => { + onError(`${file.name} failed to load.`) + }) + + reader.addEventListener('load', async (event) => { + const arrayBuffer = event.target?.result + + if (!arrayBuffer || typeof arrayBuffer === 'string') { + onError(`${file.name} failed to load.`) + + return + } + + onLoad(ext, arrayBuffer) + }) + + reader.readAsArrayBuffer(file) + setDropState('loading') + } +} diff --git a/src/lib/components/FileDrop/useFileDrop.svelte.ts b/src/lib/components/FileDrop/useFileDrop.svelte.ts new file mode 100644 index 00000000..52704a08 --- /dev/null +++ b/src/lib/components/FileDrop/useFileDrop.svelte.ts @@ -0,0 +1,37 @@ +export type DropStates = 'inactive' | 'hovering' | 'loading' + +export const useFileDrop = () => { + let dropState = $state('inactive') + + // prevent default to allow drop + const ondragenter = (event: DragEvent) => { + event.preventDefault() + dropState = 'hovering' + } + + // prevent default to allow drop + const ondragover = (event: DragEvent) => { + event.preventDefault() + } + + const ondragleave = (event: DragEvent) => { + // only deactivate if really leaving the window + if (event.relatedTarget === null) { + dropState = 'inactive' + } + } + + return { + get dropState() { + return dropState + }, + + set dropState(state: DropStates) { + dropState = state + }, + + ondragenter, + ondragover, + ondragleave, + } +} diff --git a/src/lib/components/Frame.svelte b/src/lib/components/Frame.svelte index 376de204..ff5d8434 100644 --- a/src/lib/components/Frame.svelte +++ b/src/lib/components/Frame.svelte @@ -12,8 +12,10 @@ import { useSelected } from '$lib/hooks/useSelection.svelte' import { useSettings } from '$lib/hooks/useSettings.svelte' import { use3DModels } from '$lib/hooks/use3DModels.svelte' - import { colors, darkenColor } from '$lib/color' + import { colors, darkenColor, normalizeColorValue } from '$lib/color' import { WEBLABS_EXPERIMENTS } from '$lib/hooks/useWeblabs.svelte' + import Shape from './Shape/Shape.svelte' + import { isShape } from '$lib/shape' interface Props { uuid: string @@ -24,7 +26,7 @@ children?: Snippet<[{ ref: Object3D }]> } - let { uuid, name, ...rest }: Props = $props() + let { uuid, name, geometry, ...rest }: Props = $props() const settings = useSettings() const componentModels = use3DModels() @@ -32,7 +34,30 @@ const weblabs = useWeblabs() const events = useObjectEvents(() => uuid) - const color = $derived(rest.metadata.color ?? colors.default) + const color = $derived.by(() => { + if (rest.metadata.colors) { + if (!rest.metadata.colors.length) { + return new Color(colors.default) + } + + if ( + rest.metadata.colors[0] === undefined || + rest.metadata.colors[1] === undefined || + rest.metadata.colors[2] === undefined + ) { + return new Color(colors.default) + } + + // Normalize uint8 (0-255) to float (0-1) for THREE.Color + return new Color( + normalizeColorValue(rest.metadata.colors[0]), + normalizeColorValue(rest.metadata.colors[1]), + normalizeColorValue(rest.metadata.colors[2]) + ) + } + + return rest.metadata.color ?? new Color(colors.default) + }) const model = $derived.by(() => { if (!weblabs.isActive(WEBLABS_EXPERIMENTS.MOTION_TOOLS_RENDER_ARM_MODELS)) { @@ -47,14 +72,25 @@ }) - +{#if geometry && isShape(geometry)} + +{:else} + +{/if} diff --git a/src/lib/components/Geometry.svelte b/src/lib/components/Geometry.svelte index 4b9565bc..45e47009 100644 --- a/src/lib/components/Geometry.svelte +++ b/src/lib/components/Geometry.svelte @@ -47,6 +47,7 @@ and should remain pure, i.e. no hooks should be used. const color = $derived(overrideColor ?? metadata.color ?? colors.default) const group = new Group() + const mesh = $derived.by(() => { if (type === undefined) { return @@ -130,7 +131,7 @@ and should remain pure, i.e. no hooks should be used. is={meshGeometry} {oncreate} /> - {:else if geometry.geometryType.case === 'line' && metadata.points} + {:else if geometry.geometryType.case === 'legacyLines' && metadata.points} {:else if geometry.geometryType.case === 'box'} {@const dimsMm = geometry.geometryType.value.dimsMm ?? { x: 0, y: 0, z: 0 }} @@ -154,7 +155,7 @@ and should remain pure, i.e. no hooks should be used. {/if} {/if} - {#if geometry.geometryType.case === 'line'} + {#if geometry.geometryType.case === 'legacyLines'} + object: WorldObject children?: Snippet } diff --git a/src/lib/components/SceneProviders.svelte b/src/lib/components/SceneProviders.svelte index 5b275cd5..ba5271f8 100644 --- a/src/lib/components/SceneProviders.svelte +++ b/src/lib/components/SceneProviders.svelte @@ -24,6 +24,7 @@ import { provideFramelessComponents } from '$lib/hooks/useFramelessComponents.svelte' import { provideResourceByName } from '$lib/hooks/useResourceByName.svelte' import { provide3DModels } from '$lib/hooks/use3DModels.svelte' + import { provideSnapshot } from '$lib/hooks/useSnapshot.svelte' interface Props { cameraPose?: CameraPose @@ -44,6 +45,7 @@ provideOrigin() provideStaticGeometries() provideDrawAPI() + provideSnapshot() provideResourceByName(() => partID.current) provideFrames(() => partID.current) diff --git a/src/lib/components/Shape/ArrowsShape.svelte b/src/lib/components/Shape/ArrowsShape.svelte new file mode 100644 index 00000000..a6ac0ed3 --- /dev/null +++ b/src/lib/components/Shape/ArrowsShape.svelte @@ -0,0 +1,65 @@ + + + + diff --git a/src/lib/components/Shape/LineShape.svelte b/src/lib/components/Shape/LineShape.svelte new file mode 100644 index 00000000..f17c2c5d --- /dev/null +++ b/src/lib/components/Shape/LineShape.svelte @@ -0,0 +1,106 @@ + + + +{#if positions.length > 0} + null} + bvh={{ enabled: false }} + > + + + + + + + + + {#each positions as point, i (i)} + + {/each} + +{/if} diff --git a/src/lib/components/Shape/ModelShape.svelte b/src/lib/components/Shape/ModelShape.svelte new file mode 100644 index 00000000..376c6f84 --- /dev/null +++ b/src/lib/components/Shape/ModelShape.svelte @@ -0,0 +1,127 @@ + + + +{#if scene} + +{/if} diff --git a/src/lib/components/Shape/PointsShape.svelte b/src/lib/components/Shape/PointsShape.svelte new file mode 100644 index 00000000..994b4bdf --- /dev/null +++ b/src/lib/components/Shape/PointsShape.svelte @@ -0,0 +1,125 @@ + + + + + {@render children?.()} + diff --git a/src/lib/components/Shape/Shape.svelte b/src/lib/components/Shape/Shape.svelte new file mode 100644 index 00000000..dc184032 --- /dev/null +++ b/src/lib/components/Shape/Shape.svelte @@ -0,0 +1,79 @@ + + + + + + {#if isArrows(geometry) && renderShapes.includes(RenderShapes.ARROWS)} + + {:else if isLine(geometry) && renderShapes.includes(RenderShapes.LINES)} + + {:else if isPoints(geometry) && renderShapes.includes(RenderShapes.POINTS)} + + {:else if isModel(geometry) && renderShapes.includes(RenderShapes.MODEL)} + + {/if} + + {@render children?.({ ref: group })} + diff --git a/src/lib/components/Snapshot.svelte b/src/lib/components/Snapshot.svelte new file mode 100644 index 00000000..f7d02915 --- /dev/null +++ b/src/lib/components/Snapshot.svelte @@ -0,0 +1,35 @@ + + + +{#each frames as object (object.uuid)} + + + + +{/each} + + +{#each worldObjects as object (object.uuid)} + + + + + +{/each} diff --git a/src/lib/components/Tree/TreeContainer.svelte b/src/lib/components/Tree/TreeContainer.svelte index 99db9a55..9dbe5b82 100644 --- a/src/lib/components/Tree/TreeContainer.svelte +++ b/src/lib/components/Tree/TreeContainer.svelte @@ -17,6 +17,8 @@ import { usePartConfig } from '$lib/hooks/usePartConfig.svelte' import WeblabActive from '../weblab/WeblabActive.svelte' import { WEBLABS_EXPERIMENTS } from '$lib/hooks/useWeblabs.svelte' + import { useSnapshot } from '$lib/hooks/useSnapshot.svelte' + const { ...rest } = $props() provideTreeExpandedContext() @@ -28,6 +30,7 @@ const worldStates = useWorldStates() const environment = useEnvironment() const partConfig = usePartConfig() + const snapshot = useSnapshot() let rootNode = $state({ id: 'world', @@ -36,7 +39,7 @@ href: '/', }) - const nodes = $derived(buildTreeNodes(objects.current, worldStates.current)) + const nodes = $derived(buildTreeNodes(objects.current, worldStates.current, snapshot.current)) $effect.pre(() => { if (!isEqual(rootNode.children, nodes)) { diff --git a/src/lib/components/Tree/buildTree.ts b/src/lib/components/Tree/buildTree.ts index 883d3d6c..bb72c551 100644 --- a/src/lib/components/Tree/buildTree.ts +++ b/src/lib/components/Tree/buildTree.ts @@ -1,5 +1,8 @@ import type { useWorldStates } from '$lib/hooks/useWorldState.svelte' import type { WorldObject } from '$lib/WorldObject.svelte' +import type { Snapshot } from '$lib/gen/draw/v1/snapshot_pb' +import { drawingWithUUID, fromDrawing, fromTransform } from '$lib/WorldObject.svelte' +import { transformWithUUID } from '@viamrobotics/sdk' export interface TreeNode { id: string @@ -8,12 +11,25 @@ export interface TreeNode { href: string } +// Sort all nodes naturally by name +const sortNodes = (nodes: TreeNode[]): TreeNode[] => { + return nodes + .sort((a, b) => a.name.localeCompare(b.name, undefined, { numeric: true })) + .map((node) => { + if (node.children && node.children.length > 0) { + node.children = sortNodes(node.children) + } + return node + }) +} + /** * Creates a tree representing parent child / relationships from a set of frames. */ export const buildTreeNodes = ( objects: WorldObject[], - worldStates: ReturnType['current'] + worldStates: ReturnType['current'], + snapshot?: Snapshot ): TreeNode[] => { const nodeMap = new Map() const rootNodes = [] @@ -72,5 +88,41 @@ export const buildTreeNodes = ( rootNodes.push(node) } - return rootNodes + // Add snapshot objects if available + if (snapshot) { + // Convert transforms and drawings to WorldObjects + const transforms = snapshot.transforms.map((t) => fromTransform(transformWithUUID(t))) + const drawings = snapshot.drawings.map((d) => fromDrawing(drawingWithUUID(d))) + const snapshotObjects = [...transforms, ...drawings] + + // Create nodes for all snapshot objects + for (const object of snapshotObjects) { + const node: TreeNode = { + name: object.name, + id: `snapshot-${object.uuid}`, + children: [], + href: `/snapshot/${object.name}`, + } + + nodeMap.set(object.name, node) + + // If it's a world-level object, add directly to snapshot node + if (object.referenceFrame === 'world') { + rootNodes.push(node) + } + } + + // Build hierarchy for non-world objects + for (const object of snapshotObjects) { + if (object.referenceFrame && object.referenceFrame !== 'world') { + const parentNode = nodeMap.get(object.referenceFrame) + const child = nodeMap.get(object.name) + if (parentNode && child) { + parentNode.children?.push(child) + } + } + } + } + + return sortNodes(rootNodes) } diff --git a/src/lib/components/WorldObjects.svelte b/src/lib/components/WorldObjects.svelte index 82f53ff1..d554c7c3 100644 --- a/src/lib/components/WorldObjects.svelte +++ b/src/lib/components/WorldObjects.svelte @@ -19,12 +19,16 @@ import type { WorldObject } from '$lib/WorldObject.svelte' import type { Pose as ViamPose } from '@viamrobotics/sdk' import { WEBLABS_EXPERIMENTS } from '$lib/hooks/useWeblabs.svelte' + import { useSnapshot } from '$lib/hooks/useSnapshot.svelte' + import Snapshot from './Snapshot.svelte' + const points = usePointClouds() const drawAPI = useDrawAPI() const frames = useFrames() const geometries = useGeometries() const worldStates = useWorldStates() const batchedArrow = useArrows() + const snapshot = useSnapshot() const weblabs = useWeblabs() const weblabedDeterminePose = (object: WorldObject, pose: ViamPose | undefined) => { @@ -73,6 +77,13 @@ {/each} +{#if snapshot.current} + +{/if} + {#each worldStates.names as { name } (name)} {/each} diff --git a/src/lib/hooks/useDrawAPI.svelte.ts b/src/lib/hooks/useDrawAPI.svelte.ts index 0d10edf4..8475a228 100644 --- a/src/lib/hooks/useDrawAPI.svelte.ts +++ b/src/lib/hooks/useDrawAPI.svelte.ts @@ -4,7 +4,7 @@ import type { OBB } from 'three/addons/math/OBB.js' import { NURBSCurve } from 'three/addons/curves/NURBSCurve.js' import { parsePcdInWorker } from '$lib/loaders/pcd' import { GLTFLoader } from 'three/examples/jsm/loaders/GLTFLoader.js' -import { WorldObject, type PointsGeometry } from '$lib/WorldObject.svelte' +import { WorldObject, type LegacyPointsGeometry } from '$lib/WorldObject.svelte' import { useArrows } from './useArrows.svelte' import type { Frame } from '$lib/frame' import { createGeometry } from '$lib/geometry' @@ -17,7 +17,7 @@ import { useLogs } from './useLogs.svelte' type ConnectionStatus = 'connecting' | 'open' | 'closed' interface Context { - points: WorldObject[] + points: WorldObject[] frames: WorldObject[] lines: WorldObject[] meshes: WorldObject[] @@ -27,7 +27,7 @@ interface Context { connectionStatus: ConnectionStatus - addPoints(worldObject: WorldObject): void + addPoints(worldObject: WorldObject): void addMesh(worldObject: WorldObject): void } @@ -97,7 +97,7 @@ export const provideDrawAPI = () => { let ws: WebSocket const frames = $state([]) - const points = $state[]>([]) + const points = $state[]>([]) const lines = $state([]) const meshes = $state([]) const poses = $state([]) @@ -159,7 +159,7 @@ export const provideDrawAPI = () => { { center: undefined, geometryType: { - case: 'points', + case: 'legacyPoints', value: positions, }, }, @@ -221,7 +221,7 @@ export const provideDrawAPI = () => { data.name, data.pose, data.parent, - { center: undefined, geometryType: { case: 'line', value: new Float32Array() } }, + { center: undefined, geometryType: { case: 'legacyLines', value: new Float32Array() } }, { color: new Color(color), points: curve.getPoints(200) } ) @@ -362,7 +362,7 @@ export const provideDrawAPI = () => { { center: undefined, geometryType: { - case: 'points', + case: 'legacyPoints', value: positions, }, }, @@ -415,7 +415,7 @@ export const provideDrawAPI = () => { { center: undefined, geometryType: { - case: 'line', + case: 'legacyLines', value: positions, }, }, @@ -660,7 +660,7 @@ export const provideDrawAPI = () => { get connectionStatus() { return connectionStatus }, - addPoints(worldObject: WorldObject) { + addPoints(worldObject: WorldObject) { points.push(worldObject) }, addMesh(worldObject: WorldObject) { diff --git a/src/lib/hooks/usePointclouds.svelte.ts b/src/lib/hooks/usePointclouds.svelte.ts index a1a03ead..bc99700a 100644 --- a/src/lib/hooks/usePointclouds.svelte.ts +++ b/src/lib/hooks/usePointclouds.svelte.ts @@ -7,7 +7,7 @@ import { } from '@viamrobotics/svelte-sdk' import { parsePcdInWorker } from '$lib/loaders/pcd' import { RefreshRates, useMachineSettings } from './useMachineSettings.svelte' -import { WorldObject, type PointsGeometry } from '$lib/WorldObject.svelte' +import { WorldObject, type LegacyPointsGeometry } from '$lib/WorldObject.svelte' import { usePersistentUUIDs } from './usePersistentUUIDs.svelte' import { useLogs } from './useLogs.svelte' import { RefetchRates } from '$lib/components/RefreshRate.svelte' @@ -15,7 +15,7 @@ import { RefetchRates } from '$lib/components/RefreshRate.svelte' const key = Symbol('pointcloud-context') interface Context { - current: WorldObject[] + current: WorldObject[] refetch: () => void } @@ -84,7 +84,7 @@ export const providePointclouds = (partID: () => string) => { const { updateUUIDs } = usePersistentUUIDs() - let current = $state.raw[]>([]) + let current = $state.raw[]>([]) $effect(() => { for (const [name, query] of queries) { @@ -114,12 +114,12 @@ export const providePointclouds = (partID: () => string) => { `${name}:pointcloud`, undefined, name, - { center: undefined, geometryType: { case: 'points', value: positions } }, + { center: undefined, geometryType: { case: 'legacyPoints', value: positions } }, colors ? { colors } : undefined ) }) ).then((results) => { - const worldObjects: WorldObject[] = [] + const worldObjects: WorldObject[] = [] for (const result of results) { if (result.status === 'fulfilled') { worldObjects.push(result.value) diff --git a/src/lib/hooks/useSettings.svelte.ts b/src/lib/hooks/useSettings.svelte.ts index 6be30351..c8028d03 100644 --- a/src/lib/hooks/useSettings.svelte.ts +++ b/src/lib/hooks/useSettings.svelte.ts @@ -3,6 +3,8 @@ import { getContext, setContext } from 'svelte' const key = Symbol('dashboard-context') +export type ArmModelRendering = 'colliders' | 'colliders+model' | 'model' + interface Settings { isLoaded: boolean // Camera @@ -39,7 +41,7 @@ interface Settings { enableArmPositionsWidget: boolean renderStats: boolean - renderArmModels: 'colliders' | 'colliders+model' | 'model' + renderArmModels: ArmModelRendering } interface Context { diff --git a/src/lib/hooks/useSnapshot.svelte.ts b/src/lib/hooks/useSnapshot.svelte.ts new file mode 100644 index 00000000..f242cfd8 --- /dev/null +++ b/src/lib/hooks/useSnapshot.svelte.ts @@ -0,0 +1,153 @@ +import { getContext, setContext } from 'svelte' +import { getArmModelRendering } from '$lib/snapshot' +import { RenderArmModels } from '$lib/gen/draw/v1/scene_pb' +import { Snapshot } from '$lib/gen/draw/v1/snapshot_pb' +import { useSettings } from './useSettings.svelte' +import { useCameraControls, type CameraPose } from './useControls.svelte' +import { drawingWithUUID, fromDrawing, fromTransform, WorldObject } from '$lib/WorldObject.svelte' +import { Geometry, transformWithUUID } from '@viamrobotics/sdk' +import { rgbaToHex } from '$lib/color' + +const key = Symbol('snapshot-context') + +type FrameGeometry = Geometry & { geometryType: { case: undefined; value: undefined } } + +interface Context { + current: Snapshot | undefined + frames: WorldObject[] + worldObjects: WorldObject[] +} + +export const provideSnapshot = () => { + const settings = useSettings() + const cameraControls = useCameraControls() + + let snapshot = $state.raw() + let frames = $state.raw[]>([]) + let worldObjects = $state.raw([]) + + const cameraPose = $derived.by((): CameraPose => { + const camera = snapshot?.sceneMetadata?.sceneCamera + if (!camera) return { position: [3, 3, 3], lookAt: [0, 0, 0] } + + return { + position: [ + (camera.position?.x ?? 0) * 0.001, + (camera.position?.y ?? 0) * 0.001, + (camera.position?.z ?? 0) * 0.001, + ], + lookAt: [ + (camera.lookAt?.x ?? 0) * 0.001, + (camera.lookAt?.y ?? 0) * 0.001, + (camera.lookAt?.z ?? 0) * 0.001, + ], + } + }) + + $effect(() => { + const metadata = snapshot?.sceneMetadata + if (!metadata) return + + settings.current.grid = metadata.grid ?? settings.current.grid + settings.current.gridCellSize = metadata.gridCellSize + ? metadata.gridCellSize * 0.001 + : settings.current.gridCellSize + settings.current.gridSectionSize = metadata.gridSectionSize + ? metadata.gridSectionSize * 0.001 + : settings.current.gridSectionSize + settings.current.gridFadeDistance = metadata.gridFadeDistance + ? metadata.gridFadeDistance * 0.001 + : settings.current.gridFadeDistance + + settings.current.pointSize = metadata.pointSize + ? metadata.pointSize * 0.001 + : settings.current.pointSize + settings.current.pointColor = metadata.pointColor + ? rgbaToHex(Array.from(metadata.pointColor)) + : settings.current.pointColor + + settings.current.lineWidth = metadata.lineWidth + ? metadata.lineWidth * 0.001 + : settings.current.lineWidth + settings.current.lineDotSize = metadata.linePointSize + ? metadata.linePointSize * 0.001 + : settings.current.lineDotSize + + settings.current.renderArmModels = getArmModelRendering( + metadata.renderArmModels ?? RenderArmModels.COLLIDERS_AND_MODEL + ) + }) + + $effect(() => { + if (cameraControls.current) { + cameraControls.current.setPosition( + cameraPose.position[0], + cameraPose.position[1], + cameraPose.position[2] + ) + cameraControls.current.setLookAt( + cameraPose.position[0], + cameraPose.position[1], + cameraPose.position[2], + cameraPose.lookAt[0], + cameraPose.lookAt[1], + cameraPose.lookAt[2] + ) + } + }) + + $effect(() => { + if (!snapshot) { + frames = [] + worldObjects = [] + return + } + + const nextFrames: WorldObject[] = [] + const nextWorldObjects: WorldObject[] = [] + + for (const transform of snapshot.transforms) { + const withUUID = transformWithUUID(transform) + if (!withUUID.physicalObject || withUUID.physicalObject.geometryType.case === undefined) { + nextFrames.push( + fromTransform(withUUID) as WorldObject< + Geometry & { geometryType: { case: undefined; value: undefined } } + > + ) + } else { + const worldObject = fromTransform(withUUID) + nextWorldObjects.push(worldObject) + } + } + + for (const drawing of snapshot.drawings) { + const withUUID = drawingWithUUID(drawing) + nextWorldObjects.push(fromDrawing(withUUID)) + } + + frames = nextFrames + worldObjects = nextWorldObjects + }) + + return setContext(key, { + get current() { + return snapshot + }, + + set current(value: Snapshot | undefined) { + snapshot = value + }, + + get frames() { + return frames + }, + + get worldObjects() { + return worldObjects + }, + }) +} + +export const useSnapshot = () => { + return getContext(key) +} diff --git a/src/lib/line.ts b/src/lib/line.ts new file mode 100644 index 00000000..4683a65a --- /dev/null +++ b/src/lib/line.ts @@ -0,0 +1,61 @@ +import type { WorldObject } from './WorldObject.svelte' +import type { Drawing } from './gen/draw/v1/drawing_pb' +import { RGBA_FIELDS, type RGBA } from './color' +import type { TypedArray, Vector3 } from 'three' +import { parsePoints } from './point' + +export const DEFAULT_LINE_WIDTH = 0.005 +export const DEFAULT_LINE_DOT_SIZE = 0.01 + +export const DEFAULT_LINE_COLOR: RGBA = [0, 128, 255, 255] // blue +export const DEFAULT_LINE_POINT_COLOR: RGBA = [0, 77, 204, 255] // darker blue + +interface ParsedLineBuffer { + pointsData: TypedArray + points: Vector3[] + colorData: TypedArray + hasDotColor: boolean +} + +export const parseLineBuffer = ( + drawing: Drawing, + metadata: WorldObject['metadata'] +): ParsedLineBuffer => { + const line = + drawing.physicalObject?.geometryType?.case === 'line' + ? drawing.physicalObject.geometryType.value + : null + + if (!line?.positions.length) { + console.warn('No points found for line', { + points: line?.positions.length, + }) + + return { + pointsData: new Float32Array(), + points: [], + colorData: new Float32Array(), + hasDotColor: false, + } + } + + const { pointsData, points } = parsePoints(line.positions) + + // metadata.colors should already be a Float32Array parsed in WorldObject constructor (RGBA format) + // Default to both line and point colors if no colors provided + let colorData = + metadata?.colors ?? new Float32Array([...DEFAULT_LINE_COLOR, ...DEFAULT_LINE_POINT_COLOR]) + + // Check if we have dual colors (line + dots) - now RGBA format + let hasDotColor = false + if (colorData.length === RGBA_FIELDS.length * 2) { + hasDotColor = true + } + + return { + pointsData, + points, + colorData, + hasDotColor, + } +} diff --git a/src/lib/point.ts b/src/lib/point.ts new file mode 100644 index 00000000..090bc6f6 --- /dev/null +++ b/src/lib/point.ts @@ -0,0 +1,36 @@ +import { Vector3 } from 'three' +import { BufferDataType, parseBuffer } from './buffer-metadata' +import type { RGBA } from './color' + +export const POINT_FIELDS = ['x', 'y', 'z'] +export const POINT_SIZE = [4, 4, 4] +export const POINT_TYPE = [BufferDataType.FLOAT, BufferDataType.FLOAT, BufferDataType.FLOAT] + +export const DEFAULT_POINT_COLOR: RGBA = [51, 51, 51, 255] // gray +export const DEFAULT_POINT_SIZE = 0.01 + +export const parsePoints = (data: Uint8Array) => { + const pointsDataMm = parseBuffer(data, { + fields: POINT_FIELDS, + size: POINT_SIZE, + type: POINT_TYPE, + }) + + // Convert from millimeters to meters for Three.js + const pointsData = new Float32Array(pointsDataMm.length) + for (let i = 0; i < pointsDataMm.length; i++) { + pointsData[i] = pointsDataMm[i] * 0.001 + } + + const points: Vector3[] = [] + const numPoints = pointsData.length / POINT_FIELDS.length + for (let i = 0; i < numPoints; i++) { + const idx = i * POINT_FIELDS.length + points.push(new Vector3(pointsData[idx], pointsData[idx + 1], pointsData[idx + 2])) + } + + return { + pointsData, + points, + } +} diff --git a/src/lib/shape.ts b/src/lib/shape.ts new file mode 100644 index 00000000..09c2f786 --- /dev/null +++ b/src/lib/shape.ts @@ -0,0 +1,56 @@ +import { createPose } from './transform' +import type { Nurbs, Shape as ShapeProto } from '$lib/gen/draw/v1/drawing_pb' +import type { PlainMessage } from '@bufbuild/protobuf' + +import type { Points, Line, Arrows, Model } from '$lib/gen/draw/v1/drawing_pb' +import type { Geometries } from './WorldObject.svelte' + +export type Shape = PlainMessage + +export type ArrowsGeometry = Shape & { + geometryType: { case: 'arrows'; value: PlainMessage } +} + +export type LineGeometry = Shape & { + geometryType: { case: 'line'; value: PlainMessage } +} + +export type PointsGeometry = Shape & { + geometryType: { case: 'points'; value: PlainMessage } +} + +export type ModelGeometry = Shape & { + geometryType: { case: 'model'; value: PlainMessage } +} + +export type NurbsGeometry = Shape & { + geometryType: { case: 'nurbs'; value: PlainMessage } +} + +export const isArrows = (shape?: Geometries): shape is ArrowsGeometry => { + return shape !== undefined && shape.geometryType.case === 'arrows' +} + +export const isLine = (shape?: Geometries): shape is LineGeometry => { + return shape !== undefined && shape.geometryType.case === 'line' +} + +export const isPoints = (shape?: Geometries): shape is PointsGeometry => { + return shape !== undefined && shape.geometryType.case === 'points' +} + +export const isModel = (shape?: Geometries): shape is ModelGeometry => { + return shape !== undefined && shape.geometryType.case === 'model' +} + +export const isShape = (shape?: Geometries): shape is Shape => { + return isArrows(shape) || isLine(shape) || isPoints(shape) || isModel(shape) +} + +export const createShape = (geometryType: Type, label = '') => { + return { + center: createPose(), + label, + geometryType, + } +} diff --git a/src/lib/snapshot.ts b/src/lib/snapshot.ts new file mode 100644 index 00000000..52a94015 --- /dev/null +++ b/src/lib/snapshot.ts @@ -0,0 +1,99 @@ +import type { ArmModelRendering } from './hooks/useSettings.svelte' +import { RenderArmModels } from './gen/draw/v1/scene_pb' +import { Snapshot } from './gen/draw/v1/snapshot_pb' +import { Drawing } from './gen/draw/v1/drawing_pb' +import { Transform } from './gen/common/v1/common_pb' + +export const getArmModelRendering = (model: RenderArmModels): ArmModelRendering => { + switch (model) { + case RenderArmModels.COLLIDERS: + return 'colliders' + case RenderArmModels.COLLIDERS_AND_MODEL: + return 'colliders+model' + case RenderArmModels.MODEL: + return 'model' + default: + return 'colliders+model' + } +} + +/** Decodes a Snapshot from JSON format, throws an error if the JSON is invalid or the snapshot is missing required fields. + * @param json - The JSON object to decode. + * @returns The decoded Snapshot. + * @throws An error if the JSON is invalid or the snapshot is missing required fields. + */ +export const decodeSnapshotFromJSON = (json: unknown): Snapshot => { + if (!json || typeof json !== 'object') { + throw new Error('Invalid JSON: expected an object') + } + + const data = json as Record + if (!data.transforms || !Array.isArray(data.transforms)) { + throw new Error('Invalid snapshot: missing transforms array') + } + + if (!data.drawings || !Array.isArray(data.drawings)) { + throw new Error('Invalid snapshot: missing drawings array') + } + + if (!data.uuid || typeof data.uuid !== 'string') { + throw new Error('Invalid snapshot: missing or invalid UUID') + } + + if (!data.sceneMetadata || typeof data.sceneMetadata !== 'object') { + throw new Error('Invalid snapshot: missing or invalid scene metadata') + } + + const uuid = Uint8Array.fromBase64(data.uuid) + const transforms: Snapshot['transforms'] = data.transforms.map((transformJson: any) => { + try { + const parsed = Transform.fromJson(transformJson) + return parsed + } catch (error) { + console.error('Failed to parse transform', error, transformJson) + throw error + } + }) + + const drawings: Snapshot['drawings'] = data.drawings.map((drawingJson: any) => { + try { + const parsed = Drawing.fromJson(drawingJson) + return parsed + } catch (error) { + console.error('Failed to parse drawing', error, drawingJson) + throw error + } + }) + + return new Snapshot({ + transforms, + drawings, + sceneMetadata: data.sceneMetadata as Snapshot['sceneMetadata'] | undefined, + uuid, + }) +} + +/** Decodes a Snapshot from binary protobuf format. + * @param bytes - The binary protobuf data as Uint8Array. + * @returns The decoded Snapshot. + * @throws An error if the binary data is invalid. + */ +export const decodeSnapshotFromBinary = (bytes: Uint8Array): Snapshot => { + return Snapshot.fromBinary(bytes) +} + +/** Decodes a Snapshot from gzip-compressed binary protobuf format. + * Uses the DecompressionStream API available in modern browsers. + * @param compressed - The gzip-compressed binary data as Uint8Array. + * @returns A Promise that resolves to the decoded Snapshot. + * @throws An error if decompression or decoding fails. + */ +export const decodeSnapshotFromGzip = async (compressed: Uint8Array): Promise => { + const ds = new DecompressionStream('gzip') + const blob = new Blob([compressed.buffer as ArrayBuffer]) + const decompressedStream = blob.stream().pipeThrough(ds) + const decompressedBlob = await new Response(decompressedStream).blob() + const decompressedBuffer = await decompressedBlob.arrayBuffer() + const decompressedBytes = new Uint8Array(decompressedBuffer) + return decodeSnapshotFromBinary(decompressedBytes) +} diff --git a/static/models/Avocado.glb b/static/models/Avocado.glb new file mode 100644 index 00000000..dd79cb80 Binary files /dev/null and b/static/models/Avocado.glb differ diff --git a/static/models/Duck.glb b/static/models/Duck.glb new file mode 100644 index 00000000..217170d2 Binary files /dev/null and b/static/models/Duck.glb differ diff --git a/static/models/Lantern.glb b/static/models/Lantern.glb new file mode 100644 index 00000000..0c97893a Binary files /dev/null and b/static/models/Lantern.glb differ diff --git a/tools/tools.go b/tools/tools.go new file mode 100644 index 00000000..3e8d614f --- /dev/null +++ b/tools/tools.go @@ -0,0 +1,9 @@ +//go:build tools +// +build tools + +// Package tools tracks dependencies for build tools +package tools + +import ( + _ "google.golang.org/protobuf/cmd/protoc-gen-go" +)