antioch-py 2.0.6__py3-none-any.whl → 3.0.12__py3-none-any.whl

common/message/quaternion.py

@@ -1,9 +1,13 @@
 from __future__ import annotations
 
+from typing import Any
+
 import numpy as np
+from foxglove.schemas import Quaternion as FoxgloveQuaternion
+from pydantic import Field, model_validator
 from scipy.spatial.transform import Rotation
 
-from common.message.base import Message
+from common.message.message import Message
 
 try:
     from pxr import Gf  # type: ignore
@@ -15,57 +19,41 @@ except ImportError:
 
 class Quaternion(Message):
     """
-    A quaternion for 3D rotations.
+    A quaternion for 3D rotations (w, x, y, z) where w is the scalar component.
+
+    Supports multiple construction styles:
+    - Quaternion(w=1.0, x=0.0, y=0.0, z=0.0) - keyword args
+    - Quaternion.from_rpy(roll, pitch, yaw) - from euler angles
+    - Quaternion.from_any([1.0, 0.0, 0.0, 0.0]) - from list/tuple
+
+    Example:
+        ```python
+        from common.message import Quaternion
+        import math
+
+        # Create identity quaternion (no rotation)
+        identity = Quaternion.identity()
+
+        # Create from roll-pitch-yaw angles
+        quat = Quaternion.from_rpy(
+            roll=0.0,
+            pitch=0.0,
+            yaw=math.pi / 2,  # 90 degrees
+        )
 
-    Serializes as a tuple [w, x, y, z] for space efficiency where w is the scalar component.
+        # Convert back to RPY
+        roll, pitch, yaw = quat.to_rpy()
 
-    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
+        # Create from list (w, x, y, z order)
+        q = Quaternion.from_any([1.0, 0.0, 0.0, 0.0])
+        ```
     """
 
     _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]
+    w: float = Field(default=1.0, description="Scalar (real) component of the quaternion")
+    x: float = Field(default=0.0, description="X component of the quaternion vector part")
+    y: float = Field(default=0.0, description="Y component of the quaternion vector part")
+    z: float = Field(default=0.0, description="Z component of the quaternion vector part")
 
     def __len__(self) -> int:
         """
@@ -83,7 +71,7 @@ class Quaternion(Message):
         :return: Iterator over [w, x, y, z].
         """
 
-        return iter(self.data)
+        return iter((self.w, self.x, self.y, self.z))
 
     def __getitem__(self, index: int) -> float:
         """
@@ -93,7 +81,7 @@ class Quaternion(Message):
         :return: The component value.
         """
 
-        return self.data[index]
+        return (self.w, self.x, self.y, self.z)[index]
 
     def __eq__(self, other: object) -> bool:
         """
@@ -105,7 +93,7 @@ class Quaternion(Message):
 
         if not isinstance(other, Quaternion):
             return False
-        return self.data == other.data
+        return self.w == other.w and self.x == other.x and self.y == other.y and self.z == other.z
 
     def __repr__(self) -> str:
         """
@@ -114,7 +102,7 @@ class Quaternion(Message):
         :return: String representation.
         """
 
-        return f"Quaternion(w={self.w}, x={self.x}, y={self.y}, z={self.z})"
+        return f"Quaternion({self.w}, {self.x}, {self.y}, {self.z})"
 
     def __str__(self) -> str:
         """
@@ -123,45 +111,53 @@ class Quaternion(Message):
         :return: String representation.
         """
 
-        return f"Quaternion(w={self.w}, x={self.x}, y={self.y}, z={self.z})"
+        return f"Quaternion({self.w}, {self.x}, {self.y}, {self.z})"
 
     @classmethod
-    def new(cls, w: float, x: float, y: float, z: float) -> Quaternion:
+    def identity(cls) -> Quaternion:
         """
-        Create a new quaternion.
+        Create an identity quaternion (no rotation).
 
