antioch-py 2.0.6__py3-none-any.whl

common/message/pose.py

@@ -0,0 +1,148 @@
+from __future__ import annotations
+
+from typing import Any
+
+import numpy as np
+from pydantic import Field, field_validator, model_validator
+
+from common.message.base import Message
+from common.message.vector import Vector3
+
+
+class Pose(Message):
+    """
+    A pose representing position and orientation in 3D space.
+
+    Serializes position as [x, y, z] and orientation as [roll, pitch, yaw] in radians.
+    Orientation is stored as RPY (Roll-Pitch-Yaw) Euler angles.
+
+    Automatically converts from common Python types:
+    - Lists/tuples: Pose(position=[1.0, 2.0, 3.0], orientation=[0.0, 0.0, 1.57])
+    - Dicts: Pose(position={"x": 1.0, "y": 2.0, "z": 3.0}, orientation={"x": 0.0, "y": 0.0, "z": 1.57})
+    """
+
+    _type = "antioch/pose"
+    position: Vector3 = Field(default_factory=lambda: Vector3.zeros())
+    orientation: Vector3 = Field(default_factory=lambda: Vector3.zeros())
+
+    def __add__(self, other: Vector3) -> Pose:
+        """
+        Translate the pose by a vector (adds to position).
+
+        :param other: A Vector3 representing the translation.
+        :return: The translated pose.
+        """
+
+        return Pose(position=self.position + other, orientation=self.orientation)
+
+    def __sub__(self, other: Vector3) -> Pose:
+        """
+        Translate the pose by a negative vector (subtracts from position).
+
+        :param other: A Vector3 representing the translation.
+        :return: The translated pose.
+        """
+
+        return Pose(position=self.position - other, orientation=self.orientation)
+
+    @model_validator(mode="before")
+    @classmethod
+    def convert_nested_types(cls, data: Any) -> Any:
+        """
+        Allow passing position and orientation as lists/tuples that auto-convert to Vector3/Quaternion.
+        Orientation can be 3 values (RPY) or 4 values (quaternion) - always stored as quaternion.
+        """
+
+        # Already a Pose instance
+        if isinstance(data, cls):
+            return data.model_dump()
+
+        # Dict format - let Pydantic and nested validators handle it
+        if isinstance(data, dict):
+            return data
+
+        return data
+
+    @field_validator("position", mode="before")
+    @classmethod
+    def validate_position(cls, v: Any) -> Any:
+        """
+        Convert position from list/tuple/array to Vector3 format before type validation.
+        """
+
+        if isinstance(v, (list, tuple, np.ndarray)):
+            return Vector3.convert_iterables.__func__(Vector3, v)
+        return v
+
+    @field_validator("orientation", mode="before")
+    @classmethod
+    def validate_orientation(cls, v: Any) -> Any:
+        """
+        Convert orientation from list/tuple/array to Vector3 format before type validation.
+        """
+
+        if isinstance(v, (list, tuple, np.ndarray)):
+            return Vector3.convert_iterables.__func__(Vector3, v)
+        return v
+
+    @classmethod
+    def from_any(cls, data: Any) -> Pose:
+        """
+        Create Pose from a dict or Pose instance.
+
+        :param data: Dict with position/orientation fields, or a Pose instance.
+        :return: A Pose instance.
+        :raises ValueError: If conversion fails.
+        """
+
+        # Already a Pose - return as-is
+        if isinstance(data, cls):
+            return data
+
+        try:
+            if isinstance(data, dict):
+                return cls(**data)
+            raise ValueError("Pose requires a dict with position and orientation fields")
+        except Exception as e:
+            raise ValueError(f"Cannot convert to Pose: {e}") from None
+
+    @classmethod
+    def identity(cls) -> Pose:
+        """
+        Create an identity pose at origin with zero rotation.
+
+        :return: Identity pose.
+        """
+
+        return cls(position=Vector3.zeros(), orientation=Vector3.zeros())
+
+    @classmethod
+    def from_position(cls, position: Vector3) -> Pose:
+        """
+        Create a pose from a position with zero rotation.
+
+        :param position: The position.
+        :return: The pose.
+        """
+
+        return cls(position=position, orientation=Vector3.zeros())
+
+    def translate(self, offset: Vector3) -> Pose:
+        """
+        Create a new pose with the position translated by an offset.
+
+        :param offset: The translation offset.
+        :return: The translated pose.
+        """
+
+        return Pose(position=self.position + offset, orientation=self.orientation)
+
+    def rotate(self, rotation: Vector3) -> Pose:
+        """
+        Create a new pose with additional rotation (in RPY).
+
+        :param rotation: Additional rotation in Roll-Pitch-Yaw.
+        :return: The rotated pose.
+        """
+
+        return Pose(position=self.position, orientation=self.orientation + rotation)

common/message/quaternion.py

