diff --git a/spatialmath/mesh.go b/spatialmath/mesh.go index d12a7475597..1537c23e19b 100644 --- a/spatialmath/mesh.go +++ b/spatialmath/mesh.go @@ -12,6 +12,7 @@ import ( "github.com/pkg/errors" "github.com/spf13/cast" commonpb "go.viam.com/api/common/v1" + "google.golang.org/protobuf/encoding/protojson" "go.viam.com/rdk/utils" ) @@ -441,9 +442,28 @@ func (m *Mesh) ToPoints(density float64) []r3.Vector { return points } +// UnmarshalJSON implements the json.Unmarshaler interface. +func (m *Mesh) UnmarshalJSON(data []byte) error { + var g commonpb.Geometry + if err := protojson.Unmarshal(data, &g); err != nil { + return err + } + if _, ok := g.GeometryType.(*commonpb.Geometry_Mesh); !ok { + return errors.New("geometry is not a mesh") + } + + pose := NewPoseFromProtobuf(g.GetCenter()) + mesh, err := NewMeshFromProto(pose, g.GetMesh(), g.Label) + if err != nil { + return err + } + *m = *mesh + return nil +} + // MarshalJSON implements the json.Marshaler interface. -func (m *Mesh) MarshalJSON() ([]byte, error) { - return nil, errors.New("MarshalJSON not yet implemented for Mesh") +func (m Mesh) MarshalJSON() ([]byte, error) { + return protojson.Marshal(m.ToProtobuf()) } // MeshBoxIntersectionArea calculates the summed area of all triangles in a mesh diff --git a/spatialmath/mesh_test.go b/spatialmath/mesh_test.go index 7412497eddc..d7b89b9177d 100644 --- a/spatialmath/mesh_test.go +++ b/spatialmath/mesh_test.go @@ -1,6 +1,7 @@ package spatialmath import ( + "encoding/json" "math" "testing" @@ -49,6 +50,19 @@ func assertMeshesNearlyEqual(t *testing.T, mesh1, mesh2 *Mesh) { test.That(t, R3VectorAlmostEqual(p1, p2, 1e-3), test.ShouldBeTrue) } } + test.That(t, PoseAlmostEqual(mesh1.pose, mesh2.pose), test.ShouldBeTrue) + test.That(t, mesh1.label, test.ShouldResemble, mesh2.label) +} + +func TestMeshJsonConversion(t *testing.T) { + mesh1 := makeSimpleTriangleMesh().(*Mesh) + mesh1.label = "my label" + mesh1.pose = NewPose(r3.Vector{X: 1, Y: 2, Z: 3}, &OrientationVector{OX: 4, OY: 5, OZ: 6, Theta: 7}) + b, err := json.Marshal(mesh1) + test.That(t, err, test.ShouldBeNil) + var mesh2 Mesh + test.That(t, json.Unmarshal(b, &mesh2), test.ShouldBeNil) + assertMeshesNearlyEqual(t, mesh1, &mesh2) } func TestProtoConversion(t *testing.T) {