-        :param w: The w component.
-        :param x: The x component.
-        :param y: The y component.
-        :param z: The z component.
-        :return: A quaternion.
+        :return: An identity quaternion [1, 0, 0, 0].
         """
 
-        return cls(data=(w, x, y, z))
+        return cls(w=1.0, x=0.0, y=0.0, z=0.0)
 
     @classmethod
-    def identity(cls) -> Quaternion:
+    def from_any(cls, value: Quaternion | tuple | list | dict | np.ndarray | None) -> Quaternion:
         """
-        Create an identity quaternion (no rotation).
+        Create Quaternion from any compatible type.
 
-        :return: An identity quaternion [1, 0, 0, 0].
+        Accepts:
+        - Quaternion instance (returned as-is)
+        - Tuple/list of 4 floats: (w, x, y, z)
+        - Dict with 'w', 'x', 'y', and 'z' keys
+        - Numpy array of shape (4,)
+        - None (returns identity quaternion)
+
+        :param value: Any compatible input type.
+        :return: A Quaternion instance.
         """
 
-        return cls(data=(1.0, 0.0, 0.0, 0.0))
+        if value is None:
+            return cls.identity()
+        if isinstance(value, cls):
+            return value
+        return cls.model_validate(value)
 
     @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).
+        :param array: The numpy array (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])))
+        return cls(w=float(array[0]), x=float(array[1]), y=float(array[2]), z=float(array[3]))
 
     @classmethod
     def from_rpy(cls, roll: float, pitch: float, yaw: float) -> Quaternion:
@@ -174,16 +170,9 @@ class Quaternion(Message):
         :return: A quaternion.
         """
 
-        rotation = Rotation.from_euler("xyz", [roll, pitch, yaw])
+        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]),
-            )
-        )
+        return cls(w=float(quat_xyzw[3]), x=float(quat_xyzw[0]), y=float(quat_xyzw[1]), z=float(quat_xyzw[2]))
 
     def to_numpy(self) -> np.ndarray:
         """
@@ -192,7 +181,7 @@ class Quaternion(Message):
         :return: The numpy array [w, x, y, z].
         """
 
-        return np.array(self.data, dtype=np.float32)
+        return np.array([self.w, self.x, self.y, self.z], dtype=np.float32)
 
     def to_list(self) -> list[float]:
         """
@@ -201,7 +190,16 @@ class Quaternion(Message):
         :return: The list [w, x, y, z].
         """
 
-        return list(self.data)
+        return [self.w, self.x, self.y, self.z]
+
+    def to_tuple(self) -> tuple[float, float, float, float]:
+        """
+        Convert the quaternion to a tuple.
+
+        :return: The tuple (w, x, y, z).
+        """
+
+        return (self.w, self.x, self.y, self.z)
 
     def to_gf_quatf(self) -> Gf.Quatf:
         """
@@ -213,8 +211,8 @@ class Quaternion(Message):
         :raises ImportError: If pxr is not installed.
         """
 
-        if not HAS_PXR:
-            raise ImportError("pxr is not installed")
+        from pxr import Gf
+
         return Gf.Quatf(self.w, self.x, self.y, self.z)
 
     def to_gf_quatd(self) -> Gf.Quatd:
@@ -227,8 +225,8 @@ class Quaternion(Message):
         :raises ImportError: If pxr is not installed.
         """
 
-        if not HAS_PXR:
-            raise ImportError("pxr is not installed")
+        from pxr import Gf
+
         return Gf.Quatd(self.w, self.x, self.y, self.z)
 
     def normalize(self) -> Quaternion:
@@ -242,13 +240,11 @@ class Quaternion(Message):
         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 Quaternion(
+                w=float(normalized[0]),
+                x=float(normalized[1]),
+                y=float(normalized[2]),
+                z=float(normalized[3]),
             )
         return self.identity()
 
@@ -263,11 +259,28 @@ class Quaternion(Message):
         rpy = rotation.as_euler("xyz")
         return (float(rpy[0]), float(rpy[1]), float(rpy[2]))
 
-    def as_array(self) -> list[float]:
+    def to_foxglove(self) -> FoxgloveQuaternion:
         """
-        Convert to a list (alias for to_list for Rust compatibility).
+        Convert to Foxglove Quaternion for telemetry.
 
