antioch-py 2.0.6__py3-none-any.whl

antioch/message.py

@@ -0,0 +1,87 @@
+from common.message import (
+    Array,
+    Bool,
+    CameraInfo,
+    CircleAnnotation,
+    Color,
+    DeserializationError,
+    Float,
+    FrameTransform,
+    FrameTransforms,
+    Image,
+    ImageAnnotations,
+    ImageEncoding,
+    Int,
+    JointState,
+    JointStates,
+    JointTarget,
+    JointTargets,
+    Log,
+    LogLevel,
+    Message,
+    MessageError,
+    MismatchError,
+    Point2,
+    Point3,
+    PointCloud,
+    PointsAnnotation,
+    PointsAnnotationType,
+    Pose,
+    Quaternion,
+    RadarDetection,
+    RadarScan,
+    SerializationError,
+    String,
+    TextAnnotation,
+    Vector2,
+    Vector3,
+)
+
+__all__ = [
+    # Base types
+    "Message",
+    "MessageError",
+    "DeserializationError",
+    "SerializationError",
+    "MismatchError",
+    # Primitive types
+    "Array",
+    "Bool",
+    "Float",
+    "Int",
+    "String",
+    # Geometry types
+    "Point2",
+    "Point3",
+    "Vector2",
+    "Vector3",
+    "Pose",
+    "Quaternion",
+    # Color
+    "Color",
+    # Camera types
+    "CameraInfo",
+    "Image",
+    "ImageEncoding",
+    # Joint types
+    "JointState",
+    "JointStates",
+    "JointTarget",
+    "JointTargets",
+    # Sensor types
+    "RadarDetection",
+    "RadarScan",
+    "PointCloud",
+    # Logging
+    "Log",
+    "LogLevel",
+    # Annotations
+    "CircleAnnotation",
+    "ImageAnnotations",
+    "PointsAnnotation",
+    "PointsAnnotationType",
+    "TextAnnotation",
+    # Frame transforms
+    "FrameTransform",
+    "FrameTransforms",
+]

antioch/module/__init__.py

@@ -0,0 +1,53 @@
+from antioch.module.clock import Clock, now_us
+from antioch.module.execution import Execution, Input, Output
+from antioch.module.module import Module
+from antioch.module.token import Token, TokenType
+from common.ark import Environment, HardwareAccessMode
+from common.message import (
+    CameraInfo,
+    Image,
+    ImageEncoding,
+    ImuSample,
+    JointState,
+    JointStates,
+    JointTarget,
+    JointTargets,
+    Message,
+    Pose,
+    Quaternion,
+    RadarDetection,
+    RadarScan,
+    Vector3,
+)
+
+__all__ = [
+    # Core module types
+    "Module",
+    "Execution",
+    "Input",
+    "Output",
+    # Token types
+    "Token",
+    "TokenType",
+    # Timing
+    "Clock",
+    "now_us",
+    # Enums
+    "Environment",
+    "HardwareAccessMode",
+    # Message types
+    "Message",
+    "CameraInfo",
+    "Image",
+    "ImageEncoding",
+    "ImuSample",
+    "JointState",
+    "JointStates",
+    "JointTarget",
+    "JointTargets",
+    "Pose",
+    "Quaternion",
+    "RadarDetection",
+    "RadarScan",
+    "Vector3",
+]

antioch/module/clock.py

