antioch-py 2.2.4__py3-none-any.whl → 3.0.0__py3-none-any.whl

common/message/pose.py

@@ -3,9 +3,10 @@ from __future__ import annotations
 from typing import Any
 
 import numpy as np
+from foxglove.schemas import Pose as FoxglovePose, Quaternion as FoxgloveQuaternion, Vector3 as FoxgloveVector3
 from pydantic import Field, field_validator, model_validator
 
-from common.message.base import Message
+from common.message.message import Message
 from common.message.vector import Vector3
 
 
@@ -19,11 +20,31 @@ class Pose(Message):
     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})
+
+    Example:
+        ```python
+        from common.message import Pose, Vector3
+
+        # Create a pose with position and orientation
+        pose = Pose(
+            position=[1.0, 2.0, 0.5],
+            orientation=[0.0, 0.0, 1.57],  # 90 degrees yaw
+        )
+
+        # Create from identity
+        identity = Pose.identity()
+
+        # Translate a pose
+        translated = pose.translate(Vector3(x=1.0, y=0.0, z=0.0))
+
+        # Access position components
+        print(f"Position: ({pose.position.x}, {pose.position.y}, {pose.position.z})")
+        ```
     """
 
     _type = "antioch/pose"
-    position: Vector3 = Field(default_factory=lambda: Vector3.zeros())
-    orientation: Vector3 = Field(default_factory=lambda: Vector3.zeros())
+    position: Vector3 = Field(default_factory=lambda: Vector3.zeros(), description="Position in 3D space as (x, y, z)")
+    orientation: Vector3 = Field(default_factory=lambda: Vector3.zeros(), description="Orientation as (roll, pitch, yaw) in radians")
 
     def __add__(self, other: Vector3) -> Pose:
         """
@@ -71,7 +92,7 @@ class Pose(Message):
         """
 
         if isinstance(v, (list, tuple, np.ndarray)):
-            return Vector3.convert_iterables.__func__(Vector3, v)
+            return Vector3._convert_input.__func__(Vector3, v)
         return v
 
     @field_validator("orientation", mode="before")
@@ -82,29 +103,34 @@ class Pose(Message):
         """
 
         if isinstance(v, (list, tuple, np.ndarray)):
-            return Vector3.convert_iterables.__func__(Vector3, v)
+            return Vector3._convert_input.__func__(Vector3, v)
         return v
 
     @classmethod
-    def from_any(cls, data: Any) -> Pose:
+    def from_any(cls, value: Pose | dict | tuple | list | None) -> Pose:
         """
-        Create Pose from a dict or Pose instance.
+        Create Pose from any compatible type.
+
+        Accepts:
+        - Pose instance (returned as-is)
+        - Dict with 'position' and/or 'orientation' keys
+        - Tuple/list of 3 floats (treated as position only)
+        - None (returns identity pose)
 
-        :param data: Dict with position/orientation fields, or a Pose instance.
+        :param value: Any compatible input type.
         :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
+        if value is None:
+            return cls.identity()
+        if isinstance(value, cls):
+            return value
+        if isinstance(value, dict):
+            return cls.model_validate(value)
+        if isinstance(value, (tuple, list)):
+            # Treat as position only
+            return cls(position=Vector3.from_any(value))
+        raise ValueError(f"Cannot convert {type(value).__name__} to Pose")
 
     @classmethod
     def identity(cls) -> Pose:
@@ -146,3 +172,17 @@ class Pose(Message):
         """
 
         return Pose(position=self.position, orientation=self.orientation + rotation)
+
+    def to_foxglove(self) -> FoxglovePose:
+        """
+        Convert to Foxglove Pose for telemetry.
+
+        :return: Foxglove Pose schema.
+        """
+
+        # Convert RPY orientation to quaternion for Foxglove
+        quat = self.orientation.to_quat()
+        return FoxglovePose(
+            position=FoxgloveVector3(x=self.position.x, y=self.position.y, z=self.position.z),
+            orientation=FoxgloveQuaternion(x=quat.x, y=quat.y, z=quat.z, w=quat.w),
+        )

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