APP-8776 Move geometry proto conversions to reference frame

components/arm/client.go

@@ -5,7 +5,7 @@
 	"context"
 	"sync"
 
 	commonpb "go.viam.com/api/common/v1"
 	pb "go.viam.com/api/component/arm/v1"
 	"go.viam.com/utils/protoutils"
 	"go.viam.com/utils/rpc"
@@ -218,7 +218,7 @@
 	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"
 	"go.viam.com/rdk/rimage/transform"
@@ -338,7 +339,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

@@ -15,9 +15,9 @@ 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"
-	"go.viam.com/rdk/spatialmath"
 	"go.viam.com/rdk/utils"
 )
 
@@ -285,5 +285,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

@@ -7,7 +7,7 @@
 
 	"github.com/golang/geo/r3"
 	"github.com/pkg/errors"
 	packagespb "go.viam.com/api/app/packages/v1"
 	pb "go.viam.com/api/app/v1"
 	"go.viam.com/utils"
 	"go.viam.com/utils/pexec"
@@ -519,7 +519,7 @@
 	}
 
 	if proto.GetGeometry() != nil {
-		geom, err := spatial.NewGeometryFromProto(proto.GetGeometry())
+		geom, err := referenceframe.NewGeometryFromProto(proto.GetGeometry())
 		if err != nil {
 			return nil, err
 		}

go.mod

@@ -455,3 +455,6 @@ require (
 	github.com/ziutek/mymysql v1.5.4 // indirect
 	golang.org/x/exp v0.0.0-20240904232852-e7e105dedf7e
 )
+
+// TODO: remove this when the api changes are merged
+replace go.viam.com/api => ../api

go.sum

@@ -1536,8 +1536,6 @@ 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.460 h1:8qNFCjtgX5p954tNJ8iZr1igCMn3szkn7LxwpKau5Bs=
-go.viam.com/api v0.1.460/go.mod h1:gwJriv6EVWe97uFzzzWjzP3NPfpCrKtRAdWtYglUpqs=
 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.154 h1:5rva5GcTDIQ39NeTwj3wO2fKUp1DARCX6IkNgD/dTbo=

motionplan/armplanning/motionPlanner.go

@@ -139,7 +139,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
 	}
@@ -323,7 +323,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/frame_system_test.go

@@ -42,15 +42,13 @@ func TestFrameModelPart(t *testing.T) {
 	pose := &commonpb.Pose{} // zero pose
 	exp := &robotpb.FrameSystemConfig{
 		Frame: &commonpb.Transform{
-			ReferenceFrame: "test",
-			PoseInObserverFrame: &commonpb.PoseInFrame{
-				ReferenceFrame: "world",
-				Pose:           pose,
-			},
+			Name:   "test",
+			Parent: "world",
+			Pose:   pose,
 		},
 	}
-	test.That(t, result.Frame.ReferenceFrame, test.ShouldEqual, exp.Frame.ReferenceFrame)
-	test.That(t, result.Frame.PoseInObserverFrame, test.ShouldResemble, exp.Frame.PoseInObserverFrame)
+	test.That(t, result.Frame.Name, test.ShouldEqual, exp.Frame.Name)
+	test.That(t, result.Frame.Pose, test.ShouldResemble, exp.Frame.Pose)
 	// exp.Kinematics is nil, but the struct in the struct PB
 	expKin, err := protoutils.StructToStructPb(exp.Kinematics)
 	test.That(t, err, test.ShouldBeNil)
@@ -86,15 +84,13 @@ func TestFrameModelPart(t *testing.T) {
 	pose = &commonpb.Pose{X: 1, Y: 2, Z: 3, OZ: 1, Theta: 0}
 	exp = &robotpb.FrameSystemConfig{
 		Frame: &commonpb.Transform{
-			ReferenceFrame: "test",
-			PoseInObserverFrame: &commonpb.PoseInFrame{
-				ReferenceFrame: "world",
-				Pose:           pose,
-			},
+			Name:   "test",
+			Parent: "world",
+			Pose:   pose,
 		},
 	}
-	test.That(t, result.Frame.ReferenceFrame, test.ShouldEqual, exp.Frame.ReferenceFrame)
-	test.That(t, result.Frame.PoseInObserverFrame, test.ShouldResemble, exp.Frame.PoseInObserverFrame)
+	test.That(t, result.Frame.Name, test.ShouldEqual, exp.Frame.Name)
+	test.That(t, result.Frame.Pose, test.ShouldResemble, exp.Frame.Pose)
 	test.That(t, result.Kinematics, test.ShouldNotBeNil)
 	// return to FrameSystemPart
 	partAgain, err = ProtobufToFrameSystemPart(result)

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