add-json-encode-decode-support-for-mesh

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

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) {