APP-8776 Move geometry proto conversions to reference frame

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

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.

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())
 }

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.

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)
 

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.

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
 }

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

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

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
 		}

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

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=

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
 	}

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
 	}

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.

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
+}

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
+}