Added routing to core queue from endpoint

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

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())
+        try:
-        log.debug(
+            connector_uuid = str(self.uuid())
-            "Connector endpoint output worker %s started",
+            log.debug(
-            self._input_worker_thread.name,
+                "Connector endpoint output worker %s started",
-            extra={
+                self._input_worker_thread.name,
-                "event": "endpoint.output_worker_started",
+                extra={
-                "connector_uuid": connector_uuid,
+                    "event": "endpoint.output_worker_started",
-                "thread_name": self._input_worker_thread.name,
+                    "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:
+            log.debug(f"Accepted signals: {self._connector_hello.signals}")
-            self._put_egress_packet(
+
-                HandshakeRejected(reason="Protocol versions mismatch")
+            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)

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"
 

src/dynalab_core/protocols/packets/__init__.py

@@ -1,7 +1,12 @@
+# Copyright (C) 2026 Hector van der Aa <hector@h3cx.dev>
+# Copyright (C) 2026 Association Exergie <association.exergie@gmail.com>
+# 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"),
 ]

src/dynalab_core/protocols/packets/data.py

@@ -0,0 +1,13 @@
+# Copyright (C) 2026 Hector van der Aa <hector@h3cx.dev>
+# Copyright (C) 2026 Association Exergie <association.exergie@gmail.com>
+# 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

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):

test/manual/peer.py

@@ -1,73 +1,344 @@
 import asyncio
 import logging
-from time import monotonic
+import threading
-from uuid import uuid4
+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(
+    writer: asyncio.StreamWriter | None = None
-        host="127.0.0.1",
+    sender_thread: threading.Thread | None = None
-        port=8765,
+    sender_stop_event = threading.Event()
-    )
-
-    log.info("Connected to DynaLab core")
 
     try:
-        try:
+        reader, writer = await asyncio.open_connection(
-            message = await asyncio.wait_for(read_message(reader), timeout=30.0)
+            host=HOST,
-        except TimeoutError:
+            port=PORT,
-            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(),
         )
-        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:
+        log.info(
-            while True:
+            "Connected to DynaLab core at %s:%d",
-                message = await read_message(reader)
+            HOST,
-                if isinstance(message, Heartbeat):
+            PORT,
-                    message.return_timestamp = round(monotonic() * 1000)
+        )
-                    await write_message(writer, message)
+
-        except KeyboardInterrupt:
+        write_lock = asyncio.Lock()
-            return
+
+        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()
+        sender_stop_event.set()
-        await writer.wait_closed()
+
+        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")