Camera client updates

protos/farm_ng/oak/oak.proto

@@ -1,19 +1,17 @@
-// Copyright (c) farm-ng, inc.
-//
-// Use of this source code is governed by an MIT-style
-// license that can be found in the LICENSE file or at
-// https://opensource.org/licenses/MIT.
+// Copyright (c) farm-ng, inc. All rights reserved.
 
 syntax = "proto3";
 
 package farm_ng.oak.proto;
 
 service OakService {
+  rpc cameraControl(CameraControlRequest) returns (CameraControlReply) {}
   rpc streamFrames(StreamFramesRequest) returns (stream StreamFramesReply) {}
   rpc getServiceState(GetServiceStateRequest) returns (GetServiceStateResult) {}
   rpc startService(StartServiceRequest) returns (StartServiceResult) {}
   rpc stopService(StopServiceRequest) returns (StopServiceResult) {}
   rpc pauseService(PauseServiceRequest) returns (PauseServiceResult) {}
+  rpc getCalibration(GetCalibrationRequest) returns (GetCalibrationResult) {}
 }
 
 enum ReplyStatus {
@@ -29,6 +27,15 @@ enum OakServiceState {
   UNAVAILABLE = 4;
 }
 
+message GetCalibrationRequest {
+  string message = 1;
+}
+
+message GetCalibrationResult {
+  OakCalibration calibration = 1;
+  ReplyStatus status = 2;
+}
+
 message StopServiceRequest {
   string message = 1;
 }
@@ -66,6 +73,18 @@ message GetServiceStateResult {
   ReplyStatus status = 3;
 }
 
+message CameraControlRequest {
+  CameraSettings stereo_settings = 1;
+  CameraSettings rgb_settings = 2;
+}
+
+
+message CameraControlReply {
+   ReplyStatus status = 1;
+   CameraSettings stereo_settings = 2;
+   CameraSettings rgb_settings = 3;
+}
+
 message StreamFramesRequest {
   int32 every_n = 1; // desired stream frame rate.
 }
@@ -165,3 +184,57 @@ message OakDataSample {
   OakSyncFrame frame = 1;
   Metadata metadata = 2;
 }
+
+message RotationMatrix {
+  repeated double rotation_matrix = 1;
+}
+
+message Vector3d {
+  double x = 1;
+  double y = 2;
+  double z = 3;
+}
+
+message Extrinsics {
+  repeated double rotation_matrix = 1;
+  Vector3d spec_translation = 2;
+  int32 to_camera_socket = 3;
+  Vector3d translation = 4;
+}
+
+
+message CameraData {
+  uint32 camera_number = 1;
+  int32 camera_type = 2;
+  repeated double distortion_coeff = 3;
+  Extrinsics extrinsics = 4;
+  uint32 height = 5;
+  repeated double intrinsic_matrix = 6;
+  uint32 lens_position = 7;
+  double spec_hfov_deg = 8;
+  uint32 width = 9;
+}
+
+message StereoRectificationData {
+  uint32 left_camera_socket = 1;
+  repeated double rectified_rotation_left = 2;
+  repeated double rectified_rotation_right = 3;
+  uint32 right_camera_socket = 4;
+}
+
+message OakCalibration {
+  string batch_name = 1;
+  int32 batch_time = 2;
+  string board_conf = 3;
+  string board_custom = 4;
+  string board_name = 5;
+  int32 board_options = 6;
+  string board_rev = 7;
+  repeated CameraData camera_data = 8;
+  string hardware_conf = 9;
+  Extrinsics imu_extrinsics = 10;
+  repeated string miscellaneous_data = 11;
+  string product_name = 12;
+  StereoRectificationData stereo_rectification_data = 13;
+  uint32 version = 14;
+}

py/farm_ng/oak/client.py

@@ -1,4 +1,6 @@
+import asyncio
 import logging
+import time
 from dataclasses import dataclass
 
 import grpc
@@ -7,8 +9,43 @@
 
 __all__ = ["OakCameraClientConfig", "OakCameraClient", "OakCameraServiceState"]
 
+logging.basicConfig(level=logging.INFO)
 
-logging.basicConfig(level=logging.DEBUG)
+
+class RateLimiter(object):
+    def __init__(self, period):
+        self.last_call = None
+        self.period = period
+        self.outstanding_call = False
+        self.args = None
+        self.kargs = None
+
+    def wrapper(self, func):
+        self.last_call = time.monotonic()
+        self.outstanding_call = False
+        func(*self.args, **self.kargs)
+
+    def __call__(self, func):
+        """Return a wrapped function that can only be called once per frequency where the most recent call will be
+        executed."""
+
+        def async_wrapper(*args, **kargs):
+            self.args = args
+            self.kargs = kargs
+            delay = self.next_call_wait()
+            if delay < 0:
+                self.wrapper(func)
+            else:
+                if not self.outstanding_call:
+                    asyncio.get_running_loop().call_later(delay, self.wrapper, func)
+                    self.outstanding_call = True
+
+        return async_wrapper
+
+    def next_call_wait(self):
+        if self.last_call is None:
+            return -1
+        return self.period - (time.monotonic() - self.last_call)
 
 
 @dataclass
@@ -18,6 +55,8 @@ class OakCameraClientConfig:
     Attributes:
         port (int): the port to connect to the server.
         address (str): the address to connect to the server.
+        # TODO rename update_state_frequency to update_state_period
+        update_state_frequency (float): period between queries for the service state
     """
 
     port: int  # the port of the server address
@@ -73,11 +112,44 @@ def __init__(self, config: OakCameraClientConfig) -> None:
         self.channel = grpc.aio.insecure_channel(self.server_address)
         self.stub = oak_pb2_grpc.OakServiceStub(self.channel)
 
+        self._state = OakCameraServiceState()
+
+        self._mono_camera_settings = oak_pb2.CameraSettings(auto_exposure=True)
+        self._rgb_camera_settings = oak_pb2.CameraSettings(auto_exposure=True)
+
+        self.needs_update = False
+
+    @property
+    def state(self) -> OakCameraServiceState:
+        return self._state
+
     @property
     def server_address(self) -> str:
         """Returns the composed address and port."""
         return f"{self.config.address}:{self.config.port}"
 
+    @property
+    def rgb_settings(self) -> str:
+        return self._rgb_camera_settings
+
+    @property
+    def mono_settings(self) -> str:
+        return self._mono_camera_settings
+
+    def settings_reply(self, reply) -> None:
+        if reply.status == oak_pb2.ReplyStatus.OK:
+            self._mono_camera_settings.CopyFrom(reply.stereo_settings)
+            self._rgb_camera_settings.CopyFrom(reply.rgb_settings)
+
+    async def _poll_service_state(self) -> None:
+        while True:
+            try:
+                self._state = await self.get_state()
+                await asyncio.sleep(self.config.update_state_frequency)
+            except asyncio.CancelledError:
+                self.logger.info("Got CancellededError")
+                break
+
     async def get_state(self) -> OakCameraServiceState:
         """Async call to retrieve the state of the connected service."""
         state: OakCameraServiceState
@@ -99,6 +171,8 @@ async def start_service(self) -> None:
         state: OakCameraServiceState = await self.get_state()
         if state.value == oak_pb2.OakServiceState.UNAVAILABLE:
             return
+        reply = await self.stub.cameraControl(oak_pb2.CameraControlRequest())
+        self.settings_reply(reply)
         await self.stub.startService(oak_pb2.StartServiceRequest())
 
     async def pause_service(self) -> None:
@@ -111,6 +185,23 @@ async def pause_service(self) -> None:
             return
         await self.stub.pauseService(oak_pb2.PauseServiceRequest())
 
+    async def send_settings(self) -> oak_pb2.CameraControlReply:
+        request = oak_pb2.CameraControlRequest()
+        request.stereo_settings.CopyFrom(self._mono_camera_settings)
+        request.rgb_settings.CopyFrom(self._rgb_camera_settings)
+        self.needs_update = False
+        return await self.stub.cameraControl(request)
+
+    @RateLimiter(period=1)
+    def update_rgb_settings(self, rgb_settings):
+        self.needs_update = True
+        self._rgb_camera_settings = rgb_settings
+
+    @RateLimiter(period=1)
+    def update_mono_settings(self, mono_settings):
+        self.needs_update = True
+        self._mono_camera_settings = mono_settings
+
     def stream_frames(self, every_n: int):
         """Return the async streaming object.