Add geometries to camera

components/camera/camera.go

@@ -125,6 +125,7 @@ type ImageMetadata struct {
 // [Close method docs]: https://docs.viam.com/dev/reference/apis/components/camera/#close
 type Camera interface {
 	resource.Resource
+	resource.Shaped
 
 	// Image returns a byte slice representing an image that tries to adhere to the MIME type hint.
 	// Image also may return metadata about the frame.

components/camera/client.go

@@ -15,6 +15,7 @@ import (
 	"github.com/pion/rtp"
 	"github.com/viamrobotics/webrtc/v3"
 	"go.opencensus.io/trace"
+	commonpb "go.viam.com/api/common/v1"
 	pb "go.viam.com/api/component/camera/v1"
 	streampb "go.viam.com/api/stream/v1"
 	goutils "go.viam.com/utils"
@@ -32,6 +33,7 @@ import (
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/rimage"
 	"go.viam.com/rdk/rimage/transform"
+	"go.viam.com/rdk/spatialmath"
 	"go.viam.com/rdk/utils"
 )
 
@@ -324,6 +326,21 @@ func (c *client) DoCommand(ctx context.Context, cmd map[string]interface{}) (map
 	return protoutils.DoFromResourceClient(ctx, c.client, c.name, cmd)
 }
 
+func (c *client) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) {
+	ext, err := goprotoutils.StructToStructPb(extra)
+	if err != nil {
+		return nil, err
+	}
+	resp, err := c.client.GetGeometries(ctx, &commonpb.GetGeometriesRequest{
+		Name:  c.name,
+		Extra: ext,
+	})
+	if err != nil {
+		return nil, err
+	}
+	return spatialmath.NewGeometriesFromProto(resp.GetGeometries())
+}
+
 // TODO(RSDK-6433): This method can be called more than once during a client's lifecycle.
 // For example, consider a case where a remote camera goes offline and then back online.
 // We will call `Close` on the camera client when we detect the disconnection to remove

components/camera/client_test.go

@@ -11,6 +11,7 @@ import (
 	"testing"
 	"time"
 
+	"github.com/golang/geo/r3"
 	"github.com/pion/rtp"
 	"go.viam.com/test"
 	"go.viam.com/utils/rpc"
@@ -35,6 +36,7 @@ import (
 	robotimpl "go.viam.com/rdk/robot/impl"
 	"go.viam.com/rdk/robot/web"
 	weboptions "go.viam.com/rdk/robot/web/options"
+	"go.viam.com/rdk/spatialmath"
 	"go.viam.com/rdk/testutils"
 	"go.viam.com/rdk/testutils/inject"
 	"go.viam.com/rdk/testutils/robottestutils"
@@ -51,6 +53,7 @@ func TestClient(t *testing.T) {
 
 	injectCamera := &inject.Camera{}
 	img := image.NewNRGBA(image.Rect(0, 0, 4, 4))
+	expectedGeometries := []spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{1, 2, 3}, "")}
 
 	var imgBuf bytes.Buffer
 	test.That(t, png.Encode(&imgBuf, img), test.ShouldBeNil)
@@ -105,6 +108,9 @@ func TestClient(t *testing.T) {
 		test.That(t, err, test.ShouldBeNil)
 		return resBytes, camera.ImageMetadata{MimeType: mimeType}, nil
 	}
+	injectCamera.GeometriesFunc = func(context.Context, map[string]interface{}) ([]spatialmath.Geometry, error) {
+		return expectedGeometries, nil
+	}
 	// depth camera
 	injectCameraDepth := &inject.Camera{}
 	depthImg := rimage.NewEmptyDepthMap(10, 20)
@@ -219,6 +225,13 @@ func TestClient(t *testing.T) {
 		test.That(t, resp["command"], test.ShouldEqual, testutils.TestCommand["command"])
 		test.That(t, resp["data"], test.ShouldEqual, testutils.TestCommand["data"])
 
+		// Geometries
+		geometries, err := camera1Client.Geometries(context.Background(), map[string]interface{}{"foo": "Geometries"})
+		test.That(t, err, test.ShouldBeNil)
+		for i, geometry := range geometries {
+			test.That(t, spatialmath.GeometriesAlmostEqual(expectedGeometries[i], geometry), test.ShouldBeTrue)
+		}
+
 		test.That(t, camera1Client.Close(context.Background()), test.ShouldBeNil)
 		test.That(t, conn.Close(), test.ShouldBeNil)
 	})

components/camera/replaypcd/replaypcd.go

@@ -23,6 +23,7 @@ import (
 	"go.viam.com/rdk/logging"
 	"go.viam.com/rdk/pointcloud"
 	"go.viam.com/rdk/resource"
+	"go.viam.com/rdk/spatialmath"
 	"go.viam.com/rdk/utils/contextutils"
 )
 
@@ -350,6 +351,10 @@ func (replay *pcdCamera) Image(ctx context.Context, mimeType string, extra map[s
 	return nil, camera.ImageMetadata{}, errors.New("Image is unimplemented")
 }
 
+func (replay *pcdCamera) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) {
+	return make([]spatialmath.Geometry, 0), nil
+}
+
 // Close stops replay camera, closes the channels and its connections to the cloud.
 func (replay *pcdCamera) Close(ctx context.Context) error {
 	replay.mu.Lock()

components/camera/server.go

@@ -17,6 +17,7 @@ import (
 	"go.viam.com/rdk/protoutils"
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/rimage"
+	"go.viam.com/rdk/spatialmath"
 	"go.viam.com/rdk/utils"
 )
 
@@ -274,3 +275,15 @@ func (s *serviceServer) DoCommand(ctx context.Context,
 	}
 	return protoutils.DoFromResourceServer(ctx, camera, req)
 }
+
+func (s *serviceServer) GetGeometries(ctx context.Context, req *commonpb.GetGeometriesRequest) (*commonpb.GetGeometriesResponse, error) {
+	res, err := s.coll.Resource(req.GetName())
+	if err != nil {
+		return nil, err
+	}
+	geometries, err := res.Geometries(ctx, req.Extra.AsMap())
+	if err != nil {
+		return nil, err
+	}
+	return &commonpb.GetGeometriesResponse{Geometries: spatialmath.NewGeometriesToProto(geometries)}, nil
+}

components/camera/videosource/webcam.go

@@ -28,6 +28,7 @@ import (
 	"go.viam.com/rdk/rimage"
 	"go.viam.com/rdk/rimage/depthadapter"
 	"go.viam.com/rdk/rimage/transform"
+	"go.viam.com/rdk/spatialmath"
 	"go.viam.com/rdk/utils"
 )
 
@@ -477,6 +478,10 @@ func (c *webcam) Properties(ctx context.Context) (camera.Properties, error) {
 	}, nil
 }
 
+func (c *webcam) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) {
+	return make([]spatialmath.Geometry, 0), nil
+}
+
 func (c *webcam) Close(ctx context.Context) error {
 	c.mu.Lock()
 	if c.closed {

components/camera/videosourcewrappers.go

@@ -16,6 +16,7 @@ import (
 	"go.viam.com/rdk/rimage"
 	"go.viam.com/rdk/rimage/depthadapter"
 	"go.viam.com/rdk/rimage/transform"
+	"go.viam.com/rdk/spatialmath"
 	"go.viam.com/rdk/utils"
 )
 
@@ -317,3 +318,10 @@ func (vs *videoSource) Close(ctx context.Context) error {
 	}
 	return vs.videoSource.Close(ctx)
 }
+
+func (vs *videoSource) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) {
+	if res, ok := vs.actualSource.(resource.Shaped); ok {
+		return res.Geometries(ctx, extra)
+	}
+	return nil, errors.New("videoSource: geometries unavailable")
+}