-        :return: The list [w, x, y, z].
+        :return: Foxglove Quaternion schema.
+        """
+
+        return FoxgloveQuaternion(x=self.x, y=self.y, z=self.z, w=self.w)
+
+    @model_validator(mode="before")
+    @classmethod
+    def _convert_input(cls, data: Any) -> Any:
+        """
+        Convert various input types to Quaternion format.
         """
 
-        return self.to_list()
+        if isinstance(data, cls):
+            return {"w": data.w, "x": data.x, "y": data.y, "z": data.z}
+        if isinstance(data, (tuple, list)) and len(data) == 4:
+            return {"w": float(data[0]), "x": float(data[1]), "y": float(data[2]), "z": float(data[3])}
+        if isinstance(data, np.ndarray) and data.shape == (4,):
+            return {"w": float(data[0]), "x": float(data[1]), "y": float(data[2]), "z": float(data[3])}
+        if isinstance(data, dict) and all(k in data for k in ("w", "x", "y", "z")):
+            return {"w": float(data["w"]), "x": float(data["x"]), "y": float(data["y"]), "z": float(data["z"])}
+        return data

common/message/radar.py

@@ -1,31 +1,106 @@
+from __future__ import annotations
+
+from typing import Any
+
+import numpy as np
 from pydantic import Field
 
-from common.message.base import Message
+from common.message.array import Array
+from common.message.image import Image, ImageEncoding
+from common.message.message 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.
+class RadarScan(Message):
     """
+    Radar scan data containing all detections from a single scan.
 
-    _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")
+    Data is stored as numpy arrays (via Array) for efficient processing.
 
+    Example:
+        ```python
+        from common.message import RadarScan
 
-class RadarScan(Message):
-    """
-    Radar scan data containing all detections from a single scan.
+        # Access detection data efficiently via arrays
+        scan = radar.get_scan()
+        ranges = scan.ranges.to_numpy()
+        rcs = scan.rcs.to_numpy()
+
+        # Filter detections by range
+        close_mask = ranges < 10.0
+        close_rcs = rcs[close_mask]
+
+        # Convert to point cloud for visualization
+        point_cloud = scan.to_point_cloud(frame_id="radar_link")
+        ```
     """
 
     _type = "antioch/radar_scan"
-    detections: list[RadarDetection] = Field(default_factory=list, description="List of radar detections")
+
+    # Raw spherical coordinates (stored as Arrays for serialization)
+    ranges: Array = Field(default_factory=lambda: Array.zeros(0), description="Range to targets in meters")
+    azimuths: Array = Field(default_factory=lambda: Array.zeros(0), description="Azimuth angles in radians")
+    elevations: Array = Field(default_factory=lambda: Array.zeros(0), description="Elevation angles in radians")
+    rcs: Array = Field(default_factory=lambda: Array.zeros(0), description="Radar cross section in dBsm")
+    velocities: Array = Field(default_factory=lambda: Array.zeros(0), description="Radial velocities in m/s")
+
+    # Cartesian positions (computed from spherical, stored for efficiency)
+    x: Array = Field(default_factory=lambda: Array.zeros(0), description="X positions in sensor frame")
+    y: Array = Field(default_factory=lambda: Array.zeros(0), description="Y positions in sensor frame")
+    z: Array = Field(default_factory=lambda: Array.zeros(0), description="Z positions in sensor frame")
+
+    @property
+    def num_detections(self) -> int:
+        """
+        Number of detections in this scan.
+        """
+
+        return len(self.ranges)
+
+    @classmethod
+    def from_arrays(
+        cls,
+        ranges: np.ndarray,
+        azimuths: np.ndarray,
+        elevations: np.ndarray,
+        rcs: np.ndarray,
+        velocities: np.ndarray,
+        x: np.ndarray,
+        y: np.ndarray,
+        z: np.ndarray,
+    ) -> RadarScan:
+        """
+        Create RadarScan directly from numpy arrays.
+
+        This is the most efficient way to create a RadarScan as it avoids
+        creating any intermediate Python objects.
+
+        :param ranges: Range values in meters.
+        :param azimuths: Azimuth angles in radians.
+        :param elevations: Elevation angles in radians.
+        :param rcs: RCS values in dBsm.
+        :param velocities: Radial velocities in m/s.
+        :param x: X positions in sensor frame.
+        :param y: Y positions in sensor frame.
+        :param z: Z positions in sensor frame.
+        :return: RadarScan instance.
+        :raises ValueError: If arrays have mismatched lengths.
+        """
+
+        n = len(ranges)
+        if not all(len(arr) == n for arr in [azimuths, elevations, rcs, velocities, x, y, z]):
+            raise ValueError("All arrays must have the same length")
+
+        return cls(
+            ranges=Array.from_numpy(ranges.astype(np.float32)),
+            azimuths=Array.from_numpy(azimuths.astype(np.float32)),
+            elevations=Array.from_numpy(elevations.astype(np.float32)),
+            rcs=Array.from_numpy(rcs.astype(np.float32)),
+            velocities=Array.from_numpy(velocities.astype(np.float32)),
+            x=Array.from_numpy(x.astype(np.float32)),
+            y=Array.from_numpy(y.astype(np.float32)),
+            z=Array.from_numpy(z.astype(np.float32)),
+        )
 
     def to_point_cloud(self, frame_id: str = "radar") -> PointCloud:
         """
