From 983bfc6c3c70d261c99a35811dea32793ef4e19d Mon Sep 17 00:00:00 2001 From: Ethan Rublee <ethan.rublee@gmail.com> Date: Wed, 12 Oct 2022 22:41:18 -0700 Subject: [PATCH 1/2] Add support calibration and camera settings. --- protos/farm_ng/oak/oak.proto | 83 ++++++++++++++++++++++++++++-- py/farm_ng/oak/client.py | 99 +++++++++++++++++++++++++++++++++++- 2 files changed, 176 insertions(+), 6 deletions(-) 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..2a177501 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,10 +55,13 @@ 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 address: str = "localhost" # the address name of the server + update_state_frequency: int = 2 # the frequency in floating second to ping the service and update the state class OakCameraServiceState: @@ -73,11 +113,49 @@ 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 + + # NOTE: in order to cancel this task, the consumer of this class is + # responsible to gather the task and cancel. + self._sync_task = asyncio.create_task(self._poll_service_state()) + + @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 + await asyncio.sleep(0.02) + async def get_state(self) -> OakCameraServiceState: """Async call to retrieve the state of the connected service.""" state: OakCameraServiceState @@ -99,6 +177,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 +191,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. From 12b9a400efd33acab6d78bbbb94bbc768b611e12 Mon Sep 17 00:00:00 2001 From: Ethan Rublee <ethan.rublee@gmail.com> Date: Wed, 12 Oct 2022 22:47:44 -0700 Subject: [PATCH 2/2] bad polling logic. --- py/farm_ng/oak/client.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/py/farm_ng/oak/client.py b/py/farm_ng/oak/client.py index 2a177501..52d17b7d 100644 --- a/py/farm_ng/oak/client.py +++ b/py/farm_ng/oak/client.py @@ -61,7 +61,6 @@ class OakCameraClientConfig: port: int # the port of the server address address: str = "localhost" # the address name of the server - update_state_frequency: int = 2 # the frequency in floating second to ping the service and update the state class OakCameraServiceState: @@ -120,10 +119,6 @@ def __init__(self, config: OakCameraClientConfig) -> None: self.needs_update = False - # NOTE: in order to cancel this task, the consumer of this class is - # responsible to gather the task and cancel. - self._sync_task = asyncio.create_task(self._poll_service_state()) - @property def state(self) -> OakCameraServiceState: return self._state @@ -154,7 +149,6 @@ async def _poll_service_state(self) -> None: except asyncio.CancelledError: self.logger.info("Got CancellededError") break - await asyncio.sleep(0.02) async def get_state(self) -> OakCameraServiceState: """Async call to retrieve the state of the connected service."""