diff --git a/components/arm/client.go b/components/arm/client.go index d3ac803ff0c..01e791865ba 100644 --- a/components/arm/client.go +++ b/components/arm/client.go @@ -218,7 +218,7 @@ func (c *client) Geometries(ctx context.Context, extra map[string]interface{}) ( if err != nil { return nil, err } - return spatialmath.NewGeometriesFromProto(resp.GetGeometries()) + return referenceframe.NewGeometriesFromProto(resp.GetGeometries()) } // warnKinematicsUnsafe is a helper function to warn the user that no kinematics have been supplied for the conversion between diff --git a/components/arm/server.go b/components/arm/server.go index 04bf35c2356..1b08dc9856e 100644 --- a/components/arm/server.go +++ b/components/arm/server.go @@ -184,11 +184,12 @@ func (s *serviceServer) GetGeometries(ctx context.Context, req *commonpb.GetGeom if err != nil { return nil, err } - return &commonpb.GetGeometriesResponse{Geometries: spatialmath.NewGeometriesToProto(gifs.Geometries())}, nil + return &commonpb.GetGeometriesResponse{Geometries: referenceframe.NewGeometriesToProto( + gifs.Geometries())}, nil } return nil, err } - return &commonpb.GetGeometriesResponse{Geometries: spatialmath.NewGeometriesToProto(geometries)}, nil + return &commonpb.GetGeometriesResponse{Geometries: referenceframe.NewGeometriesToProto(geometries)}, nil } // GetKinematics returns the kinematics information associated with the arm. diff --git a/components/base/client.go b/components/base/client.go index edb6f05e0c4..2bf26e61fb6 100644 --- a/components/base/client.go +++ b/components/base/client.go @@ -13,6 +13,7 @@ import ( "go.viam.com/rdk/logging" rprotoutils "go.viam.com/rdk/protoutils" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/spatialmath" ) @@ -162,5 +163,5 @@ func (c *client) Geometries(ctx context.Context, extra map[string]interface{}) ( if err != nil { return nil, err } - return spatialmath.NewGeometriesFromProto(resp.GetGeometries()) + return referenceframe.NewGeometriesFromProto(resp.GetGeometries()) } diff --git a/components/base/server.go b/components/base/server.go index d96cbb36765..f7532a72089 100644 --- a/components/base/server.go +++ b/components/base/server.go @@ -10,8 +10,8 @@ import ( "go.viam.com/rdk/operation" "go.viam.com/rdk/protoutils" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" - "go.viam.com/rdk/spatialmath" ) // ErrGeometriesNil is the returned error if base geometries are nil. @@ -171,7 +171,7 @@ func (s *serviceServer) GetGeometries(ctx context.Context, req *commonpb.GetGeom if geometries == nil { return nil, ErrGeometriesNil(req.GetName()) } - return &commonpb.GetGeometriesResponse{Geometries: spatialmath.NewGeometriesToProto(geometries)}, nil + return &commonpb.GetGeometriesResponse{Geometries: referenceframe.NewGeometriesToProto(geometries)}, nil } // DoCommand receives arbitrary commands. diff --git a/components/base/server_test.go b/components/base/server_test.go index ad98dece489..d4831060488 100644 --- a/components/base/server_test.go +++ b/components/base/server_test.go @@ -11,6 +11,7 @@ import ( "go.viam.com/test" "go.viam.com/rdk/components/base" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/testutils/inject" @@ -210,7 +211,7 @@ func TestServer(t *testing.T) { req := &pbcommon.GetGeometriesRequest{Name: testBaseName} resp, err := server.GetGeometries(context.Background(), req) // TODO (rh) rename server to bServer after review test.That(t, resp, test.ShouldResemble, &pbcommon.GetGeometriesResponse{ - Geometries: spatialmath.NewGeometriesToProto([]spatialmath.Geometry{box}), + Geometries: referenceframe.NewGeometriesToProto([]spatialmath.Geometry{box}), }) test.That(t, err, test.ShouldBeNil) diff --git a/components/camera/client.go b/components/camera/client.go index 9273d58dcfe..9060e90e5ea 100644 --- a/components/camera/client.go +++ b/components/camera/client.go @@ -30,6 +30,7 @@ import ( "go.viam.com/rdk/logging" "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/protoutils" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/rimage/transform" "go.viam.com/rdk/spatialmath" @@ -340,7 +341,7 @@ func (c *client) Geometries(ctx context.Context, extra map[string]interface{}) ( if err != nil { return nil, err } - return spatialmath.NewGeometriesFromProto(resp.GetGeometries()) + return referenceframe.NewGeometriesFromProto(resp.GetGeometries()) } // TODO(RSDK-6433): This method can be called more than once during a client's lifecycle. diff --git a/components/camera/server.go b/components/camera/server.go index 8045f44b034..5f00c24f128 100644 --- a/components/camera/server.go +++ b/components/camera/server.go @@ -14,8 +14,8 @@ import ( "go.viam.com/rdk/logging" "go.viam.com/rdk/pointcloud" "go.viam.com/rdk/protoutils" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" - "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" ) @@ -256,5 +256,5 @@ func (s *serviceServer) GetGeometries(ctx context.Context, req *commonpb.GetGeom if err != nil { return nil, err } - return &commonpb.GetGeometriesResponse{Geometries: spatialmath.NewGeometriesToProto(geometries)}, nil + return &commonpb.GetGeometriesResponse{Geometries: referenceframe.NewGeometriesToProto(geometries)}, nil } diff --git a/components/gripper/client.go b/components/gripper/client.go index 49bae5fdf5d..657dfeeb18d 100644 --- a/components/gripper/client.go +++ b/components/gripper/client.go @@ -128,7 +128,7 @@ func (c *client) Geometries(ctx context.Context, extra map[string]interface{}) ( if err != nil { return nil, err } - return spatialmath.NewGeometriesFromProto(resp.GetGeometries()) + return referenceframe.NewGeometriesFromProto(resp.GetGeometries()) } func (c *client) Kinematics(ctx context.Context) (referenceframe.Model, error) { diff --git a/components/gripper/server.go b/components/gripper/server.go index d383332254c..e5466fca590 100644 --- a/components/gripper/server.go +++ b/components/gripper/server.go @@ -13,7 +13,6 @@ import ( rprotoutils "go.viam.com/rdk/protoutils" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" - "go.viam.com/rdk/spatialmath" ) // ErrGeometriesNil is the returned error if gripper geometries are nil. @@ -118,7 +117,7 @@ func (s *serviceServer) GetGeometries(ctx context.Context, req *commonpb.GetGeom if geometries == nil { return nil, ErrGeometriesNil(req.GetName()) } - return &commonpb.GetGeometriesResponse{Geometries: spatialmath.NewGeometriesToProto(geometries)}, nil + return &commonpb.GetGeometriesResponse{Geometries: referenceframe.NewGeometriesToProto(geometries)}, nil } func (s *serviceServer) GetKinematics(ctx context.Context, req *commonpb.GetKinematicsRequest) (*commonpb.GetKinematicsResponse, error) { diff --git a/config/proto_conversions.go b/config/proto_conversions.go index 8ecbd6033ad..f3bcb2a03d4 100644 --- a/config/proto_conversions.go +++ b/config/proto_conversions.go @@ -519,7 +519,7 @@ func FrameConfigFromProto(proto *pb.Frame) (*referenceframe.LinkConfig, error) { } if proto.GetGeometry() != nil { - geom, err := spatial.NewGeometryFromProto(proto.GetGeometry()) + geom, err := referenceframe.NewGeometryFromProto(proto.GetGeometry()) if err != nil { return nil, err } diff --git a/go.mod b/go.mod index 42404653f96..d349f090244 100644 --- a/go.mod +++ b/go.mod @@ -81,7 +81,7 @@ require ( go.uber.org/atomic v1.11.0 go.uber.org/multierr v1.11.0 go.uber.org/zap v1.27.0 - go.viam.com/api v0.1.474 + go.viam.com/api v0.1.475 go.viam.com/test v1.2.4 go.viam.com/utils v0.1.164 goji.io v2.0.2+incompatible diff --git a/go.sum b/go.sum index f78b0d39133..1f07644d228 100644 --- a/go.sum +++ b/go.sum @@ -1530,8 +1530,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.474 h1:PQz7d3PrSzWGl70rTQDW9rCE3TriuQIhn4/KDSaUs0c= -go.viam.com/api v0.1.474/go.mod h1:p/am76zx8SZ74V/F4rEAYQIpHaaLUwJgY2q3Uw3FIWk= +go.viam.com/api v0.1.475 h1:d0lpY/Ibgbv7CzSsET+ujSDRbq1K+xXeV76XXHGcy3s= +go.viam.com/api v0.1.475/go.mod h1:p/am76zx8SZ74V/F4rEAYQIpHaaLUwJgY2q3Uw3FIWk= go.viam.com/test v1.2.4 h1:JYgZhsuGAQ8sL9jWkziAXN9VJJiKbjoi9BsO33TW3ug= go.viam.com/test v1.2.4/go.mod h1:zI2xzosHdqXAJ/kFqcN+OIF78kQuTV2nIhGZ8EzvaJI= go.viam.com/utils v0.1.164 h1:EVKu5AuulD2m7V0OQyXb1YBEgvbHd4p/OwHkw8uT25Y= diff --git a/motionplan/armplanning/api.go b/motionplan/armplanning/api.go index 6d5cccdba1a..7732e473ea0 100644 --- a/motionplan/armplanning/api.go +++ b/motionplan/armplanning/api.go @@ -112,7 +112,7 @@ func (req *PlanRequest) validatePlanRequest() error { req.WorldState = newWS } - boundingRegions, err := spatialmath.NewGeometriesFromProto(req.BoundingRegions) + boundingRegions, err := referenceframe.NewGeometriesFromProto(req.BoundingRegions) if err != nil { return err } diff --git a/motionplan/armplanning/motion_planner.go b/motionplan/armplanning/motion_planner.go index 9963302214e..93d0ac2ce47 100644 --- a/motionplan/armplanning/motion_planner.go +++ b/motionplan/armplanning/motion_planner.go @@ -17,7 +17,6 @@ import ( "go.viam.com/rdk/motionplan" "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/spatialmath" ) // When we generate solutions, if a new solution is within this level of similarity to an existing one, discard it as a duplicate. @@ -83,7 +82,7 @@ func newPlannerFromPlanRequest(logger logging.Logger, request *PlanRequest) (*pl return nil, err } - boundingRegions, err := spatialmath.NewGeometriesFromProto(request.BoundingRegions) + boundingRegions, err := referenceframe.NewGeometriesFromProto(request.BoundingRegions) if err != nil { return nil, err } diff --git a/pointcloud/basic_octree.go b/pointcloud/basic_octree.go index 7b50a6a2403..514df61e0ba 100644 --- a/pointcloud/basic_octree.go +++ b/pointcloud/basic_octree.go @@ -258,9 +258,21 @@ func (octree *BasicOctree) Transform(pose spatialmath.Pose) spatialmath.Geometry } // ToProtobuf converts the octree to a Geometry proto message. -// TODO (RSDK-3743): Implement BasicOctree Geometry functions. func (octree *BasicOctree) ToProtobuf() *commonpb.Geometry { - return nil + bytes, err := ToBytes(octree) + if err != nil { + return nil + } + + return &commonpb.Geometry{ + Center: spatialmath.PoseToProtobuf(octree.Pose()), + GeometryType: &commonpb.Geometry_Pointcloud{ + Pointcloud: &commonpb.PointCloud{ + PointCloud: bytes, + }, + }, + Label: octree.Label(), + } } // CollidesWith checks if the given octree collides with the given geometry and returns true if it does. diff --git a/pointcloud/pointcloud.go b/pointcloud/pointcloud.go index f2c7ebf7b87..348a0ae699a 100644 --- a/pointcloud/pointcloud.go +++ b/pointcloud/pointcloud.go @@ -6,10 +6,12 @@ package pointcloud import ( + "bytes" "math" "sync" "github.com/golang/geo/r3" + commonpb "go.viam.com/api/common/v1" "go.viam.com/utils" "gonum.org/v1/gonum/mat" @@ -237,3 +239,18 @@ func CloudMatrix(pc PointCloud) (*mat.Dense, []CloudMatrixCol) { } return mat.NewDense(pc.Size(), pointSize, matData), header } + +// NewPointCloudFromProto creates a new point cloud geometry from a protobuf point cloud. +func NewPointCloudFromProto(pointCloud *commonpb.PointCloud, label string) (*BasicOctree, error) { + reader := bytes.NewReader(pointCloud.PointCloud) + pc, err := ReadPCD(reader, BasicOctreeType) + if err != nil { + return nil, err + } + octree, err := ToBasicOctree(pc, 0) + if err != nil { + return nil, err + } + octree.SetLabel(label) + return octree, nil +} diff --git a/referenceframe/geometry_proto.go b/referenceframe/geometry_proto.go new file mode 100644 index 00000000000..db0e4af6340 --- /dev/null +++ b/referenceframe/geometry_proto.go @@ -0,0 +1,90 @@ +package referenceframe + +import ( + "github.com/golang/geo/r3" + geo "github.com/kellydunn/golang-geo" + "github.com/pkg/errors" + commonpb "go.viam.com/api/common/v1" + + "go.viam.com/rdk/pointcloud" + "go.viam.com/rdk/spatialmath" +) + +// NewGeometryFromProto instantiates a new Geometry from a protobuf Geometry message. +func NewGeometryFromProto(geometry *commonpb.Geometry) (spatialmath.Geometry, error) { + if geometry.Center == nil { + return nil, errors.New("cannot have nil pose for geometry") + } + pose := spatialmath.NewPoseFromProtobuf(geometry.Center) + if box := geometry.GetBox().GetDimsMm(); box != nil { + return spatialmath.NewBox(pose, r3.Vector{X: box.X, Y: box.Y, Z: box.Z}, geometry.Label) + } + if capsule := geometry.GetCapsule(); capsule != nil { + return spatialmath.NewCapsule(pose, capsule.RadiusMm, capsule.LengthMm, geometry.Label) + } + if sphere := geometry.GetSphere(); sphere != nil { + // Fallback to point if radius is 0 + if sphere.RadiusMm == 0 { + return spatialmath.NewPoint(pose.Point(), geometry.Label), nil + } + return spatialmath.NewSphere(pose, sphere.RadiusMm, geometry.Label) + } + if mesh := geometry.GetMesh(); mesh != nil { + return spatialmath.NewMeshFromProto(pose, mesh, geometry.Label) + } + if pointCloud := geometry.GetPointcloud(); pointCloud != nil { + return pointcloud.NewPointCloudFromProto(pointCloud, geometry.Label) + } + return nil, errGeometryTypeUnsupported +} + +// NewGeometriesFromProto converts a list of Geometries from protobuf. +func NewGeometriesFromProto(proto []*commonpb.Geometry) ([]spatialmath.Geometry, error) { + if proto == nil { + return nil, nil + } + geometries := []spatialmath.Geometry{} + for _, geometry := range proto { + g, err := NewGeometryFromProto(geometry) + if err != nil { + return nil, err + } + geometries = append(geometries, g) + } + return geometries, nil +} + +// NewGeometriesToProto converts a list of Geometries to profobuf. +func NewGeometriesToProto(geometries []spatialmath.Geometry) []*commonpb.Geometry { + var proto []*commonpb.Geometry + for _, geometry := range geometries { + proto = append(proto, geometry.ToProtobuf()) + } + return proto +} + +// GeoGeometryToProtobuf converts the GeoGeometry struct into an equivalent Protobuf message. +func GeoGeometryToProtobuf(geoObst *spatialmath.GeoGeometry) *commonpb.GeoGeometry { + var convGeoms []*commonpb.Geometry + for _, geometry := range geoObst.Geometries() { + convGeoms = append(convGeoms, geometry.ToProtobuf()) + } + return &commonpb.GeoGeometry{ + Location: &commonpb.GeoPoint{Latitude: geoObst.Location().Lat(), Longitude: geoObst.Location().Lng()}, + Geometries: convGeoms, + } +} + +// GeoGeometryFromProtobuf takes a Protobuf representation of a GeoGeometry and converts back into a Go struct. +func GeoGeometryFromProtobuf(protoGeoObst *commonpb.GeoGeometry) (*spatialmath.GeoGeometry, error) { + convPoint := geo.NewPoint(protoGeoObst.GetLocation().GetLatitude(), protoGeoObst.GetLocation().GetLongitude()) + convGeoms := []spatialmath.Geometry{} + for _, protoGeom := range protoGeoObst.GetGeometries() { + newGeom, err := NewGeometryFromProto(protoGeom) + if err != nil { + return nil, err + } + convGeoms = append(convGeoms, newGeom) + } + return spatialmath.NewGeoGeometry(convPoint, convGeoms), nil +} diff --git a/referenceframe/geometry_proto_test.go b/referenceframe/geometry_proto_test.go new file mode 100644 index 00000000000..d3d68f1cbb8 --- /dev/null +++ b/referenceframe/geometry_proto_test.go @@ -0,0 +1,293 @@ +package referenceframe + +import ( + "math" + "testing" + + "github.com/golang/geo/r3" + geo "github.com/kellydunn/golang-geo" + commonpb "go.viam.com/api/common/v1" + "go.viam.com/test" + + "go.viam.com/rdk/pointcloud" + "go.viam.com/rdk/spatialmath" +) + +func TestGeometryProtobufRoundTrip(t *testing.T) { + deg45 := math.Pi / 4 + box, _ := spatialmath.NewBox(spatialmath.NewPose(r3.Vector{0, 0, 0}, &spatialmath.EulerAngles{0, 0, deg45}), r3.Vector{2, 2, 2}, "box") + sphere, _ := spatialmath.NewSphere(spatialmath.NewPose(r3.Vector{3, 4, 5}, spatialmath.NewZeroOrientation()), 10, "sphere") + point := spatialmath.NewPoint(r3.Vector{3, 4, 5}, "point") + capsule, _ := spatialmath.NewCapsule(spatialmath.NewPose(r3.Vector{1, 2, 3}, &spatialmath.EulerAngles{0, 0, deg45}), 5, 20, "capsule") + testCases := []struct { + name string + geometry spatialmath.Geometry + }{ + {"box", box}, + {"sphere", sphere}, + {"point", point}, + {"capsule", capsule}, + } + + for _, testCase := range testCases { + t.Run(testCase.name, func(t *testing.T) { + proto := testCase.geometry.ToProtobuf() + test.That(t, proto, test.ShouldNotBeNil) + test.That(t, proto.Center, test.ShouldNotBeNil) + test.That(t, proto.Label, test.ShouldEqual, testCase.name) + + newGeometry, err := NewGeometryFromProto(proto) + test.That(t, err, test.ShouldBeNil) + test.That(t, newGeometry, test.ShouldNotBeNil) + test.That(t, testCase.geometry.Label(), test.ShouldEqual, newGeometry.Label()) + test.That(t, spatialmath.GeometriesAlmostEqual(testCase.geometry, newGeometry), test.ShouldBeTrue) + }) + } +} + +func makeTestPointCloud(label string) *pointcloud.BasicOctree { + pc := pointcloud.NewBasicPointCloud(3) + err := pc.Set(r3.Vector{X: 0, Y: 0, Z: 0}, pointcloud.NewBasicData()) + if err != nil { + return nil + } + err = pc.Set(r3.Vector{X: 1, Y: 0, Z: 0}, pointcloud.NewBasicData()) + if err != nil { + return nil + } + err = pc.Set(r3.Vector{X: 0, Y: 1, Z: 0}, pointcloud.NewBasicData()) + if err != nil { + return nil + } + + octree, err := pointcloud.ToBasicOctree(pc, 50) + if err != nil { + return nil + } + + octree.SetLabel(label) + return octree +} + +func TestPointCloudProtobufRoundTrip(t *testing.T) { + pc := makeTestPointCloud("pointcloud") + + proto := pc.ToProtobuf() + test.That(t, proto, test.ShouldNotBeNil) + test.That(t, proto.Center, test.ShouldNotBeNil) + test.That(t, proto.Label, test.ShouldEqual, "pointcloud") + + newGeometry, err := NewGeometryFromProto(proto) + test.That(t, err, test.ShouldBeNil) + test.That(t, newGeometry, test.ShouldNotBeNil) + test.That(t, pc.Label(), test.ShouldEqual, newGeometry.Label()) + + newVolPC, ok := newGeometry.(pointcloud.PointCloud) + test.That(t, ok, test.ShouldBeTrue) + test.That(t, newVolPC.Size(), test.ShouldEqual, 3) + + _, exists := newVolPC.At(0, 0, 0) + test.That(t, exists, test.ShouldBeTrue) + _, exists = newVolPC.At(1, 0, 0) + test.That(t, exists, test.ShouldBeTrue) + _, exists = newVolPC.At(0, 1, 0) + test.That(t, exists, test.ShouldBeTrue) + + test.That(t, newVolPC.MetaData(), test.ShouldResemble, pc.MetaData()) +} + +func TestMeshProtobufRoundTrip(t *testing.T) { + // Create a complex mesh with various triangles for thorough testing + triangles := []*spatialmath.Triangle{ + spatialmath.NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 0}, + r3.Vector{X: 1000, Y: 0, Z: 0}, + r3.Vector{X: 0, Y: 1000, Z: 0}, + ), + spatialmath.NewTriangle( + r3.Vector{X: -500, Y: -500, Z: 0}, + r3.Vector{X: 500, Y: -500, Z: 0}, + r3.Vector{X: 0, Y: 500, Z: 0}, + ), + spatialmath.NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 1000}, + r3.Vector{X: 1000, Y: 0, Z: 1000}, + r3.Vector{X: 500, Y: 1000, Z: 1000}, + ), + spatialmath.NewTriangle( + r3.Vector{X: 123.456, Y: 789.012, Z: 345.678}, + r3.Vector{X: 456.789, Y: 123.456, Z: 678.901}, + r3.Vector{X: 789.012, Y: 456.789, Z: 123.456}, + ), + spatialmath.NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 0}, + r3.Vector{X: 10000, Y: 0, Z: 0}, + r3.Vector{X: 0, Y: 10000, Z: 0}, + ), + spatialmath.NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 0}, + r3.Vector{X: 1, Y: 0, Z: 0}, + r3.Vector{X: 0, Y: 1, Z: 0}, + ), + spatialmath.NewTriangle( + r3.Vector{X: -1000, Y: -1000, Z: -1000}, + r3.Vector{X: -500, Y: -1000, Z: -1000}, + r3.Vector{X: -1000, Y: -500, Z: -1000}, + ), + spatialmath.NewTriangle( + r3.Vector{X: 100, Y: 100, Z: -500}, + r3.Vector{X: 200, Y: 100, Z: 500}, + r3.Vector{X: 150, Y: 200, Z: 0}, + ), + spatialmath.NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 0}, + r3.Vector{X: 1000, Y: 0, Z: 0}, + r3.Vector{X: 1000, Y: 1000, Z: 0}, + ), + spatialmath.NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 0}, + r3.Vector{X: 1000, Y: 1000, Z: 0}, + r3.Vector{X: 0, Y: 1000, Z: 0}, + ), + spatialmath.NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 0}, + r3.Vector{X: 0, Y: 1000, Z: 0}, + r3.Vector{X: 0, Y: 1000, Z: 1000}, + ), + spatialmath.NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 0}, + r3.Vector{X: 0, Y: 1000, Z: 1000}, + r3.Vector{X: 0, Y: 0, Z: 1000}, + ), + } + + // Create mesh with a pose and label + originalPose := spatialmath.NewPose(r3.Vector{X: 100, Y: 200, Z: 300}, spatialmath.NewZeroOrientation()) + originalMesh := spatialmath.NewMesh(originalPose, triangles, "test_mesh_from_triangles") + + // Convert to protobuf + proto := originalMesh.ToProtobuf() + test.That(t, proto, test.ShouldNotBeNil) + test.That(t, proto.Label, test.ShouldEqual, "test_mesh_from_triangles") + + // Restore from protobuf + restoredGeometry, err := NewGeometryFromProto(proto) + test.That(t, err, test.ShouldBeNil) + restoredMesh, ok := restoredGeometry.(*spatialmath.Mesh) + test.That(t, ok, test.ShouldBeTrue) + + test.That(t, restoredMesh.Label(), test.ShouldEqual, originalMesh.Label()) + test.That(t, spatialmath.PoseAlmostEqual(restoredMesh.Pose(), originalMesh.Pose()), test.ShouldBeTrue) + test.That(t, len(restoredMesh.Triangles()), test.ShouldEqual, len(originalMesh.Triangles())) + + // Verify all triangles match + originalTriangles := originalMesh.Triangles() + restoredTriangles := restoredMesh.Triangles() + for i, originalTri := range originalTriangles { + restoredTri := restoredTriangles[i] + origPoints := originalTri.Points() + restoredPoints := restoredTri.Points() + + test.That(t, len(restoredPoints), test.ShouldEqual, len(origPoints)) + + for j, origPoint := range origPoints { + restoredPoint := restoredPoints[j] + // The conversion from mm to meters and back can create micrometer-level float changes + epsilon := 1e-4 + test.That(t, math.Abs(origPoint.X-restoredPoint.X), test.ShouldBeLessThan, epsilon) + test.That(t, math.Abs(origPoint.Y-restoredPoint.Y), test.ShouldBeLessThan, epsilon) + test.That(t, math.Abs(origPoint.Z-restoredPoint.Z), test.ShouldBeLessThan, epsilon) + } + } + + // Verify that the mesh can be converted to protobuf again + secondProto := restoredMesh.ToProtobuf() + test.That(t, secondProto, test.ShouldNotBeNil) + test.That(t, secondProto.Label, test.ShouldEqual, originalMesh.Label()) + + // Verify the protobuf content is the same + test.That(t, secondProto.GetMesh().ContentType, test.ShouldEqual, proto.GetMesh().ContentType) + test.That(t, len(secondProto.GetMesh().Mesh), test.ShouldEqual, len(proto.GetMesh().Mesh)) +} + +func TestNewGeometryFromProtoErrors(t *testing.T) { + testCases := []struct { + name string + geometry *commonpb.Geometry + expectedErr string + }{ + { + name: "nil pose", + geometry: &commonpb.Geometry{}, + expectedErr: "cannot have nil pose for geometry", + }, + { + name: "unsupported geometry type", + geometry: &commonpb.Geometry{ + Center: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), + }, + expectedErr: errGeometryTypeUnsupported.Error(), + }, + } + + for _, testCase := range testCases { + t.Run(testCase.name, func(t *testing.T) { + viamGeom, err := NewGeometryFromProto(testCase.geometry) + test.That(t, viamGeom, test.ShouldBeNil) + test.That(t, err.Error(), test.ShouldContainSubstring, testCase.expectedErr) + }) + } +} + +func TestGeoGeometryProtobufRoundTrip(t *testing.T) { + singleSphere, _ := spatialmath.NewSphere(spatialmath.NewPose(r3.Vector{0, 0, 0}, spatialmath.NewZeroOrientation()), 5, "test_sphere") + multiSphere, _ := spatialmath.NewSphere(spatialmath.NewPose(r3.Vector{0, 0, 0}, spatialmath.NewZeroOrientation()), 5, "sphere1") + multiBox, _ := spatialmath.NewBox(spatialmath.NewPose(r3.Vector{10, 0, 0}, spatialmath.NewZeroOrientation()), r3.Vector{2, 2, 2}, "box1") + multiSphere.SetLabel("sphere1") + multiBox.SetLabel("box1") + + testCases := []struct { + name string + geometries []spatialmath.Geometry + latitude float64 + longitude float64 + description string + }{ + { + name: "single sphere", + geometries: []spatialmath.Geometry{singleSphere}, + latitude: 37.7749, + longitude: -122.4194, + description: "San Francisco", + }, + { + name: "multiple geometries", + geometries: []spatialmath.Geometry{ + multiSphere, + multiBox, + }, + latitude: 40.7128, + longitude: -74.0060, + description: "New York", + }, + } + + for _, testCase := range testCases { + t.Run(testCase.name, func(t *testing.T) { + testPoint := geo.NewPoint(testCase.latitude, testCase.longitude) + testGeoObst := spatialmath.NewGeoGeometry(testPoint, testCase.geometries) + + convGeoObstProto := GeoGeometryToProtobuf(testGeoObst) + test.That(t, convGeoObstProto, test.ShouldNotBeNil) + test.That(t, testPoint.Lat(), test.ShouldEqual, convGeoObstProto.GetLocation().GetLatitude()) + test.That(t, testPoint.Lng(), test.ShouldEqual, convGeoObstProto.GetLocation().GetLongitude()) + test.That(t, len(testCase.geometries), test.ShouldEqual, len(convGeoObstProto.GetGeometries())) + + convGeoObst, err := GeoGeometryFromProtobuf(convGeoObstProto) + test.That(t, err, test.ShouldBeNil) + test.That(t, testPoint.Lat(), test.ShouldEqual, convGeoObst.Location().Lat()) + test.That(t, testPoint.Lng(), test.ShouldEqual, convGeoObst.Location().Lng()) + test.That(t, len(testCase.geometries), test.ShouldEqual, len(convGeoObst.Geometries())) + }) + } +} diff --git a/referenceframe/transformable.go b/referenceframe/transformable.go index af93b57d420..ce234736175 100644 --- a/referenceframe/transformable.go +++ b/referenceframe/transformable.go @@ -193,7 +193,7 @@ func LinkInFrameFromTransformProtobuf(proto *commonpb.Transform) (*LinkInFrame, pose := spatialmath.NewPoseFromProtobuf(poseMsg) var geometry spatialmath.Geometry if proto.PhysicalObject != nil { - geometry, err = spatialmath.NewGeometryFromProto(proto.PhysicalObject) + geometry, err = NewGeometryFromProto(proto.PhysicalObject) if err != nil { return nil, err } @@ -291,13 +291,13 @@ func (gF *GeometriesInFrame) Transform(tf *PoseInFrame) Transformable { func GeometriesInFrameToProtobuf(framedGeometries *GeometriesInFrame) *commonpb.GeometriesInFrame { return &commonpb.GeometriesInFrame{ ReferenceFrame: framedGeometries.frame, - Geometries: spatialmath.NewGeometriesToProto(framedGeometries.Geometries()), + Geometries: NewGeometriesToProto(framedGeometries.Geometries()), } } // ProtobufToGeometriesInFrame converts a GeometriesInFrame message as specified in common.proto to a GeometriesInFrame struct. func ProtobufToGeometriesInFrame(proto *commonpb.GeometriesInFrame) (*GeometriesInFrame, error) { - geometries, err := spatialmath.NewGeometriesFromProto(proto.GetGeometries()) + geometries, err := NewGeometriesFromProto(proto.GetGeometries()) if err != nil { return nil, err } diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 7fbc9be1d73..67b9f5297df 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -685,7 +685,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( // TODO (GV) - Remove this unnecessary converion/re-conversion when an // opinionated proto message is created for PlanRequest - boundingRegionsProto := spatialmath.NewGeometriesToProto(boundingRegions) + boundingRegionsProto := referenceframe.NewGeometriesToProto(boundingRegions) mr, err := ms.createBaseMoveRequest( ctx, diff --git a/services/motion/motion_test.go b/services/motion/motion_test.go index c0fa63548fb..cb8320d0c0a 100644 --- a/services/motion/motion_test.go +++ b/services/motion/motion_test.go @@ -1175,7 +1175,7 @@ func TestMoveOnMapReq(t *testing.T) { Destination: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), ComponentName: rprotoutils.ResourceNameToProto(myBase), SlamServiceName: rprotoutils.ResourceNameToProto(mySlam), - Obstacles: spatialmath.NewGeometriesToProto([]spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{2, 2, 2}, "pt")}), + Obstacles: referenceframe.NewGeometriesToProto([]spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{2, 2, 2}, "pt")}), Extra: &structpb.Struct{}, }, err: nil, @@ -1267,7 +1267,7 @@ func TestMoveOnMapReq(t *testing.T) { Destination: spatialmath.PoseToProtobuf(spatialmath.NewPoseFromPoint(r3.Vector{2700, 0, 0})), ComponentName: rprotoutils.ResourceNameToProto(myBase), SlamServiceName: rprotoutils.ResourceNameToProto(mySlam), - Obstacles: spatialmath.NewGeometriesToProto( + Obstacles: referenceframe.NewGeometriesToProto( []spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{X: 2, Y: 2, Z: 2}, "pt")}, ), }, diff --git a/services/motion/pbhelpers.go b/services/motion/pbhelpers.go index fff9aa79d88..1c197413d9c 100644 --- a/services/motion/pbhelpers.go +++ b/services/motion/pbhelpers.go @@ -231,14 +231,14 @@ func (r MoveOnGlobeReq) toProto(name string) (*pb.MoveOnGlobeRequest, error) { if len(r.Obstacles) > 0 { obstaclesProto := make([]*commonpb.GeoGeometry, 0, len(r.Obstacles)) for _, obstacle := range r.Obstacles { - obstaclesProto = append(obstaclesProto, spatialmath.GeoGeometryToProtobuf(obstacle)) + obstaclesProto = append(obstaclesProto, referenceframe.GeoGeometryToProtobuf(obstacle)) } req.Obstacles = obstaclesProto } if len(r.BoundingRegions) > 0 { obstaclesProto := make([]*commonpb.GeoGeometry, 0, len(r.BoundingRegions)) for _, obstacle := range r.BoundingRegions { - obstaclesProto = append(obstaclesProto, spatialmath.GeoGeometryToProtobuf(obstacle)) + obstaclesProto = append(obstaclesProto, referenceframe.GeoGeometryToProtobuf(obstacle)) } req.BoundingRegions = obstaclesProto } @@ -262,7 +262,7 @@ func moveOnGlobeRequestFromProto(req *pb.MoveOnGlobeRequest) (MoveOnGlobeReq, er obstaclesProto := req.GetObstacles() obstacles := make([]*spatialmath.GeoGeometry, 0, len(obstaclesProto)) for _, eachProtoObst := range obstaclesProto { - convObst, err := spatialmath.GeoGeometryFromProtobuf(eachProtoObst) + convObst, err := referenceframe.GeoGeometryFromProtobuf(eachProtoObst) if err != nil { return MoveOnGlobeReq{}, err } @@ -272,7 +272,7 @@ func moveOnGlobeRequestFromProto(req *pb.MoveOnGlobeRequest) (MoveOnGlobeReq, er boundingRegionGeometriesProto := req.GetBoundingRegions() boundingRegionGeometries := make([]*spatialmath.GeoGeometry, 0, len(boundingRegionGeometriesProto)) for _, eachProtoObst := range boundingRegionGeometriesProto { - convObst, err := spatialmath.GeoGeometryFromProtobuf(eachProtoObst) + convObst, err := referenceframe.GeoGeometryFromProtobuf(eachProtoObst) if err != nil { return MoveOnGlobeReq{}, err } @@ -363,7 +363,7 @@ func moveOnMapRequestFromProto(req *pb.MoveOnMapRequest) (MoveOnMapReq, error) { } geoms := []spatialmath.Geometry{} if obs := req.GetObstacles(); len(obs) > 0 { - convertedGeom, err := spatialmath.NewGeometriesFromProto(obs) + convertedGeom, err := referenceframe.NewGeometriesFromProto(obs) if err != nil { return MoveOnMapReq{}, errors.Wrap(err, "cannot convert obstacles into geometries") } @@ -392,7 +392,7 @@ func (r MoveOnMapReq) toProto(name string) (*pb.MoveOnMapRequest, error) { ComponentName: rprotoutils.ResourceNameToProto(r.ComponentName), Destination: spatialmath.PoseToProtobuf(r.Destination), SlamServiceName: rprotoutils.ResourceNameToProto(r.SlamName), - Obstacles: spatialmath.NewGeometriesToProto(r.Obstacles), + Obstacles: referenceframe.NewGeometriesToProto(r.Obstacles), Extra: ext, } diff --git a/services/motion/server_test.go b/services/motion/server_test.go index 9d1726faeee..7b15ba0cbe2 100644 --- a/services/motion/server_test.go +++ b/services/motion/server_test.go @@ -208,11 +208,11 @@ func TestServerMoveOnGlobe(t *testing.T) { geoGeometry3 := spatialmath.NewGeoGeometry(geo.NewPoint(1, 2), []spatialmath.Geometry{geometries3}) obs := []*commonpb.GeoGeometry{ - spatialmath.GeoGeometryToProtobuf(geoGeometry1), - spatialmath.GeoGeometryToProtobuf(geoGeometry2), + referenceframe.GeoGeometryToProtobuf(geoGeometry1), + referenceframe.GeoGeometryToProtobuf(geoGeometry2), } boundingRegionGeoms := []*commonpb.GeoGeometry{ - spatialmath.GeoGeometryToProtobuf(geoGeometry3), + referenceframe.GeoGeometryToProtobuf(geoGeometry3), } angularDegsPerSec := 1. linearMPerSec := 2. @@ -411,7 +411,7 @@ func TestServerMoveOnMap(t *testing.T) { ComponentName: protoutils.ResourceNameToProto(base.Named("test-base")), Destination: spatialmath.PoseToProtobuf(spatialmath.NewZeroPose()), SlamServiceName: protoutils.ResourceNameToProto(slam.Named("test-slam")), - Obstacles: spatialmath.NewGeometriesToProto([]spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{2, 2, 2}, "pt")}), + Obstacles: referenceframe.NewGeometriesToProto([]spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{2, 2, 2}, "pt")}), } firstExecutionID := uuid.New() diff --git a/services/navigation/client.go b/services/navigation/client.go index d8d2ede35fd..422cb977af6 100644 --- a/services/navigation/client.go +++ b/services/navigation/client.go @@ -13,6 +13,7 @@ import ( "go.viam.com/rdk/logging" rprotoutils "go.viam.com/rdk/protoutils" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/spatialmath" ) @@ -181,7 +182,7 @@ func (c *client) Obstacles(ctx context.Context, extra map[string]interface{}) ([ protoObs := resp.GetObstacles() geos := []*spatialmath.GeoGeometry{} for _, o := range protoObs { - obstacle, err := spatialmath.GeoGeometryFromProtobuf(o) + obstacle, err := referenceframe.GeoGeometryFromProtobuf(o) if err != nil { return nil, err } diff --git a/services/navigation/server.go b/services/navigation/server.go index 7e4609845ce..e520f8e6aaa 100644 --- a/services/navigation/server.go +++ b/services/navigation/server.go @@ -10,8 +10,8 @@ import ( pb "go.viam.com/api/service/navigation/v1" "go.viam.com/rdk/protoutils" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" - "go.viam.com/rdk/spatialmath" ) // serviceServer implements the contract from navigation.proto. @@ -150,7 +150,7 @@ func (server *serviceServer) GetObstacles(ctx context.Context, req *pb.GetObstac } protoObs := []*commonpb.GeoGeometry{} for _, obstacle := range obstacles { - protoObs = append(protoObs, spatialmath.GeoGeometryToProtobuf(obstacle)) + protoObs = append(protoObs, referenceframe.GeoGeometryToProtobuf(obstacle)) } return &pb.GetObstaclesResponse{Obstacles: protoObs}, nil } diff --git a/spatialmath/box_test.go b/spatialmath/box_test.go index d9256c78b56..8d684cbb4b9 100644 --- a/spatialmath/box_test.go +++ b/spatialmath/box_test.go @@ -8,8 +8,8 @@ import ( "go.viam.com/test" ) -func makeTestBox(o Orientation, pt, dims r3.Vector, label string) Geometry { - box, _ := NewBox(NewPose(pt, o), dims, label) +func makeTestBox(o Orientation, pt, dims r3.Vector) Geometry { + box, _ := NewBox(NewPose(pt, o), dims, "") return box } @@ -31,16 +31,16 @@ func TestNewBox(t *testing.T) { } func TestBoxAlmostEqual(t *testing.T) { - original := makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{1, 1, 1}, "") - good := makeTestBox(NewZeroOrientation(), r3.Vector{1e-16, 1e-16, 1e-16}, r3.Vector{1 + 1e-16, 1 + 1e-16, 1 + 1e-16}, "") - bad := makeTestBox(NewZeroOrientation(), r3.Vector{1e-2, 1e-2, 1e-2}, r3.Vector{1 + 1e-2, 1 + 1e-2, 1 + 1e-2}, "") + original := makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{1, 1, 1}) + good := makeTestBox(NewZeroOrientation(), r3.Vector{1e-16, 1e-16, 1e-16}, r3.Vector{1 + 1e-16, 1 + 1e-16, 1 + 1e-16}) + bad := makeTestBox(NewZeroOrientation(), r3.Vector{1e-2, 1e-2, 1e-2}, r3.Vector{1 + 1e-2, 1 + 1e-2, 1 + 1e-2}) test.That(t, original.(*box).almostEqual(good), test.ShouldBeTrue) test.That(t, original.(*box).almostEqual(bad), test.ShouldBeFalse) } func TestBoxVertices(t *testing.T) { offset := r3.Vector{2, 2, 2} - boxGeom := makeTestBox(NewZeroOrientation(), offset, r3.Vector{2, 2, 2}, "") + boxGeom := makeTestBox(NewZeroOrientation(), offset, r3.Vector{2, 2, 2}) box, ok := boxGeom.(*box) test.That(t, ok, test.ShouldBeTrue) vertices := box.vertices() diff --git a/spatialmath/geo_geometry.go b/spatialmath/geo_geometry.go index 66039ae85eb..71ff919c9bc 100644 --- a/spatialmath/geo_geometry.go +++ b/spatialmath/geo_geometry.go @@ -34,32 +34,6 @@ func (gob *GeoGeometry) Geometries() []Geometry { return gob.geometries } -// GeoGeometryToProtobuf converts the GeoGeometry struct into an equivalent Protobuf message. -func GeoGeometryToProtobuf(geoObst *GeoGeometry) *commonpb.GeoGeometry { - var convGeoms []*commonpb.Geometry - for _, geometry := range geoObst.geometries { - convGeoms = append(convGeoms, geometry.ToProtobuf()) - } - return &commonpb.GeoGeometry{ - Location: &commonpb.GeoPoint{Latitude: geoObst.location.Lat(), Longitude: geoObst.location.Lng()}, - Geometries: convGeoms, - } -} - -// GeoGeometryFromProtobuf takes a Protobuf representation of a GeoGeometry and converts back into a Go struct. -func GeoGeometryFromProtobuf(protoGeoObst *commonpb.GeoGeometry) (*GeoGeometry, error) { - convPoint := geo.NewPoint(protoGeoObst.GetLocation().GetLatitude(), protoGeoObst.GetLocation().GetLongitude()) - convGeoms := []Geometry{} - for _, protoGeom := range protoGeoObst.GetGeometries() { - newGeom, err := NewGeometryFromProto(protoGeom) - if err != nil { - return nil, err - } - convGeoms = append(convGeoms, newGeom) - } - return NewGeoGeometry(convPoint, convGeoms), nil -} - // GeoGeometryConfig specifies the format of GeoGeometries specified through the configuration file. type GeoGeometryConfig struct { Location *commonpb.GeoPoint `json:"location"` diff --git a/spatialmath/geo_geometry_test.go b/spatialmath/geo_geometry_test.go index 202afa65f2b..0ba95d96f77 100644 --- a/spatialmath/geo_geometry_test.go +++ b/spatialmath/geo_geometry_test.go @@ -50,26 +50,6 @@ func TestGeoGeometries(t *testing.T) { test.That(t, testPoint, test.ShouldResemble, testGeoObst.Location()) test.That(t, testGeoms, test.ShouldResemble, testGeoObst.Geometries()) - t.Run("Conversion from GeoGeometry to Protobuf", func(t *testing.T) { - convGeoObstProto := GeoGeometryToProtobuf(testGeoObst) - test.That(t, err, test.ShouldBeNil) - test.That(t, testPoint.Lat(), test.ShouldEqual, convGeoObstProto.GetLocation().GetLatitude()) - test.That(t, testPoint.Lng(), test.ShouldEqual, convGeoObstProto.GetLocation().GetLongitude()) - test.That(t, len(testGeoms), test.ShouldEqual, len(convGeoObstProto.GetGeometries())) - }) - - t.Run("Conversion from Protobuf to GeoGeometry", func(t *testing.T) { - testProtobuf := &commonpb.GeoGeometry{ - Location: &commonpb.GeoPoint{Latitude: testLatitude, Longitude: testLongitude}, - Geometries: []*commonpb.Geometry{testSphere.ToProtobuf()}, - } - - convGeoObst, err := GeoGeometryFromProtobuf(testProtobuf) - test.That(t, err, test.ShouldBeNil) - test.That(t, testPoint, test.ShouldResemble, convGeoObst.Location()) - test.That(t, testGeoms, test.ShouldResemble, convGeoObst.Geometries()) - }) - // test forward and backward conversion from GeoGeometryConfig to GeoGeometry gc, err := NewGeometryConfig(testSphere) test.That(t, err, test.ShouldBeNil) diff --git a/spatialmath/geometry.go b/spatialmath/geometry.go index dce3feb2a6e..aebe0fad41f 100644 --- a/spatialmath/geometry.go +++ b/spatialmath/geometry.go @@ -2,7 +2,6 @@ package spatialmath import ( "encoding/json" - "errors" "fmt" "github.com/golang/geo/r3" @@ -189,52 +188,3 @@ func GeometriesAlmostEqual(a, b Geometry) bool { return false } } - -// NewGeometryFromProto instantiates a new Geometry from a protobuf Geometry message. -func NewGeometryFromProto(geometry *commonpb.Geometry) (Geometry, error) { - if geometry.Center == nil { - return nil, errors.New("cannot have nil pose for geometry") - } - pose := NewPoseFromProtobuf(geometry.Center) - if box := geometry.GetBox().GetDimsMm(); box != nil { - return NewBox(pose, r3.Vector{X: box.X, Y: box.Y, Z: box.Z}, geometry.Label) - } - if capsule := geometry.GetCapsule(); capsule != nil { - return NewCapsule(pose, capsule.RadiusMm, capsule.LengthMm, geometry.Label) - } - if sphere := geometry.GetSphere(); sphere != nil { - if sphere.RadiusMm == 0 { - return NewPoint(pose.Point(), geometry.Label), nil - } - return NewSphere(pose, sphere.RadiusMm, geometry.Label) - } - if mesh := geometry.GetMesh(); mesh != nil { - return newMeshFromProto(pose, mesh, geometry.Label) - } - return nil, errGeometryTypeUnsupported -} - -// NewGeometriesFromProto converts a list of Geometries from protobuf. -func NewGeometriesFromProto(proto []*commonpb.Geometry) ([]Geometry, error) { - if proto == nil { - return nil, nil - } - geometries := []Geometry{} - for _, geometry := range proto { - g, err := NewGeometryFromProto(geometry) - if err != nil { - return nil, err - } - geometries = append(geometries, g) - } - return geometries, nil -} - -// NewGeometriesToProto converts a list of Geometries to profobuf. -func NewGeometriesToProto(geometries []Geometry) []*commonpb.Geometry { - var proto []*commonpb.Geometry - for _, geometry := range geometries { - proto = append(proto, geometry.ToProtobuf()) - } - return proto -} diff --git a/spatialmath/geometry_test.go b/spatialmath/geometry_test.go index 050429e2bf9..658b78434dd 100644 --- a/spatialmath/geometry_test.go +++ b/spatialmath/geometry_test.go @@ -7,8 +7,6 @@ import ( "testing" "github.com/golang/geo/r3" - "github.com/pkg/errors" - commonpb "go.viam.com/api/common/v1" "go.viam.com/test" ) @@ -64,30 +62,6 @@ func TestGeometrySerializationJSON(t *testing.T) { } } -func TestGeometryToFromProtobuf(t *testing.T) { - deg45 := math.Pi / 4 - testCases := []struct { - name string - geometry Geometry - }{ - {"box", makeTestBox(&EulerAngles{0, 0, deg45}, r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}, "box")}, - {"sphere", makeTestSphere(r3.Vector{3, 4, 5}, 10, "sphere")}, - {"point", NewPoint(r3.Vector{3, 4, 5}, "point")}, - } - for _, testCase := range testCases { - t.Run(testCase.name, func(t *testing.T) { - newVol, err := NewGeometryFromProto(testCase.geometry.ToProtobuf()) - test.That(t, err, test.ShouldBeNil) - test.That(t, GeometriesAlmostEqual(testCase.geometry, newVol), test.ShouldBeTrue) - test.That(t, testCase.geometry.Label(), test.ShouldEqual, testCase.name) - }) - } - - // test that bad message does not generate error - _, err := NewGeometryFromProto(&commonpb.Geometry{Center: PoseToProtobuf(NewZeroPose())}) - test.That(t, err.Error(), test.ShouldContainSubstring, errGeometryTypeUnsupported.Error()) -} - type geometryComparisonTestCase struct { testname string geometries [2]Geometry @@ -122,136 +96,136 @@ func TestBoxVsBoxCollision(t *testing.T) { { "inscribed", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{1, 1, 1}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{1, 1, 1}), }, -1.5, }, { "face to face contact", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{2, 0, 0}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}), + makeTestBox(NewZeroOrientation(), r3.Vector{2, 0, 0}, r3.Vector{2, 2, 2}), }, 0, }, { "face to face near contact", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{2.01, 0, 0}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}), + makeTestBox(NewZeroOrientation(), r3.Vector{2.01, 0, 0}, r3.Vector{2, 2, 2}), }, 0.01, }, { "coincident edge contact", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{2, 4, 0}, r3.Vector{2, 6, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}), + makeTestBox(NewZeroOrientation(), r3.Vector{2, 4, 0}, r3.Vector{2, 6, 2}), }, 0, }, { "coincident edges near contact", [2]Geometry{ - makeTestBox((NewZeroOrientation()), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{2, 4.01, 0}, r3.Vector{2, 6, 2}, ""), + makeTestBox((NewZeroOrientation()), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}), + makeTestBox(NewZeroOrientation(), r3.Vector{2, 4.01, 0}, r3.Vector{2, 6, 2}), }, 0.01, }, { "vertex to vertex contact", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{2, 2, 2}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}), + makeTestBox(NewZeroOrientation(), r3.Vector{2, 2, 2}, r3.Vector{2, 2, 2}), }, 0, }, { "vertex to vertex near contact", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{2.01, 2, 2}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}), + makeTestBox(NewZeroOrientation(), r3.Vector{2.01, 2, 2}, r3.Vector{2, 2, 2}), }, 0.005, }, { "edge along face contact", [2]Geometry{ - makeTestBox(&EulerAngles{deg45, 0, 0}, r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{0, 1 + math.Sqrt2, 0}, r3.Vector{2, 2, 2}, ""), + makeTestBox(&EulerAngles{deg45, 0, 0}, r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 1 + math.Sqrt2, 0}, r3.Vector{2, 2, 2}), }, 0, }, { "edge along face near contact", [2]Geometry{ - makeTestBox(&EulerAngles{deg45, 0, 0}, r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{0, 1.01 + math.Sqrt2, 0}, r3.Vector{2, 2, 2}, ""), + makeTestBox(&EulerAngles{deg45, 0, 0}, r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 1.01 + math.Sqrt2, 0}, r3.Vector{2, 2, 2}), }, 0.01, }, { "edge to edge contact", [2]Geometry{ - makeTestBox(&EulerAngles{0, 0, deg45}, r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(&EulerAngles{0, deg45, 0}, r3.Vector{2 * math.Sqrt2, 0, 0}, r3.Vector{2, 2, 2}, ""), + makeTestBox(&EulerAngles{0, 0, deg45}, r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}), + makeTestBox(&EulerAngles{0, deg45, 0}, r3.Vector{2 * math.Sqrt2, 0, 0}, r3.Vector{2, 2, 2}), }, 0, }, { "edge to edge near contact", [2]Geometry{ - makeTestBox(&EulerAngles{0, 0, deg45}, r3.Vector{-.01, 0, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(&EulerAngles{0, deg45, 0}, r3.Vector{2 * math.Sqrt2, 0, 0}, r3.Vector{2, 2, 2}, ""), + makeTestBox(&EulerAngles{0, 0, deg45}, r3.Vector{-.01, 0, 0}, r3.Vector{2, 2, 2}), + makeTestBox(&EulerAngles{0, deg45, 0}, r3.Vector{2 * math.Sqrt2, 0, 0}, r3.Vector{2, 2, 2}), }, 0.01, }, { "vertex to face contact", [2]Geometry{ - makeTestBox(&EulerAngles{deg45, deg45, 0}, r3.Vector{0.5, -.5, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(&EulerAngles{0, 0, 0}, r3.Vector{0, 0, 0.97 + math.Sqrt(3)}, r3.Vector{2, 2, 2}, ""), + makeTestBox(&EulerAngles{deg45, deg45, 0}, r3.Vector{0.5, -.5, 0}, r3.Vector{2, 2, 2}), + makeTestBox(&EulerAngles{0, 0, 0}, r3.Vector{0, 0, 0.97 + math.Sqrt(3)}, r3.Vector{2, 2, 2}), }, -.005, }, { "vertex to face near contact", [2]Geometry{ - makeTestBox(&EulerAngles{deg45, deg45, 0}, r3.Vector{0, 0, -0.01}, r3.Vector{2, 2, 2}, ""), - makeTestBox(&EulerAngles{0, 0, 0}, r3.Vector{0, 0, 0.97 + math.Sqrt(3)}, r3.Vector{2, 2, 2}, ""), + makeTestBox(&EulerAngles{deg45, deg45, 0}, r3.Vector{0, 0, -0.01}, r3.Vector{2, 2, 2}), + makeTestBox(&EulerAngles{0, 0, 0}, r3.Vector{0, 0, 0.97 + math.Sqrt(3)}, r3.Vector{2, 2, 2}), }, 0.005, }, { "separated axis aligned", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{5, 6, 0}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{2, 2, 2}), + makeTestBox(NewZeroOrientation(), r3.Vector{5, 6, 0}, r3.Vector{2, 2, 2}), }, 4.346, // upper bound on separation distance }, { "axis aligned overlap", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{20, 20, 20}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{20, 20, 20}, r3.Vector{24, 26, 28}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{20, 20, 20}), + makeTestBox(NewZeroOrientation(), r3.Vector{20, 20, 20}, r3.Vector{24, 26, 28}), }, -2, }, { "full overlap", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{10, 10, 10}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{10, 10, 10}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{10, 10, 10}), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{10, 10, 10}), }, -10, }, { "zero geometry box", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{20, 20, 20}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{2, 2, 2}, r3.Vector{0, 0, 0}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 0}, r3.Vector{20, 20, 20}), + makeTestBox(NewZeroOrientation(), r3.Vector{2, 2, 2}, r3.Vector{0, 0, 0}), }, -8, }, @@ -263,17 +237,17 @@ func TestSphereVsSphereCollision(t *testing.T) { cases := []geometryComparisonTestCase{ { "test inscribed spheres", - [2]Geometry{makeTestSphere(r3.Vector{}, 1, ""), makeTestSphere(r3.Vector{}, 2, "")}, + [2]Geometry{makeTestSphere(r3.Vector{}, 1), makeTestSphere(r3.Vector{}, 2)}, -3, }, { "test tangent spheres", - [2]Geometry{makeTestSphere(r3.Vector{}, 1, ""), makeTestSphere(r3.Vector{0, 0, 2}, 1, "")}, + [2]Geometry{makeTestSphere(r3.Vector{}, 1), makeTestSphere(r3.Vector{0, 0, 2}, 1)}, 0, }, { "separated spheres", - [2]Geometry{makeTestSphere(r3.Vector{}, 1, ""), makeTestSphere(r3.Vector{0, 0, 2 + 1e-3}, 1, "")}, + [2]Geometry{makeTestSphere(r3.Vector{}, 1), makeTestSphere(r3.Vector{0, 0, 2 + 1e-3}, 1)}, 1e-3, }, } @@ -301,64 +275,64 @@ func TestSphereVsBoxCollision(t *testing.T) { { "separated face closest", [2]Geometry{ - makeTestSphere(r3.Vector{0, 0, 2 + 1e-3}, 1, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestSphere(r3.Vector{0, 0, 2 + 1e-3}, 1), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, 1e-3, }, { "separated edge closest", [2]Geometry{ - makeTestSphere(r3.Vector{0, 2, 2}, 1, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestSphere(r3.Vector{0, 2, 2}, 1), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, math.Sqrt2 - 1, }, { "separated vertex closest", [2]Geometry{ - makeTestSphere(r3.Vector{2, 2, 2}, 1, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestSphere(r3.Vector{2, 2, 2}, 1), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, math.Sqrt(3) - 1, }, { "face tangent", [2]Geometry{ - makeTestSphere(r3.Vector{0, 0, 2}, 1, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestSphere(r3.Vector{0, 0, 2}, 1), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, 0, }, { "edge tangent", [2]Geometry{ - makeTestSphere(r3.Vector{0, 2, 2}, math.Sqrt2, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestSphere(r3.Vector{0, 2, 2}, math.Sqrt2), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, 0, }, { "vertex tangent", [2]Geometry{ - makeTestSphere(r3.Vector{2, 2, 2}, math.Sqrt(3), ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestSphere(r3.Vector{2, 2, 2}, math.Sqrt(3)), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, 0, }, { "center point inside", [2]Geometry{ - makeTestSphere(r3.Vector{-.2, 0.1, .75}, 1, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestSphere(r3.Vector{-.2, 0.1, .75}, 1), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, -1.25, }, { "inscribed", [2]Geometry{ - makeTestSphere(r3.Vector{2, 2, 2}, 1, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{2, 2, 2}, r3.Vector{2, 2, 2}, ""), + makeTestSphere(r3.Vector{2, 2, 2}, 1), + makeTestBox(NewZeroOrientation(), r3.Vector{2, 2, 2}, r3.Vector{2, 2, 2}), }, -2, }, @@ -372,7 +346,7 @@ func TestPointVsBoxCollision(t *testing.T) { "separated face closest", [2]Geometry{ NewPoint(r3.Vector{2, 0, 0}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, 1, }, @@ -380,7 +354,7 @@ func TestPointVsBoxCollision(t *testing.T) { "separated edge closest", [2]Geometry{ NewPoint(r3.Vector{2, 2, 0}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, math.Sqrt2, }, @@ -388,7 +362,7 @@ func TestPointVsBoxCollision(t *testing.T) { "separated vertex closest", [2]Geometry{ NewPoint(r3.Vector{2, 2, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, math.Sqrt(3), }, @@ -396,7 +370,7 @@ func TestPointVsBoxCollision(t *testing.T) { "inside", [2]Geometry{ NewPoint(r3.Vector{0, 0.3, 0.5}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, -0.5, }, @@ -410,7 +384,7 @@ func TestPointVsSphereCollision(t *testing.T) { "coincident", [2]Geometry{ NewPoint(r3.Vector{}, ""), - makeTestSphere(r3.Vector{}, 1, ""), + makeTestSphere(r3.Vector{}, 1), }, -1, }, @@ -418,7 +392,7 @@ func TestPointVsSphereCollision(t *testing.T) { "separated", [2]Geometry{ NewPoint(r3.Vector{2, 0, 0}, ""), - makeTestSphere(r3.Vector{}, 1, ""), + makeTestSphere(r3.Vector{}, 1), }, 1, }, @@ -446,16 +420,16 @@ func TestBoxVsBoxEncompassed(t *testing.T) { { "encompassed", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, 0, }, { "not encompassed", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{0, 1, 0}, r3.Vector{2, 3, 2}, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 1, 0}, r3.Vector{2, 3, 2}), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, 1, }, @@ -468,16 +442,16 @@ func TestBoxVsSphereEncompassed(t *testing.T) { { "encompassed", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), - makeTestSphere(r3.Vector{}, math.Sqrt(3), ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), + makeTestSphere(r3.Vector{}, math.Sqrt(3)), }, 0, }, { "not encompassed", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{0, 1, 0}, r3.Vector{2, 2.1, 2}, ""), - makeTestSphere(r3.Vector{}, math.Sqrt(3), ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 1, 0}, r3.Vector{2, 2.1, 2}), + makeTestSphere(r3.Vector{}, math.Sqrt(3)), }, .1, }, @@ -489,7 +463,7 @@ func TestBoxVsPointEncompassed(t *testing.T) { cases := []geometryComparisonTestCase{ { "coincident", - [2]Geometry{makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{1, 1, 1}, ""), NewPoint(r3.Vector{}, "")}, + [2]Geometry{makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{1, 1, 1}), NewPoint(r3.Vector{}, "")}, math.Sqrt(3), }, } @@ -501,16 +475,16 @@ func TestSphereVsBoxEncompassed(t *testing.T) { { "encompassed", [2]Geometry{ - makeTestSphere(r3.Vector{3, 0, 0}, 1, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{8, 8, 8}, ""), + makeTestSphere(r3.Vector{3, 0, 0}, 1), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{8, 8, 8}), }, 0, }, { "not encompassed", [2]Geometry{ - makeTestSphere(r3.Vector{3.5, 0, 0}, 1, ""), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{8, 8, 8}, ""), + makeTestSphere(r3.Vector{3.5, 0, 0}, 1), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{8, 8, 8}), }, 0.5, }, @@ -523,16 +497,16 @@ func TestSphereVsSphereEncompassed(t *testing.T) { { "encompassed", [2]Geometry{ - makeTestSphere(r3.Vector{3, 0, 0}, 1, ""), - makeTestSphere(r3.Vector{}, 4, ""), + makeTestSphere(r3.Vector{3, 0, 0}, 1), + makeTestSphere(r3.Vector{}, 4), }, 0, }, { "not encompassed", [2]Geometry{ - makeTestSphere(r3.Vector{3, 0, 0}, 1, ""), - makeTestSphere(r3.Vector{}, 3.5, ""), + makeTestSphere(r3.Vector{3, 0, 0}, 1), + makeTestSphere(r3.Vector{}, 3.5), }, 0.5, }, @@ -544,7 +518,7 @@ func TestSphereVsPointEncompassed(t *testing.T) { cases := []geometryComparisonTestCase{ { "coincident", - [2]Geometry{makeTestSphere(r3.Vector{}, 1, ""), NewPoint(r3.Vector{}, "")}, + [2]Geometry{makeTestSphere(r3.Vector{}, 1), NewPoint(r3.Vector{}, "")}, 1, }, } @@ -557,7 +531,7 @@ func TestCapsuleVsBoxCollision(t *testing.T) { "separated face closest", [2]Geometry{ makeTestCapsule(NewZeroOrientation(), r3.Vector{0, 0, 3 + 1e-3}, 1, 4), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, 1e-3, }, @@ -565,7 +539,7 @@ func TestCapsuleVsBoxCollision(t *testing.T) { "separated edge closest", [2]Geometry{ makeTestCapsule(&OrientationVector{0, 0, 1, 1}, r3.Vector{0, 4, 4}, 1, 4*math.Sqrt2), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, math.Sqrt2, }, @@ -573,7 +547,7 @@ func TestCapsuleVsBoxCollision(t *testing.T) { "separated vertex closest", [2]Geometry{ makeTestCapsule(&OrientationVector{0, 2, 2, 2}, r3.Vector{4, 4, 4}, 1, 4*math.Sqrt(3)), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, math.Sqrt(3), }, @@ -581,7 +555,7 @@ func TestCapsuleVsBoxCollision(t *testing.T) { "face tangent", [2]Geometry{ makeTestCapsule(NewZeroOrientation(), r3.Vector{0, 0, 3}, 1, 4), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, 0, }, @@ -589,7 +563,7 @@ func TestCapsuleVsBoxCollision(t *testing.T) { "edge tangent to capsule cylinder", [2]Geometry{ makeTestCapsule(&OrientationVector{0, 0, -2, 2}, r3.Vector{0, 3, 0}, math.Sqrt2/2, 6), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, 0, }, @@ -597,7 +571,7 @@ func TestCapsuleVsBoxCollision(t *testing.T) { "center line segment inside", [2]Geometry{ makeTestCapsule(NewZeroOrientation(), r3.Vector{0.3, 0.3, -0.75}, 1, 4), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, -1.7, }, @@ -605,7 +579,7 @@ func TestCapsuleVsBoxCollision(t *testing.T) { "inscribed", [2]Geometry{ makeTestCapsule(NewZeroOrientation(), r3.Vector{0, 0, 0}, 1, 40), - makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 1}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{0, 0, 1}, r3.Vector{2, 2, 2}), }, -2, }, @@ -622,7 +596,7 @@ func TestCapsuleVsBoxCollision(t *testing.T) { "colliding face closest", [2]Geometry{ makeTestCapsule(&OrientationVector{0, norm.X, norm.Y, norm.Z}, r3.Vector{adjust(norm.X), adjust(norm.Y), adjust(norm.Z)}, 1, 4), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{2, 2, 2}), }, -1e-3, }, @@ -699,7 +673,7 @@ func TestCapsuleVsBoxEncompassed(t *testing.T) { "encompassed", [2]Geometry{ makeTestCapsule(NewZeroOrientation(), r3.Vector{0, 0, 3}, 1, 4.75), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{16, 16, 16}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{16, 16, 16}), }, 0, }, @@ -707,14 +681,14 @@ func TestCapsuleVsBoxEncompassed(t *testing.T) { "not encompassed", [2]Geometry{ makeTestCapsule(NewZeroOrientation(), r3.Vector{0, 0, 5.875}, 1, 4.75), - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{16, 16, 16}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{16, 16, 16}), }, 0.25, }, { "encompassed box", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{4, 4, 4}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{4, 4, 4}), makeTestCapsule(NewZeroOrientation(), r3.Vector{0, 0, 0}, 4, 10), }, 0, @@ -722,7 +696,7 @@ func TestCapsuleVsBoxEncompassed(t *testing.T) { { "not encompassed box", [2]Geometry{ - makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{16, 16, 16}, ""), + makeTestBox(NewZeroOrientation(), r3.Vector{}, r3.Vector{16, 16, 16}), makeTestCapsule(NewZeroOrientation(), r3.Vector{0, 0, 3.5}, 1, 4.75), }, 0.25, @@ -737,7 +711,7 @@ func TestCapsuleVsSphereEncompassed(t *testing.T) { "encompassed", [2]Geometry{ makeTestCapsule(NewZeroOrientation(), r3.Vector{0, 0, 0.1}, 1, 6.75), - makeTestSphere(r3.Vector{}, 4, ""), + makeTestSphere(r3.Vector{}, 4), }, 0, }, @@ -745,14 +719,14 @@ func TestCapsuleVsSphereEncompassed(t *testing.T) { "not encompassed", [2]Geometry{ makeTestCapsule(NewZeroOrientation(), r3.Vector{0, 0, 3}, 1, 6.75), - makeTestSphere(r3.Vector{}, 3.5, ""), + makeTestSphere(r3.Vector{}, 3.5), }, 0.5, }, { "encompassed sphere", [2]Geometry{ - makeTestSphere(r3.Vector{}, 2, ""), + makeTestSphere(r3.Vector{}, 2), makeTestCapsule(NewZeroOrientation(), r3.Vector{0, 0, 1.5}, 2.5, 9.75), }, 0, @@ -760,7 +734,7 @@ func TestCapsuleVsSphereEncompassed(t *testing.T) { { "not encompassed sphere", [2]Geometry{ - makeTestSphere(r3.Vector{}, 3.5, ""), + makeTestSphere(r3.Vector{}, 3.5), makeTestCapsule(NewZeroOrientation(), r3.Vector{0, 0, 3}, 1, 6.75), }, 0.5, @@ -801,24 +775,3 @@ func TestCapsuleVsPointEncompassed(t *testing.T) { } testGeometryEncompassed(t, cases) } - -func TestNewGeometryFromProto(t *testing.T) { - malformedGeom := commonpb.Geometry{} - viamGeom, err := NewGeometryFromProto(&malformedGeom) - test.That(t, viamGeom, test.ShouldBeNil) - test.That(t, err, test.ShouldBeError, errors.New("cannot have nil pose for geometry")) - - properGeom := commonpb.Geometry{ - Center: &commonpb.Pose{OZ: 1}, - GeometryType: &commonpb.Geometry_Sphere{ - Sphere: &commonpb.Sphere{ - RadiusMm: 1, - }, - }, - } - viamGeom, err = NewGeometryFromProto(&properGeom) - test.That(t, err, test.ShouldBeNil) - sphereGeom, err := NewSphere(NewZeroPose(), 1, "") - test.That(t, err, test.ShouldBeNil) - test.That(t, viamGeom, test.ShouldResemble, sphereGeom) -} diff --git a/spatialmath/mesh.go b/spatialmath/mesh.go index 9a01cfc77b0..6c360c4f510 100644 --- a/spatialmath/mesh.go +++ b/spatialmath/mesh.go @@ -113,7 +113,8 @@ func newMeshFromBytes(pose Pose, data []byte, label string) (mesh *Mesh, err err }, nil } -func newMeshFromProto(pose Pose, m *commonpb.Mesh, label string) (*Mesh, error) { +// NewMeshFromProto creates a new mesh from a protobuf mesh. +func NewMeshFromProto(pose Pose, m *commonpb.Mesh, label string) (*Mesh, error) { switch m.ContentType { case string(plyType): return newMeshFromBytes(pose, m.Mesh, label) diff --git a/spatialmath/mesh_test.go b/spatialmath/mesh_test.go index 7437fe7ae42..91d94d661e2 100644 --- a/spatialmath/mesh_test.go +++ b/spatialmath/mesh_test.go @@ -6,16 +6,13 @@ import ( "github.com/golang/geo/r3" "go.viam.com/test" - - "go.viam.com/rdk/utils" ) -func makeTestMesh(o Orientation, pt r3.Vector, triangles []*Triangle) *Mesh { +func makeTestMesh(o Orientation, pt r3.Vector, triangles []*Triangle) Geometry { return NewMesh(NewPose(pt, o), triangles, "") } -func makeSimpleTriangleMesh() *Mesh { - // Create a simple triangle mesh at origin +func makeSimpleTriangleMesh() Geometry { tri1 := NewTriangle( r3.Vector{X: 0, Y: 0, Z: 0}, r3.Vector{X: 1, Y: 0, Z: 0}, @@ -49,20 +46,6 @@ func TestNewMesh(t *testing.T) { test.That(t, len(mesh.Triangles()), test.ShouldEqual, 1) } -func TestMeshProtoConversion(t *testing.T) { - m, err := NewMeshFromPLYFile(utils.ResolveFile("spatialmath/data/simple.ply")) - test.That(t, err, test.ShouldBeNil) - m2, err := NewGeometryFromProto(m.ToProtobuf()) - test.That(t, err, test.ShouldBeNil) - - test.That(t, PoseAlmostEqual(m.Pose(), m2.Pose()), test.ShouldBeTrue) - test.That(t, m.Label(), test.ShouldResemble, m2.Label()) - test.That(t, len(m.Triangles()), test.ShouldEqual, 2) - test.That(t, len(m2.(*Mesh).Triangles()), test.ShouldEqual, 2) - test.That(t, m.Triangles()[0], test.ShouldResemble, m2.(*Mesh).Triangles()[0]) - test.That(t, m.Triangles()[1], test.ShouldResemble, m2.(*Mesh).Triangles()[1]) -} - func TestMeshTransform(t *testing.T) { mesh := makeSimpleTriangleMesh() @@ -630,120 +613,6 @@ func TestMeshEncompassedBy(t *testing.T) { test.That(t, encompassed, test.ShouldBeFalse) } -func TestMeshProtoConversionFromTriangles(t *testing.T) { - // Manually create a mesh with a variety of shapes and shared vertices - triangles := []*Triangle{ - NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 0}, - r3.Vector{X: 1000, Y: 0, Z: 0}, - r3.Vector{X: 0, Y: 1000, Z: 0}, - ), - NewTriangle( - r3.Vector{X: -500, Y: -500, Z: 0}, - r3.Vector{X: 500, Y: -500, Z: 0}, - r3.Vector{X: 0, Y: 500, Z: 0}, - ), - NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 1000}, - r3.Vector{X: 1000, Y: 0, Z: 1000}, - r3.Vector{X: 500, Y: 1000, Z: 1000}, - ), - NewTriangle( - r3.Vector{X: 123.456, Y: 789.012, Z: 345.678}, - r3.Vector{X: 456.789, Y: 123.456, Z: 678.901}, - r3.Vector{X: 789.012, Y: 456.789, Z: 123.456}, - ), - NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 0}, - r3.Vector{X: 10000, Y: 0, Z: 0}, - r3.Vector{X: 0, Y: 10000, Z: 0}, - ), - NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 0}, - r3.Vector{X: 1, Y: 0, Z: 0}, - r3.Vector{X: 0, Y: 1, Z: 0}, - ), - NewTriangle( - r3.Vector{X: -1000, Y: -1000, Z: -1000}, - r3.Vector{X: -500, Y: -1000, Z: -1000}, - r3.Vector{X: -1000, Y: -500, Z: -1000}, - ), - NewTriangle( - r3.Vector{X: 100, Y: 100, Z: -500}, - r3.Vector{X: 200, Y: 100, Z: 500}, - r3.Vector{X: 150, Y: 200, Z: 0}, - ), - NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 0}, - r3.Vector{X: 1000, Y: 0, Z: 0}, - r3.Vector{X: 1000, Y: 1000, Z: 0}, - ), - NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 0}, - r3.Vector{X: 1000, Y: 1000, Z: 0}, - r3.Vector{X: 0, Y: 1000, Z: 0}, - ), - NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 0}, - r3.Vector{X: 0, Y: 1000, Z: 0}, - r3.Vector{X: 0, Y: 1000, Z: 1000}, - ), - NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 0}, - r3.Vector{X: 0, Y: 1000, Z: 1000}, - r3.Vector{X: 0, Y: 0, Z: 1000}, - ), - } - - // Create mesh with a pose and label - originalPose := NewPose(r3.Vector{X: 100, Y: 200, Z: 300}, NewZeroOrientation()) - originalMesh := NewMesh(originalPose, triangles, "test_mesh_from_triangles") - - // Convert to protobuf - proto := originalMesh.ToProtobuf() - test.That(t, proto, test.ShouldNotBeNil) - test.That(t, proto.Label, test.ShouldEqual, "test_mesh_from_triangles") - - // Restore from protobuf - restoredGeometry, err := NewGeometryFromProto(proto) - test.That(t, err, test.ShouldBeNil) - restoredMesh, ok := restoredGeometry.(*Mesh) - test.That(t, ok, test.ShouldBeTrue) - - test.That(t, restoredMesh.Label(), test.ShouldEqual, originalMesh.Label()) - test.That(t, PoseAlmostEqual(restoredMesh.Pose(), originalMesh.Pose()), test.ShouldBeTrue) - test.That(t, len(restoredMesh.Triangles()), test.ShouldEqual, len(originalMesh.Triangles())) - - // Verify all triangles match - originalTriangles := originalMesh.Triangles() - restoredTriangles := restoredMesh.Triangles() - for i, originalTri := range originalTriangles { - restoredTri := restoredTriangles[i] - origPoints := originalTri.Points() - restoredPoints := restoredTri.Points() - - test.That(t, len(restoredPoints), test.ShouldEqual, len(origPoints)) - - for j, origPoint := range origPoints { - restoredPoint := restoredPoints[j] - // The conversion from mm to meters and back can create micrometer-level float changes - epsilon := 1e-4 - test.That(t, math.Abs(origPoint.X-restoredPoint.X), test.ShouldBeLessThan, epsilon) - test.That(t, math.Abs(origPoint.Y-restoredPoint.Y), test.ShouldBeLessThan, epsilon) - test.That(t, math.Abs(origPoint.Z-restoredPoint.Z), test.ShouldBeLessThan, epsilon) - } - } - - // Verify that the mesh can be converted to protobuf again - secondProto := restoredMesh.ToProtobuf() - test.That(t, secondProto, test.ShouldNotBeNil) - test.That(t, secondProto.Label, test.ShouldEqual, originalMesh.Label()) - - // Verify the protobuf content is the same - test.That(t, secondProto.GetMesh().ContentType, test.ShouldEqual, proto.GetMesh().ContentType) - test.That(t, len(secondProto.GetMesh().Mesh), test.ShouldEqual, len(proto.GetMesh().Mesh)) -} - func TestBoxTriangleIntersectionArea(t *testing.T) { b, err := NewBox(NewZeroPose(), r3.Vector{X: 2, Y: 2, Z: 2}, "") bbox, ok := b.(*box) diff --git a/spatialmath/sphere_test.go b/spatialmath/sphere_test.go index a9bd025e09f..2a1c723485f 100644 --- a/spatialmath/sphere_test.go +++ b/spatialmath/sphere_test.go @@ -8,8 +8,8 @@ import ( "go.viam.com/test" ) -func makeTestSphere(point r3.Vector, radius float64, label string) Geometry { - sphere, _ := NewSphere(NewPoseFromPoint(point), radius, label) +func makeTestSphere(point r3.Vector, radius float64) Geometry { + sphere, _ := NewSphere(NewPoseFromPoint(point), radius, "") return sphere } @@ -31,9 +31,9 @@ func TestNewSphere(t *testing.T) { } func TestSphereAlmostEqual(t *testing.T) { - original := makeTestSphere(r3.Vector{}, 1, "") - good := makeTestSphere(r3.Vector{1e-16, 1e-16, 1e-16}, 1+1e-16, "") - bad := makeTestSphere(r3.Vector{1e-2, 1e-2, 1e-2}, 1+1e-2, "") + original := makeTestSphere(r3.Vector{}, 1) + good := makeTestSphere(r3.Vector{1e-16, 1e-16, 1e-16}, 1+1e-16) + bad := makeTestSphere(r3.Vector{1e-2, 1e-2, 1e-2}, 1+1e-2) test.That(t, original.(*sphere).almostEqual(good), test.ShouldBeTrue) test.That(t, original.(*sphere).almostEqual(bad), test.ShouldBeFalse) } diff --git a/vision/object.go b/vision/object.go index 0b8744e1829..8cb298f518e 100644 --- a/vision/object.go +++ b/vision/object.go @@ -7,6 +7,7 @@ import ( commonpb "go.viam.com/api/common/v1" pc "go.viam.com/rdk/pointcloud" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -37,7 +38,7 @@ func NewObjectWithLabel(cloud pc.PointCloud, label string, geometry *commonpb.Ge if label != "" { // will override geometry proto label with given label (unless empty) geometry.Label = label } - geom, err := spatialmath.NewGeometryFromProto(geometry) + geom, err := referenceframe.NewGeometryFromProto(geometry) if err != nil { return nil, err }