@@ -35,24 +110,115 @@ class RadarScan(Message):
         :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],
+            x=self.x.to_list(),
+            y=self.y.to_list(),
+            z=self.z.to_list(),
         )
 
-    @staticmethod
-    def combine(scans: list["RadarScan"]) -> "RadarScan":
+    def to_foxglove(self) -> dict[str, Any]:
+        """
+        Convert to a JSON-serializable dict for Foxglove visualization.
+
+        :return: Dictionary with all scan data as lists.
+        """
+
+        return {
+            "num_detections": self.num_detections,
+            "ranges": self.ranges.to_list(),
+            "azimuths": self.azimuths.to_list(),
+            "elevations": self.elevations.to_list(),
+            "rcs": self.rcs.to_list(),
+            "velocities": self.velocities.to_list(),
+            "x": self.x.to_list(),
+            "y": self.y.to_list(),
+            "z": self.z.to_list(),
+        }
+
+
+class RangeMap(Message):
+    """
+    1D range bin map for radar processing.
+
+    Represents radar signal strength across range bins, commonly used for
+    MTI (Moving Target Indication) processing and detection.
+    """
+
+    _type = "antioch/range_map"
+
+    r_bins: int = Field(description="Number of range bins")
+    r_min_m: float = Field(description="Minimum range in meters (inclusive)")
+    r_max_m: float = Field(description="Maximum range in meters (inclusive)")
+    dr_m: float = Field(description="Range bin size in meters")
+    data: Array = Field(description="Float32 array of shape (r_bins,)")
+
+    def to_numpy(self) -> np.ndarray:
+        """
+        Convert the range map to a 1D numpy array.
+
+        :return: float32 array with shape (r_bins,).
+        """
+
+        return self.data.to_numpy().reshape(self.r_bins)
+
+    def to_image(
+        self,
+        *,
+        transform: str = "log1p",
+        encoding: ImageEncoding = ImageEncoding.MONO8,
+        clip_percentile: float = 99.5,
+        height: int = 32,
+    ) -> Image:
+        """
+        Convert this range map into an Image for visualization.
+
+        The image is a horizontal bar where x-axis is range bins.
+
+        :param transform: "linear" or "log1p".
+        :param encoding: Image encoding to use (default MONO8).
+        :param clip_percentile: When encoding is MONO8, clip values to this percentile.
+        :param height: Height of the output image in pixels.
+        :return: Image representing the range map.
+        """
+
+        data = self.to_numpy()
+        if transform == "log1p":
+            values = np.log1p(data).astype(np.float32)
+        elif transform == "linear":
+            values = data.astype(np.float32, copy=False)
+        else:
+            raise ValueError(f"Unsupported transform '{transform}'")
+
+        if encoding == ImageEncoding.DEPTH_F32:
+            tiled = np.tile(values.reshape(1, -1), (height, 1))
+            return Image.from_numpy(tiled.astype(np.float32, copy=False), encoding=encoding)
+
+        if encoding != ImageEncoding.MONO8:
+            raise ValueError(f"Unsupported encoding '{encoding.value}' for range map image")
+
+        # Normalize to 8-bit
+        vmax = float(np.percentile(values, clip_percentile)) if values.size else 0.0
+        if vmax <= 0.0 or not np.isfinite(vmax):
+            mono = np.zeros((height, self.r_bins), dtype=np.uint8)
+        else:
+            scaled = np.clip(values / np.float32(vmax), 0.0, 1.0)
+            mono_row = (scaled * np.float32(255.0)).astype(np.uint8, copy=False)
+            mono = np.tile(mono_row.reshape(1, -1), (height, 1))
+
+        return Image.from_numpy(mono, encoding=encoding)
+
+    def to_foxglove(self) -> dict[str, Any]:
         """
-        Combine multiple radar scans into a single scan.
+        Convert to a JSON-serializable dict for Foxglove visualization.
 
-        :param scans: List of radar scans to combine.
-        :return: Combined radar scan with all detections.
+        :return: Dictionary with range map metadata and data as list.
         """
 
