diff --git a/src/dynalab_core/__init__.py b/src/dynalab_core/__init__.py index c6efbda..ab44df6 100644 --- a/src/dynalab_core/__init__.py +++ b/src/dynalab_core/__init__.py @@ -3,7 +3,10 @@ # SPDX-License-Identifier: GPL-3.0-or-later import logging +from queue import Empty, Queue import threading +from threading import Thread +from time import monotonic, monotonic_ns, sleep from typing import Literal @@ -13,6 +16,8 @@ from dynalab_core.errors import CoreStateMismatchError from dynalab_core.protocols.endpoint import ConnectorRegistry from dynalab_core.protocols.common import VersionDescriptor from dynalab_core.protocols.json.server import JsonServer +from dynalab_core.protocols.packets import ProtocolMessage +from dynalab_core.protocols.packets.data import ValueDescriptor log = logging.getLogger(__name__) @@ -27,7 +32,14 @@ class Core: self._core_version: VersionDescriptor = CORE_VERSION self._stop_event: threading.Event = threading.Event() self._core_config: CoreConfig = config - self._connector_registry = ConnectorRegistry() + + self._data_input_queue: Queue[ProtocolMessage] = Queue(524288) + self._input_worker_thread = Thread( + target=self._input_worker, name="input_worker_thread", daemon=True + ) + self._input_worker_thread.start() + + self._connector_registry = ConnectorRegistry(self._data_input_queue) self._json_server = JsonServer(self._core_config, self._connector_registry) self._state = "initd" @@ -84,3 +96,23 @@ class Core: "Core stopped", extra={"event": "core.stopped", "core_state": self._state}, ) + + def _input_worker(self) -> None: + last_log = 0 + while not self._stop_event.is_set(): + now = monotonic_ns() + messages: list[ProtocolMessage] = [] + try: + while not self._data_input_queue.empty(): + messages.append(self._data_input_queue.get_nowait()) + except Empty: + self._stop_event.wait(0.01) + continue + else: + for message in messages: + if isinstance(message, ValueDescriptor): + pass + + if now > last_log + 2000 * 1_000_000: + log.debug(f"Core queue size: {self._data_input_queue.qsize()}") + last_log = now diff --git a/src/dynalab_core/protocols/endpoint.py b/src/dynalab_core/protocols/endpoint.py index acd15c1..cde313a 100644 --- a/src/dynalab_core/protocols/endpoint.py +++ b/src/dynalab_core/protocols/endpoint.py @@ -15,6 +15,7 @@ from dynalab_core.protocols.errors import ( ConnectorRegistryAlreadyRegisteredError, ) from dynalab_core.protocols.packets import ProtocolMessage +from dynalab_core.protocols.packets.data import ValueDescriptor from dynalab_core.protocols.packets.handshake import ( ConnectorHello, HandshakeAccepted, @@ -29,17 +30,21 @@ class ConnectorEndpoint: def __init__( self, hello: ConnectorHello, - timeout_event: threading.Event | None = None, + timeout_event: threading.Event, + reject_event: threading.Event, + core_input_queue: Queue, ) -> None: # external IO queues self._packet_ingress_queue: Queue[ProtocolMessage] = Queue(524288) self._packet_egress_queue: Queue[ProtocolMessage] = Queue(524288) + self._core_input_queue: Queue[ProtocolMessage] = core_input_queue # internal IO queues self._heartbeat_ingress_queue: Queue[ProtocolMessage] = Queue(524288) self._heartbeat_egress_queue: Queue[ProtocolMessage] = Queue(524288) self._connector_hello = hello - self._timed_out_event = timeout_event or threading.Event() + self._timed_out_event = timeout_event + self._reject_event = reject_event self._stop_event = threading.Event() self._input_worker_stopped_event = threading.Event() self._input_worker_thread = Thread( @@ -212,6 +217,11 @@ class ConnectorEndpoint: else: if isinstance(message, Heartbeat): self._heartbeat_ingress_queue.put_nowait(message) + elif isinstance(message, ValueDescriptor): + if message.signal_id in ( + signal.id for signal in self._connector_hello.signals + ): + self._core_input_queue.put_nowait(message) self._input_worker_stopped_event.set() log.debug( "Connector endpoint input worker %s stopped", @@ -224,61 +234,81 @@ class ConnectorEndpoint: ) def _output_worker(self) -> None: - connector_uuid = str(self.uuid()) - log.debug( - "Connector endpoint output worker %s started", - self._input_worker_thread.name, - extra={ - "event": "endpoint.output_worker_started", - "connector_uuid": connector_uuid, - "thread_name": self._input_worker_thread.name, - }, - ) + try: + connector_uuid = str(self.uuid()) + log.debug( + "Connector endpoint output worker %s started", + self._input_worker_thread.name, + extra={ + "event": "endpoint.output_worker_started", + "connector_uuid": connector_uuid, + "thread_name": self._input_worker_thread.name, + }, + ) + + if ( + not self._connector_hello.protocol_version + == HELLO_PACKET.protocol_version + ): + self._put_egress_packet( + HandshakeRejected(reason="Protocol versions mismatch") + ) + log.debug(f"Rejected handshake for endpoint {self.uuid()}") + self._reject_event.set() + return + + if not self._connector_hello.signals: + self._put_egress_packet( + HandshakeRejected(reason="No signals available") + ) + log.debug(f"Rejected handshake for endpoint {self.uuid()}") + self._reject_event.set() + return - if self._connector_hello.protocol_version == HELLO_PACKET.protocol_version: self._put_egress_packet(HandshakeAccepted(accepted_signals=[])) log.debug(f"Accepted handshake for endpoint {self.uuid()}") - else: - self._put_egress_packet( - HandshakeRejected(reason="Protocol versions mismatch") + log.debug(f"Accepted signals: {self._connector_hello.signals}") + + self._heartbeat_worker_thread.start() + + while not self._stop_event.is_set(): + try: + message = self._heartbeat_egress_queue.get_nowait() + except Empty: + self._stop_event.wait(0.01) + pass + else: + self._put_egress_packet(message) + + finally: + self._output_worker_stopped_event.set() + + log.debug( + "Connector endpoint output worker %s stopped", + self._input_worker_thread.name, + extra={ + "event": "endpoint.output_worker_stopped", + "connector_uuid": connector_uuid, + "thread_name": self._input_worker_thread.name, + }, ) - log.debug(f"Rejected handshake for endpoint {self.uuid()}") - - self._heartbeat_worker_thread.start() - - while not self._stop_event.is_set(): - messages: list[ProtocolMessage] = [] - try: - messages.append(self._heartbeat_egress_queue.get_nowait()) - except Empty: - self._stop_event.wait(0.01) - pass - - for message in messages: - self._put_egress_packet(message) - - self._output_worker_stopped_event.set() - - log.debug( - "Connector endpoint output worker %s stopped", - self._input_worker_thread.name, - extra={ - "event": "endpoint.output_worker_stopped", - "connector_uuid": connector_uuid, - "thread_name": self._input_worker_thread.name, - }, - ) class ConnectorRegistry: - def __init__(self) -> None: + def __init__(self, core_input_queue: Queue) -> None: self._endpoints: dict[UUID, ConnectorEndpoint] = {} self._lock = RLock() + self._core_input_queue = core_input_queue def register( - self, hello: ConnectorHello, timeout_event: threading.Event + self, + hello: ConnectorHello, + timeout_event: threading.Event, + reject_event: threading.Event, ) -> ConnectorEndpoint: - endpoint = ConnectorEndpoint(hello, timeout_event) + endpoint = ConnectorEndpoint( + hello, timeout_event, reject_event, self._core_input_queue + ) with self._lock: current = self._endpoints.get(hello.connector_uuid) diff --git a/src/dynalab_core/protocols/json/server.py b/src/dynalab_core/protocols/json/server.py index 5aa8ae2..2d6c928 100644 --- a/src/dynalab_core/protocols/json/server.py +++ b/src/dynalab_core/protocols/json/server.py @@ -272,8 +272,9 @@ class JsonServer: connector_hello = message timeout_event = threading.Event() + reject_event = threading.Event() connector_endpoint = self._connector_registry.register( - connector_hello, timeout_event + connector_hello, timeout_event, reject_event ) reason = "handler_completed" @@ -302,11 +303,13 @@ class JsonServer: timeout_task = asyncio.create_task( self._wait_for_thread_event(timeout_event) ) + reject_task = asyncio.create_task(self._wait_for_thread_event(reject_event)) connection_tasks = [ input_task, output_task, server_stop_task, timeout_task, + reject_task, ] done, _ = await asyncio.wait( @@ -314,6 +317,8 @@ class JsonServer: ) if timeout_task in done: reason = "endpoint_timeout" + elif reject_event in done: + reason = "connection_rejected" elif server_stop_task in done: reason = "server_shutdown" diff --git a/src/dynalab_core/protocols/packets/__init__.py b/src/dynalab_core/protocols/packets/__init__.py index 055158c..5447b0b 100644 --- a/src/dynalab_core/protocols/packets/__init__.py +++ b/src/dynalab_core/protocols/packets/__init__.py @@ -1,7 +1,12 @@ +# Copyright (C) 2026 Hector van der Aa +# Copyright (C) 2026 Association Exergie +# SPDX-License-Identifier: GPL-3.0-or-later + from typing import Annotated from pydantic import Field +from dynalab_core.protocols.packets.data import ValueDescriptor from dynalab_core.protocols.packets.handshake import ( ConnectorHello, DynaLabHello, @@ -12,6 +17,11 @@ from dynalab_core.protocols.packets.heartbeat import Heartbeat ProtocolMessage = Annotated[ - DynaLabHello | ConnectorHello | HandshakeAccepted | HandshakeRejected | Heartbeat, + DynaLabHello + | ConnectorHello + | HandshakeAccepted + | HandshakeRejected + | Heartbeat + | ValueDescriptor, Field(discriminator="type"), ] diff --git a/src/dynalab_core/protocols/packets/data.py b/src/dynalab_core/protocols/packets/data.py new file mode 100644 index 0000000..8361fdf --- /dev/null +++ b/src/dynalab_core/protocols/packets/data.py @@ -0,0 +1,13 @@ +# Copyright (C) 2026 Hector van der Aa +# Copyright (C) 2026 Association Exergie +# SPDX-License-Identifier: GPL-3.0-or-later + +from typing import Literal +from uuid import UUID +from pydantic import BaseModel + + +class ValueDescriptor(BaseModel): + type: Literal["value_descriptor"] = "value_descriptor" + signal_id: UUID + value: float diff --git a/src/dynalab_core/protocols/packets/handshake.py b/src/dynalab_core/protocols/packets/handshake.py index 21b8a8c..d9e5578 100644 --- a/src/dynalab_core/protocols/packets/handshake.py +++ b/src/dynalab_core/protocols/packets/handshake.py @@ -10,6 +10,15 @@ from dynalab_core.protocols.common import VersionDescriptor from dynalab_core.protocols.constants import PROTOCOL_VERSION +class SignalDescriptor(BaseModel): + id: UUID + name: str + type: Literal["number", "binary"] + min_value: float | None = None + max_value: float | None = None + unit: str | None = None + + class DynaLabHello(BaseModel): type: Literal["dynalab_hello"] = "dynalab_hello" instance_id: UUID @@ -28,8 +37,7 @@ class ConnectorHello(BaseModel): connector_name: str connector_version: str - # TODO: implement SignalDescriptor - # signals: list[SignalDescriptor] + signals: list[SignalDescriptor] class HandshakeAccepted(BaseModel): diff --git a/test/manual/peer.py b/test/manual/peer.py index f49c901..198582d 100644 --- a/test/manual/peer.py +++ b/test/manual/peer.py @@ -1,73 +1,344 @@ import asyncio import logging -from time import monotonic -from uuid import uuid4 +import threading +from concurrent.futures import CancelledError as FutureCancelledError +from concurrent.futures import TimeoutError as FutureTimeoutError +from statistics import mean +from time import monotonic, perf_counter +from uuid import UUID, uuid4 -from dynalab_core.protocols.packets import ProtocolMessage from rich.logging import RichHandler from dynalab_core.protocols.common import VersionDescriptor from dynalab_core.protocols.constants import PROTOCOL_VERSION from dynalab_core.protocols.json.wire import read_message, write_message -from dynalab_core.protocols.packets.handshake import ConnectorHello, DynaLabHello +from dynalab_core.protocols.packets.data import ValueDescriptor +from dynalab_core.protocols.packets.handshake import ( + ConnectorHello, + DynaLabHello, + SignalDescriptor, +) from dynalab_core.protocols.packets.heartbeat import Heartbeat -CONNECTOR_VERSION = VersionDescriptor(type="alpha", major=0, minor=0, patch=1) + +HOST = "127.0.0.1" +PORT = 8765 + +# Set to None to send as quickly as possible. +# For a controlled rate, use something like 5_000.0. +TARGET_FREQUENCY_HZ: float | None = None + +FREQUENCY_SAMPLE_SIZE = 500 + +CONNECTOR_VERSION = VersionDescriptor( + type="alpha", + major=0, + minor=0, + patch=1, +) logging.basicConfig( level=logging.DEBUG, format="%(name)s %(message)s", datefmt="%H:%M:%S", - handlers=[RichHandler(rich_tracebacks=True, show_path=False)], + handlers=[ + RichHandler( + rich_tracebacks=True, + show_path=False, + ) + ], force=True, ) + log = logging.getLogger(__name__) +signal_id: UUID = uuid4() + + +def monotonic_ms() -> int: + return round(monotonic() * 1_000) + + +async def send_message( + writer: asyncio.StreamWriter, + write_lock: asyncio.Lock, + message: object, +) -> None: + """ + Serialize access to the asyncio StreamWriter. + + Both heartbeat replies and value messages use this function. + """ + async with write_lock: + await write_message(writer, message) + + +def value_sender_thread( + loop: asyncio.AbstractEventLoop, + writer: asyncio.StreamWriter, + write_lock: asyncio.Lock, + stop_event: threading.Event, +) -> None: + """ + Run the high-frequency timing loop in a dedicated OS thread. + + The asyncio StreamWriter is not thread-safe, so each write is submitted + back to the asyncio event loop with run_coroutine_threadsafe(). + """ + intervals: list[float] = [] + previous_send_time: float | None = None + + if TARGET_FREQUENCY_HZ is not None: + period = 1.0 / TARGET_FREQUENCY_HZ + next_send_time = perf_counter() + else: + period = None + next_send_time = 0.0 + + log.debug("Value sender thread started") + + try: + while not stop_event.is_set(): + if period is not None: + while True: + remaining = next_send_time - perf_counter() + + if remaining <= 0: + break + + # Sleep for larger remaining times, then spin for the + # final fraction of a millisecond. + if remaining > 0.001: + stop_event.wait(remaining - 0.0005) + + if stop_event.is_set(): + return + + next_send_time += period + + message = ValueDescriptor( + signal_id=signal_id, + value=2.0, + ) + + future = asyncio.run_coroutine_threadsafe( + send_message( + writer, + write_lock, + message, + ), + loop, + ) + + try: + # Waiting prevents an ever-growing queue of scheduled writes. + future.result(timeout=2.0) + except FutureTimeoutError: + log.warning("Timed out sending value message") + future.cancel() + return + except FutureCancelledError: + return + except Exception: + log.exception("Value sender failed") + return + + current_send_time = perf_counter() + + if previous_send_time is not None: + intervals.append(current_send_time - previous_send_time) + + previous_send_time = current_send_time + + if len(intervals) >= FREQUENCY_SAMPLE_SIZE: + average_interval = mean(intervals) + frequency_hz = 1.0 / average_interval + + log.debug( + "Value frequency: %.3f kHz", + frequency_hz / 1_000.0, + ) + + intervals.clear() + + if period is not None: + # Do not try to catch up by rapidly sending many old periods. + current_time = perf_counter() + + if next_send_time < current_time - period: + missed_periods = int((current_time - next_send_time) / period) + next_send_time += missed_periods * period + + finally: + log.debug("Value sender thread stopped") + + +async def perform_handshake( + reader: asyncio.StreamReader, + writer: asyncio.StreamWriter, + write_lock: asyncio.Lock, +) -> None: + try: + message = await asyncio.wait_for( + read_message(reader), + timeout=30.0, + ) + except TimeoutError as exc: + raise TimeoutError("Timed out waiting for server hello") from exc + + if not isinstance(message, DynaLabHello): + raise RuntimeError(f"Expected DynaLabHello, received {type(message).__name__}") + + log.debug( + "Received hello from core instance %s", + message.instance_id, + ) + + connector_hello = ConnectorHello( + connector_uuid=uuid4(), + protocol_version=PROTOCOL_VERSION, + connector_name="Test connector", + connector_version=CONNECTOR_VERSION.get_version(), + signals=[ + SignalDescriptor( + id=signal_id, + name="Dummy signal", + type="number", + ) + ], + ) + + try: + await asyncio.wait_for( + send_message( + writer, + write_lock, + connector_hello, + ), + timeout=5.0, + ) + except TimeoutError as exc: + raise TimeoutError("Timed out sending connector hello") from exc + + log.info( + "Connector hello sent for %s", + connector_hello.connector_uuid, + ) + + +async def receive_messages( + reader: asyncio.StreamReader, + writer: asyncio.StreamWriter, + write_lock: asyncio.Lock, +) -> None: + while True: + message = await read_message(reader) + + if isinstance(message, Heartbeat): + message.return_timestamp = monotonic_ms() + + await send_message( + writer, + write_lock, + message, + ) + + log.debug( + "Returned heartbeat %s", + message.sequence, + ) + else: + log.debug( + "Received unexpected message: %s", + type(message).__name__, + ) + async def main() -> None: - reader, writer = await asyncio.open_connection( - host="127.0.0.1", - port=8765, - ) - - log.info("Connected to DynaLab core") + writer: asyncio.StreamWriter | None = None + sender_thread: threading.Thread | None = None + sender_stop_event = threading.Event() try: - try: - message = await asyncio.wait_for(read_message(reader), timeout=30.0) - except TimeoutError: - log.warning("Timed out waiting for server hello") - return - if not isinstance(message, DynaLabHello): - log.warning("Expected server hello, received %s", message.type) - return - dynalab_hello = message - log.debug("Received hello from core instance %s", dynalab_hello.instance_id) - connector_hello = ConnectorHello( - connector_uuid=uuid4(), - protocol_version=PROTOCOL_VERSION, - connector_name="Test connector", - connector_version=CONNECTOR_VERSION.get_version(), + reader, writer = await asyncio.open_connection( + host=HOST, + port=PORT, ) - try: - await asyncio.wait_for(write_message(writer, connector_hello), timeout=5.0) - except TimeoutError: - log.warning("Timed out sending connector hello") - return - log.info("Connector hello sent for %s", connector_hello.connector_uuid) - try: - while True: - message = await read_message(reader) - if isinstance(message, Heartbeat): - message.return_timestamp = round(monotonic() * 1000) - await write_message(writer, message) - except KeyboardInterrupt: - return + log.info( + "Connected to DynaLab core at %s:%d", + HOST, + PORT, + ) + + write_lock = asyncio.Lock() + + await perform_handshake( + reader, + writer, + write_lock, + ) + + loop = asyncio.get_running_loop() + + sender_thread = threading.Thread( + target=value_sender_thread, + name="value_sender", + args=( + loop, + writer, + write_lock, + sender_stop_event, + ), + daemon=True, + ) + sender_thread.start() + + await receive_messages( + reader, + writer, + write_lock, + ) + + except TimeoutError as exc: + log.warning("%s", exc) + + except asyncio.IncompleteReadError: + log.warning("DynaLab core closed the connection") + + except ConnectionError as exc: + log.warning("Connection error: %s", exc) + + except RuntimeError as exc: + log.warning("Protocol error: %s", exc) + finally: - writer.close() - await writer.wait_closed() + sender_stop_event.set() + + if sender_thread is not None: + # Do not call thread.join() directly because that would block the + # asyncio event loop while the thread may be waiting on it. + await asyncio.to_thread( + sender_thread.join, + 3.0, + ) + + if sender_thread.is_alive(): + log.warning("Value sender thread did not stop cleanly") + + if writer is not None: + writer.close() + + try: + await writer.wait_closed() + except ConnectionError: + pass + log.info("Disconnected from DynaLab core") -asyncio.run(main()) +if __name__ == "__main__": + try: + asyncio.run(main()) + except KeyboardInterrupt: + log.info("Stopped by user")