testutils/inject/camera.go

@@ -10,6 +10,7 @@ import (
 	"go.viam.com/rdk/pointcloud"
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/rimage/transform"
+	"go.viam.com/rdk/spatialmath"
 )
 
 // Camera is an injected camera.
@@ -24,6 +25,7 @@ type Camera struct {
 	ProjectorFunc        func(ctx context.Context) (transform.Projector, error)
 	PropertiesFunc       func(ctx context.Context) (camera.Properties, error)
 	CloseFunc            func(ctx context.Context) error
+	GeometriesFunc       func(context.Context, map[string]interface{}) ([]spatialmath.Geometry, error)
 }
 
 // NewCamera returns a new injected camera.
@@ -98,6 +100,14 @@ func (c *Camera) DoCommand(ctx context.Context, cmd map[string]interface{}) (map
 	return c.Camera.DoCommand(ctx, cmd)
 }
 
+// Geometries calls the injected Geometries or the real version.
+func (c *Camera) Geometries(ctx context.Context, cmd map[string]interface{}) ([]spatialmath.Geometry, error) {
+	if c.GeometriesFunc != nil {
+		return c.GeometriesFunc(ctx, cmd)
+	}
+	return c.Camera.Geometries(ctx, cmd)
+}
+
 // SubscribeRTP calls the injected RTPPassthroughSource or returns an error if unimplemented.
 func (c *Camera) SubscribeRTP(
 	ctx context.Context,