-        detections = sum((scan.detections for scan in scans), [])
-        return RadarScan(detections=detections)
+        return {
+            "r_bins": self.r_bins,
+            "r_min_m": self.r_min_m,
+            "r_max_m": self.r_max_m,
+            "dr_m": self.dr_m,
+            "data": self.data.to_list(),
+        }

common/message/twist.py

@@ -0,0 +1,34 @@
+from pydantic import Field
+
+from common.message.message import Message
+from common.message.vector import Vector3
+
+
+class Twist(Message):
+    """
+    Linear and angular velocity (twist).
+
+    Represents the velocity of a rigid body in 3D space, combining both
+    linear velocity (translation) and angular velocity (rotation).
+
+    Example:
+        ```python
+        from common.message import Twist, Vector3
+
+        # Create a twist for forward motion with rotation
+        twist = Twist(
+            linear=Vector3(x=1.0, y=0.0, z=0.0),  # 1 m/s forward
+            angular=Vector3(x=0.0, y=0.0, z=0.1),  # 0.1 rad/s yaw
+        )
+
+        # Zero velocity
+        stationary = Twist(
+            linear=Vector3.zeros(),
+            angular=Vector3.zeros(),
+        )
+        ```
+    """
+
+    _type = "antioch/twist"
+    linear: Vector3 = Field(description="Linear velocity in m/s (x, y, z)")
+    angular: Vector3 = Field(description="Angular velocity in rad/s (x, y, z)")

common/message/types.py

@@ -1,37 +1,72 @@
-from common.message.base import Message
+from pydantic import Field
+
+from common.message.message import Message
 
 
 class Bool(Message):
     """
     Boolean value message.
+
+    Example:
+        ```python
+        from common.message import Bool
+
+        msg = Bool(value=True)
+        if msg.value:
+            print("Value is true")
+        ```
     """
 
     _type = "antioch/bool"
-    value: bool
+    value: bool = Field(description="Boolean value")
 
 
 class Int(Message):
     """
     Integer value message.
+
+    Example:
+        ```python
+        from common.message import Int
+
+        msg = Int(value=42)
+        print(f"The answer is {msg.value}")
+        ```
     """
 
     _type = "antioch/int"
-    value: int
+    value: int = Field(description="Integer value")
 
 
 class Float(Message):
     """
     Float value message.
+
+    Example:
+        ```python
+        from common.message import Float
+
+        msg = Float(value=3.14159)
+        print(f"Pi is approximately {msg.value:.2f}")
+        ```
     """
 
     _type = "antioch/float"
-    value: float
+    value: float = Field(description="Floating-point value")
 
 
 class String(Message):
     """
     String value message.
+
+    Example:
+        ```python
+        from common.message import String
+
+        msg = String(value="Hello, World!")
+        print(msg.value)
+        ```
     """
 
     _type = "antioch/string"
-    value: str
+    value: str = Field(description="String value")