@@ -0,0 +1,62 @@
+import time
+from threading import Event
+
+from common.utils.time import now_us
+
+
+class Clock:
+    """
+    Real-time clock for LET-to-wall-time mapping (real mode only).
+
+    Maps logical execution time to wall-clock time using a fixed start time.
+    Provides precise timing for real-mode execution to avoid clock drift.
+    """
+
+    def __init__(self, start_timestamp_us: int, event: Event | None = None):
+        """
+        Create a new real-time clock.
+
+        :param start_timestamp_us: Wall-clock time (microseconds since epoch) corresponding to LET=0.
+        :param event: Optional event to check for shutdown requests during waits.
+        """
+
+        self._start_timestamp_us = start_timestamp_us
+        self._event = event
+
+    @property
+    def let_us(self) -> int:
+        """
+        Get the current logical time in microseconds.
+
+        :return: Current logical time.
+        """
+
+        return now_us() - self._start_timestamp_us
+
+    def wait_until(self, let_us: int) -> bool:
+        """
+        Sleep until reaching target LET.
+
+        Checks shutdown event every 100ms. Uses hybrid sleep approach:
+        - For waits > 10ms: Sleep in chunks (max 100ms) for shutdown responsiveness
+        - For waits < 10ms: Sleep in 100μs increments for precision
+
+        :param let_us: Target logical execution time in microseconds.
+        :return: True if wait completed normally, False if interrupted by event.
+        """
+
+        target_timestamp_us = self._start_timestamp_us + let_us
+        while True:
+            # Check if we should exit
+            current = now_us()
+            if current >= target_timestamp_us:
+                return True
+
+            # Check event if provided
+            if self._event and self._event.is_set():
+                return False
+
+            # Sleep for chunk
+            remaining_us = target_timestamp_us - current
+            sleep_us = min(remaining_us, 100_000) if remaining_us > 10_000 else min(remaining_us, 100)
+            time.sleep(sleep_us / 1_000_000)

antioch/module/execution.py