@@ -0,0 +1,273 @@
+from __future__ import annotations
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+from common.message.base import Message
+
+try:
+    from pxr import Gf  # type: ignore
+
+    HAS_PXR = True
+except ImportError:
+    HAS_PXR = False
+
+
+class Quaternion(Message):
+    """
+    A quaternion for 3D rotations.
+
+    Serializes as a tuple [w, x, y, z] for space efficiency where w is the scalar component.
+
+    Examples:
+        Quaternion.new(1.0, 0.0, 0.0, 0.0)  # Create identity quaternion
+        Quaternion.from_rpy(0.0, 0.0, 1.57)  # Create from RPY angles
+    """
+
+    _type = "antioch/quaternion"
+    data: tuple[float, float, float, float]
+
+    @property
+    def w(self) -> float:
+        """
+        Get the w (scalar) component.
+
+        :return: The w (scalar) component.
+        """
+
+        return self.data[0]
+
+    @property
+    def x(self) -> float:
+        """
+        Get the x component.
+
+        :return: The x component.
+        """
+
+        return self.data[1]
+
+    @property
+    def y(self) -> float:
+        """
+        Get the y component.
+
+        :return: The y component.
+        """
+
+        return self.data[2]
+
+    @property
+    def z(self) -> float:
+        """
+        Get the z component.
+
+        :return: The z component.
+        """
+
+        return self.data[3]
+
+    def __len__(self) -> int:
+        """
+        Get the length of the quaternion (always 4).
+
+        :return: The length of the quaternion.
+        """
+
+        return 4
+
+    def __iter__(self):
+        """
+        Iterate over quaternion components.
+
+        :return: Iterator over [w, x, y, z].
+        """
+
+        return iter(self.data)
+
+    def __getitem__(self, index: int) -> float:
+        """
+        Get quaternion component by index.
+
+        :param index: Index (0=w, 1=x, 2=y, 3=z).
+        :return: The component value.
+        """
+
+        return self.data[index]
+
+    def __eq__(self, other: object) -> bool:
+        """
+        Check equality with another Quaternion.
+
+        :param other: Another Quaternion.
+        :return: True if equal.
+        """
+
+        if not isinstance(other, Quaternion):
+            return False
+        return self.data == other.data
+
+    def __repr__(self) -> str:
+        """
+        Return a readable string representation.
+
+        :return: String representation.
+        """
+
+        return f"Quaternion(w={self.w}, x={self.x}, y={self.y}, z={self.z})"
+
+    def __str__(self) -> str:
+        """
+        Return a readable string representation.
+
+        :return: String representation.
+        """
+
+        return f"Quaternion(w={self.w}, x={self.x}, y={self.y}, z={self.z})"
+
+    @classmethod
+    def new(cls, w: float, x: float, y: float, z: float) -> Quaternion:
+        """
+        Create a new quaternion.
+
+        :param w: The w component.
+        :param x: The x component.
+        :param y: The y component.
+        :param z: The z component.
+        :return: A quaternion.
+        """
+
+        return cls(data=(w, x, y, z))
+
+    @classmethod
+    def identity(cls) -> Quaternion:
+        """
+        Create an identity quaternion (no rotation).
+
+        :return: An identity quaternion [1, 0, 0, 0].
+        """
+
+        return cls(data=(1.0, 0.0, 0.0, 0.0))
+
+    @classmethod
+    def from_numpy(cls, array: np.ndarray) -> Quaternion:
+        """
+        Create a quaternion from a numpy array [w, x, y, z].
+
+        :param array: The numpy array to create the quaternion from (must have 4 elements).
+        :return: A quaternion.
+        :raises ValueError: If the array does not have exactly 4 elements.
+        """
+
+        if array.shape != (4,):
+            raise ValueError(f"Quaternion array must have shape (4,), got {array.shape}")
+        return cls(data=(float(array[0]), float(array[1]), float(array[2]), float(array[3])))
+
+    @classmethod
+    def from_rpy(cls, roll: float, pitch: float, yaw: float) -> Quaternion:
+        """
+        Create a quaternion from roll-pitch-yaw angles (in radians).
+
+        :param roll: Roll angle in radians (rotation around x-axis).
+        :param pitch: Pitch angle in radians (rotation around y-axis).
+        :param yaw: Yaw angle in radians (rotation around z-axis).
+        :return: A quaternion.
+        """
+
+        rotation = Rotation.from_euler("xyz", [roll, pitch, yaw])
+        quat_xyzw = rotation.as_quat()
+        return cls(
+            data=(
+                float(quat_xyzw[3]),
+                float(quat_xyzw[0]),
+                float(quat_xyzw[1]),
+                float(quat_xyzw[2]),
+            )
+        )
+
+    def to_numpy(self) -> np.ndarray:
+        """
+        Convert the quaternion to a numpy array.
+
+        :return: The numpy array [w, x, y, z].
+        """
+
+        return np.array(self.data, dtype=np.float32)
+
+    def to_list(self) -> list[float]:
+        """
+        Convert the quaternion to a list.
+
+        :return: The list [w, x, y, z].
+        """
+
+        return list(self.data)
+
+    def to_gf_quatf(self) -> Gf.Quatf:
+        """
+        Convert the quaternion to a Gf.Quatf.
+
+        Note: Gf.Quatf expects (w, x, y, z) order.
+
+        :return: The Gf.Quatf.
+        :raises ImportError: If pxr is not installed.
+        """
+
+        if not HAS_PXR:
+            raise ImportError("pxr is not installed")
+        return Gf.Quatf(self.w, self.x, self.y, self.z)
+
+    def to_gf_quatd(self) -> Gf.Quatd:
+        """
+        Convert the quaternion to a Gf.Quatd (double precision).
+
+        Note: Gf.Quatd expects (w, x, y, z) order.
+
+        :return: The Gf.Quatd.
+        :raises ImportError: If pxr is not installed.
+        """
+
+        if not HAS_PXR:
+            raise ImportError("pxr is not installed")
+        return Gf.Quatd(self.w, self.x, self.y, self.z)
+
+    def normalize(self) -> Quaternion:
+        """
+        Return a normalized version of this quaternion.
+
+        :return: A normalized quaternion.
+        """
+
+        arr = self.to_numpy()
+        norm = np.linalg.norm(arr)
+        if norm > 0:
+            normalized = arr / norm
+            return self.__class__(
+                data=(
+                    float(normalized[0]),
+                    float(normalized[1]),
+                    float(normalized[2]),
+                    float(normalized[3]),
+                )
+            )
+        return self.identity()
+
+    def to_rpy(self) -> tuple[float, float, float]:
+        """
+        Convert the quaternion to roll-pitch-yaw angles (in radians).
+
+        :return: A tuple of (roll, pitch, yaw) angles in radians.
+        """
+
+        rotation = Rotation.from_quat([self.x, self.y, self.z, self.w])
+        rpy = rotation.as_euler("xyz")
+        return (float(rpy[0]), float(rpy[1]), float(rpy[2]))
+
+    def as_array(self) -> list[float]:
+        """
+        Convert to a list (alias for to_list for Rust compatibility).
+
+        :return: The list [w, x, y, z].
+        """
+
+        return self.to_list()

