diff --git a/protos/farm_ng/oak/oak.proto b/protos/farm_ng/oak/oak.proto index 1f231253..f86c46d4 100644 --- a/protos/farm_ng/oak/oak.proto +++ b/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; +} diff --git a/py/farm_ng/oak/client.py b/py/farm_ng/oak/client.py index bb71db15..52d17b7d 100644 --- a/py/farm_ng/oak/client.py +++ b/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.