@@ -0,0 +1,278 @@
+import time
+from typing import TypeVar, overload
+
+from antioch.module.token import Token, TokenType
+from common.ark.module import ModuleParameter
+from common.ark.node import NodeOutput
+from common.message import Image, ImuSample, JointStates, JointTargets, Message, PirStatus, RadarScan
+from common.utils.logger import Logger
+from common.utils.time import now_us
+
+T = TypeVar("T", bound=Message)
+
+
+class Execution:
+    """
+    Node execution context.
+
+    Provides access to inputs, outputs, hardware, logger, and module parameters
+    for user callbacks.
+    """
+
+    name: str
+    let_us: int
+    budget_us: int
+    parameters: dict[str, ModuleParameter]
+    logger: Logger
+
+    def __init__(
+        self,
+        name: str,
+        budget_us: int,
+        parameters: dict[str, ModuleParameter],
+        outputs: dict[str, NodeOutput],
+        logger: Logger,
+    ):
+        """
+        Initialize execution context with static configuration.
+
+        :param name: Node name.
+        :param budget_us: Node execution budget in microseconds.
+        :param parameters: Module parameters.
+        :param outputs: Node output configurations.
+        :param logger: Logger for this execution context.
+        """
+
+        self.name = name
+        self.budget_us = budget_us
+        self.parameters = parameters
+        self.logger = logger
+
+        self._output_configs = outputs
+        self._outputs = {name: Output(name, output_config, self) for name, output_config in outputs.items()}
+
+        # Execution runtime state
+        self.let_us = 0
+        self._start_timestamp_us = 0
+        self._start_time_us = 0
+        self._inputs: dict[str, Input] = {}
+        self._output_data: dict[str, bytes | None] = {}
+        self._hardware_reads: dict[str, bytes] = {}
+        self._hardware_writes: dict[str, bytes] = {}
+
+    def start(
+        self,
+        let_us: int,
+        input_data: dict[str, list[Token]] = {},
+        hardware_reads: dict[str, bytes] = {},
+    ) -> None:
+        """
+        Start a new execution with runtime data.
+
+        :param let_us: Logical execution time.
+        :param input_data: Input tokens keyed by input name.
+        :param hardware_reads: Hardware read data keyed by hardware name.
+        """
+
+        self.let_us = let_us
+        self._start_timestamp_us = now_us()
+        self._start_time_us = time.monotonic_ns() // 1000
+        self._inputs = {name: Input(tokens) for name, tokens in input_data.items()}
+        self._output_data.clear()
+        self._hardware_reads = hardware_reads
+        self._hardware_writes.clear()
+
+    @property
+    def elapsed_us(self) -> int:
+        """
+        Get elapsed time in microseconds since execution start.
+
+        :return: Elapsed time in microseconds.
+        """
+
+        return time.monotonic_ns() // 1000 - self._start_time_us
+
+    @property
+    def remaining_us(self) -> int:
+        """
+        Get remaining time in microseconds until execution budget is exhausted.
+
+        :return: Remaining time in microseconds.
+        """
+
+        return max(0, self.budget_us - self.elapsed_us)
+
+    def input(self, name: str) -> "Input":
+        """
+        Get input by name.
+
+        :param name: Input name.
+        :return: Input wrapper.
+        :raises KeyError: If input not found.
+        """
+
+        if name not in self._inputs:
+            raise KeyError(f"Input '{name}' not found")
+        return self._inputs[name]
+
+    def output(self, name: str) -> "Output":
+        """
+        Get output by name.
+
+        :param name: Output name.
+        :return: Output wrapper.
+        :raises KeyError: If output not found.
+        """
+
+        if name not in self._outputs:
+            raise KeyError(f"Output '{name}' not found")
+        return self._outputs[name]
+
+    def read_camera(self, name: str) -> Image | None:
+        """
+        Read camera image.
+
+        :param name: Hardware name.
+        :return: Camera image, or None if no data.
+        """
+
+        data = self._hardware_reads.get(name)
+        return Image.unpack(data) if data else None
+
+    def read_imu(self, name: str) -> ImuSample | None:
+        """
+        Read IMU sample.
+
+        :param name: Hardware name.
+        :return: IMU sensor measurements, or None if no data.
+        """
+
+        data = self._hardware_reads.get(name)
+        return ImuSample.unpack(data) if data else None
+
+    def read_radar(self, name: str) -> RadarScan | None:
+        """
+        Read radar scan.
+
+        :param name: Hardware name.
+        :return: Radar scan with detections, or None if no data.
+        """
+
+        data = self._hardware_reads.get(name)
+        return RadarScan.unpack(data) if data else None
+
+    def read_pir(self, name: str) -> PirStatus | None:
+        """
+        Read PIR sensor status.
+
+        :param name: Hardware name.
+        :return: PIR status with detection state and signal info, or None if no data.
+        """
+
+        data = self._hardware_reads.get(name)
+        return PirStatus.unpack(data) if data else None
+
+    def read_actuator_group(self, name: str) -> JointStates | None:
+        """
+        Read joint states from actuator group.
+
+        :param name: Hardware name.
+        :return: Joint states, or None if no data.
+        """
+
+        data = self._hardware_reads.get(name)
+        return JointStates.unpack(data) if data else None
+
+    def write_actuator_group(self, name: str, targets: JointTargets) -> None:
+        """
+        Write joint targets to actuator group.
+
+        Queues joint targets for node completion.
+
+        :param name: Hardware name.
+        :param targets: Joint targets to apply.
+        """
+
+        self._hardware_writes[name] = targets.pack()
+
+
+class Input:
+    """
+    Input wrapper for accessing tokens.
+    """
+
+    def __init__(self, tokens: list[Token]):
+        """
+        Initialize input.
+
+        :param tokens: List of tokens for this input.
+        """
+
+        self._tokens = tokens
+
+    @overload
+    def data(self, message_cls: type[T], n: int = 1) -> T | None: ...
+
+    @overload
+    def data(self, message_cls: type[T], n: int) -> list[T]: ...
+
+    def data(self, message_cls: type[T], n: int = 1) -> list[T] | T | None:
+        """
+        Get deserialized data from tokens.
+
+        :param message_cls: Message type to deserialize.
+        :param n: Number of tokens to return (1 for single, >1 for list).
+        :return: Single message, list of messages, or None if no data.
+        """
+
+        data = []
+        for token in reversed(self._tokens):
+            if token.status == TokenType.DATA and token.payload:
+                data.append(message_cls.unpack(token.payload))
+                if len(data) >= n:
+                    break
+        return data if n > 1 else (data[0] if data else None)
+
+    def tokens(self) -> list[Token]:
+        """
+        Get all tokens for this input.
+
+        :return: List of tokens.
+        """
+
+        return self._tokens
+
+
+class Output:
+    """
+    Output wrapper for setting data.
+    """
+
+    def __init__(self, name: str, output_config: NodeOutput, execution: Execution):
+        """
+        Initialize output.
+
+        :param name: Output name.
+        :param output_config: Output configuration from node.
+        :param execution: Parent execution context.
+        """
+
+        self._name = name
+        self._output_config = output_config
+        self._execution = execution
+
+    def set(self, message: Message) -> "Output":
+        """
+        Set output data.
+
+        :param message: Message to serialize and set.
+        :return: Self for chaining.
+        :raises ValueError: If message type doesn't match expected output type.
+        """
+
+        # Validate message type matches expected output type
+        if message.get_type() != self._output_config.type:
+            raise ValueError(f"Output '{self._name}' expects type '{self._output_config.type}', got '{message.get_type()}'")
+
+        self._execution._output_data[self._name] = message.pack()
+        return self