common/message/radar.py

@@ -0,0 +1,58 @@
+from pydantic import Field
+
+from common.message.base import Message
+from common.message.point_cloud import PointCloud
+from common.message.vector import Vector3
+
+
+class RadarDetection(Message):
+    """
+    Single radar detection point with position and properties.
+    """
+
+    _type = "antioch/radar_detection"
+    position: Vector3 = Field(description="3D position of detection in sensor frame")
+    range: float = Field(description="Range to target in meters")
+    azimuth: float = Field(description="Azimuth angle in radians")
+    elevation: float = Field(description="Elevation angle in radians")
+    velocity: float = Field(default=0.0, description="Radial velocity in m/s (positive = moving away)")
+    rcs: float = Field(default=0.0, description="Radar cross section in dBsm")
+
+
+class RadarScan(Message):
+    """
+    Radar scan data containing all detections from a single scan.
+    """
+
+    _type = "antioch/radar_scan"
+    detections: list[RadarDetection] = Field(default_factory=list, description="List of radar detections")
+
+    def to_point_cloud(self, frame_id: str = "radar") -> PointCloud:
+        """
+        Convert radar scan to a point cloud.
+
+        :param frame_id: Frame of reference for the point cloud.
+        :return: PointCloud with detection positions.
+        """
+
+        if not self.detections:
+            return PointCloud(frame_id=frame_id, x=[], y=[], z=[])
+
+        return PointCloud(
+            frame_id=frame_id,
+            x=[d.position.x for d in self.detections],
+            y=[d.position.y for d in self.detections],
+            z=[d.position.z for d in self.detections],
+        )
+
+    @staticmethod
+    def combine(scans: list["RadarScan"]) -> "RadarScan":
+        """
+        Combine multiple radar scans into a single scan.
+
+        :param scans: List of radar scans to combine.
+        :return: Combined radar scan with all detections.
+        """
+
+        detections = sum((scan.detections for scan in scans), [])
+        return RadarScan(detections=detections)

common/message/types.py

@@ -0,0 +1,37 @@
+from common.message.base import Message
+
+
+class Bool(Message):
+    """
+    Boolean value message.
+    """
+
+    _type = "antioch/bool"
+    value: bool
+
+
+class Int(Message):
+    """
+    Integer value message.
+    """
+
+    _type = "antioch/int"
+    value: int
+
+
+class Float(Message):
+    """
+    Float value message.
+    """
+
+    _type = "antioch/float"
+    value: float
+
+
+class String(Message):
+    """
+    String value message.
+    """
+
+    _type = "antioch/string"
+    value: str