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"
@@ -558,7 +558,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

@@ -444,3 +444,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

@@ -1513,8 +1513,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.455 h1:WY++sFAydRGcg/E1va85BFPi9U7IuEf2Dk0JrPZ1Wzs=
-go.viam.com/api v0.1.455/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.151 h1:VHKoE3ZrltrN5ej/Fq27aztWQuk5mPApfXEm7DlUIfk=

motionplan/motionPlanner.go

@@ -132,7 +132,7 @@ func (req *PlanRequest) validatePlanRequest(fs *referenceframe.FrameSystem) erro
 		req.WorldState = newWS
 	}
 
-	boundingRegions, err := spatialmath.NewGeometriesFromProto(req.BoundingRegions)
+	boundingRegions, err := referenceframe.NewGeometriesFromProto(req.BoundingRegions)
 	if err != nil {
 		return err
 	}
@@ -316,7 +316,7 @@ func newPlannerFromPlanRequest(logger logging.Logger, fs *referenceframe.FrameSy
 		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,27 @@ 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
+	pc := NewBasicPointCloud(octree.Size())
+	octree.Iterate(0, 0, func(p r3.Vector, d Data) bool {
+		err := pc.Set(p, d)
+		return err == nil
+	})
+
+	bytes, err := ToBytes(pc)
+	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 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
+}

pointcloud/testutils.go

@@ -0,0 +1,30 @@
+package pointcloud
+
+import (
+	"github.com/golang/geo/r3"
+)
+
+// MakeTestPointCloud creates a test point cloud with 3 points.
+func MakeTestPointCloud(label string) *BasicOctree {
+	pc := NewBasicPointCloud(3)
+	err := pc.Set(r3.Vector{X: 0, Y: 0, Z: 0}, NewBasicData())
+	if err != nil {
+		return nil
+	}
+	err = pc.Set(r3.Vector{X: 1, Y: 0, Z: 0}, NewBasicData())
+	if err != nil {
+		return nil
+	}
+	err = pc.Set(r3.Vector{X: 0, Y: 1, Z: 0}, NewBasicData())
+	if err != nil {
+		return nil
+	}
+
+	octree, err := ToBasicOctree(pc, 50)
+	if err != nil {
+		return nil
+	}
+
+	octree.SetLabel(label)
+	return octree
+}

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)