antioch/module/input.py

@@ -0,0 +1,127 @@
+from threading import Lock
+
+import zenoh
+from sortedcontainers import SortedDict
+
+from antioch.module.token import Token, TokenType
+from common.ark.node import NodeInput
+from common.utils.comms import CommsSession
+
+TOKEN_INPUT_PATH = "_token/{path}"
+
+
+class NodeInputBuffer:
+    """
+    Thread-safe input buffer with policy-based token retention.
+
+    Manages tokens for a single node input with automatic arrival validation
+    and buffer policy enforcement (last_n, max_age_ms, always_run, required, consume_inputs).
+    """
+
+    def __init__(self, input_name: str, config: NodeInput, comms: CommsSession):
+        """
+        Create a new node input buffer.
+
+        :param input_name: Name of the input.
+        :param config: Input configuration with buffer policies.
+        :param comms: Communication session for subscribing to tokens.
+        """
+
+        self._input_name = input_name
+        self._config = config
+        self._lock = Lock()
+        self._tokens: SortedDict[tuple[int, str, str], Token] = SortedDict()
+
+        # Subscribe to token path with callback
+        path = TOKEN_INPUT_PATH.format(path=config.path)
+        self._subscriber = comms.declare_callback_subscriber(path, self._on_token)
+
+    def close(self) -> None:
+        """
+        Close the input buffer and clean up resources.
+        """
+
+        self._subscriber.undeclare()
+
+    def add_token(self, token: Token) -> None:
+        """
+        Add token to buffer.
+
+        :param token: Token to add.
+        """
+
+        with self._lock:
+            key = (token.let_us, token.module_name, token.node_name)
+            self._tokens[key] = token
+
+    def get_token(self, let_us: int, source_module: str, source_node: str) -> Token | None:
+        """
+        Get specific token or None if not arrived.
+
+        :param let_us: Logical execution time of the token.
+        :param source_module: Source module name.
+        :param source_node: Source node name.
+        :return: Token if found, None otherwise.
+        """
+
+        with self._lock:
+            key = (let_us, source_module, source_node)
+            return self._tokens.get(key)
+
+    def prepare_execution(self, let_us: int) -> list[Token] | None:
+        """
+        Prepare tokens for execution by collecting and applying buffer policies.
+
+        :param let_us: Current logical execution time.
+        :return: Filtered tokens ready for execution, or None if required input missing.
+        """
+
+        with self._lock:
+            # Collect tokens with let_us <= current execution time
+            tokens = [t for t in self._tokens.values() if t.let_us <= let_us]
+
+            # Filter to data tokens only (unless always_run allows errors/overruns)
+            if not self._config.always_run:
+                tokens = [t for t in tokens if t.status == TokenType.DATA]
+
+            # Check required input constraint
+            if self._config.required and len(tokens) == 0:
+                return None
+
+            # Keep only last N tokens
+            if len(tokens) > self._config.last_n:
+                tokens = tokens[-self._config.last_n :]
+
+            # Filter out stale tokens based on max age
+            if self._config.max_age_ms is not None:
+                max_age_us = self._config.max_age_ms * 1000
+                tokens = [t for t in tokens if let_us - t.let_us <= max_age_us]
+
+            # Add filtered tokens back to buffer if not consuming
+            if not self._config.consume_inputs:
+                for t in tokens:
+                    key = (t.let_us, t.module_name, t.node_name)
+                    self._tokens[key] = t
+
+            return tokens
+
+    def _on_token(self, sample: zenoh.Sample) -> None:
+        """
+        Callback thread: receive and buffer token.
+
+        :param sample: Zenoh sample containing token.
+        """
+
+        token = Token.unpack(sample.payload.to_bytes())
+        if token.elapsed() > token.budget_us:
+            token = Token(
+                module_name=token.module_name,
+                node_name=token.node_name,
+                output_name=token.output_name,
+                let_us=token.let_us,
+                start_timestamp_us=token.start_timestamp_us,
+                budget_us=token.budget_us,
+                status=TokenType.OVERRUN,
+            )
+
+        self.add_token(token)