feat(skills): typed 2D Odometry state for skills — quaternions kept as legacy

ros2_ws/src/brain/brain_client/brain_client/skills/odometry.py

@@ -0,0 +1,188 @@
+# SPDX-License-Identifier: Apache-2.0
+# Copyright (c) 2026 Innate Inc
+"""Skill-facing odometry state.
+
+MARS is a differential-drive base on flat ground, so its pose is fully
+described by (x, y, yaw) — skills get that directly instead of the raw ROS
+Odometry message with its quaternion orientation. This module is ROS-free on
+purpose; contexts without rclpy should import it from here directly (the
+`innate` namespace, where it is also exported, pulls in ROS-dependent skill
+types).
+"""
+
+import math
+import warnings
+from dataclasses import dataclass, field
+from functools import cached_property
+from typing import Any
+
+
+@dataclass(frozen=True)
+class Odometry:
+    """A 2D odometry snapshot: pose in the odom frame plus body velocities.
+
+    Injected for ``RobotState(RobotStateType.LAST_ODOM)`` and refreshed at
+    50 Hz while a skill runs.
+    """
+
+    x: float
+    """Position along X in meters, odom frame."""
+    y: float
+    """Position along Y in meters, odom frame."""
+    theta: float
+    """Yaw in radians, counter-clockwise positive, wrapped to [-pi, pi]."""
+    linear_velocity: float = 0.0
+    """Forward speed in m/s (negative when driving backward)."""
+    angular_velocity: float = 0.0
+    """Turn rate in rad/s, counter-clockwise positive."""
+    stamp: float = 0.0
+    """Sensor timestamp in seconds (ROS time). float64: sub-microsecond
+    precision at present-day epochs; the exact integer sec/nanosec are in
+    ``raw``."""
+    frame_id: str = "odom"
+    child_frame_id: str = "base_link"
+    raw_source: Any = field(default=None, repr=False, compare=False)
+    """Source for ``raw``: a rosbridge-style dict (served as-is) or a
+    nav_msgs/Odometry-like message (converted lazily on first access).
+    Excluded from ==/hash — it is provenance, not part of the 2D snapshot;
+    including it would also make production instances unhashable (dicts)."""
+
+    def __post_init__(self):
+        # theta's docstring is a contract (turn_in_place does heading math on
+        # it), so enforce it for hand-built instances too. The injected path
+        # already gets atan2 output; the guard keeps it trig-free at 50 Hz.
+        if not -math.pi <= self.theta <= math.pi:
+            # frozen dataclass: object.__setattr__ bypasses the immutability guard
+            object.__setattr__(self, "theta", math.atan2(math.sin(self.theta), math.cos(self.theta)))
+
+    @cached_property
+    def raw(self) -> dict | None:
+        """Escape hatch: the full nav_msgs/Odometry as plain data
+        (rosbridge-style keys) — real quaternion, z, covariances, full twist —
+        for skills that need more than the flat 2D pose. Built lazily so the
+        50 Hz injection path pays nothing for it until a skill asks."""
+        source = self.raw_source
+        if source is None or isinstance(source, dict):
+            return source
+        return _msg_to_raw(source)
+
+    @property
+    def theta_degrees(self) -> float:
+        """Yaw in degrees, counter-clockwise positive."""
+        return math.degrees(self.theta)
+
+    @property
+    def position(self) -> tuple[float, float]:
+        """(x, y) in meters, odom frame."""
+        return (self.x, self.y)
+
+    # --- legacy dict compatibility ---------------------------------------
+    # LAST_ODOM injected a raw-message dict from 0.3.0 through 0.6.x, so old
+    # skill files use dict-style access: odom["theta_degrees"], .get(), `in`,
+    # iteration, .keys()/.items()/.values(). The full read-only mapping
+    # protocol is provided so that code behaves exactly as it did on the real
+    # dict. Soft-deprecated (each call warns to nudge authors to the
+    # attributes) but kept as a permanent compatibility layer -- there is no
+    # scheduled removal, old skills keep working indefinitely. Do not delete.
+
+    def __getitem__(self, key):
+        return self._legacy_mapping()[key]
+
+    def __iter__(self):
+        return iter(self._legacy_mapping())
+
+    def __len__(self) -> int:
+        return len(self._legacy_mapping())
+
+    def __bool__(self) -> bool:
+        # without this, __len__ would define truthiness -- making the
+        # documented `if self.odom:` None-check fire the deprecation warning
+        return True
+
+    def get(self, key, default=None):
+        return self._legacy_mapping().get(key, default)
+
+    def __contains__(self, key) -> bool:
+        return key in self._legacy_mapping()
+
+    def keys(self):
+        return self._legacy_mapping().keys()
+
+    def items(self):
+        return self._legacy_mapping().items()
+
+    def values(self):
+        return self._legacy_mapping().values()
+
+    def _legacy_mapping(self) -> dict:
+        warnings.warn(
+            "dict-style odometry access is deprecated; use the Odometry "
+            "attributes instead (odom.x, odom.theta_degrees, ...) or odom.raw "
+            "for the full message",
+            DeprecationWarning,
+            stacklevel=3,  # past the dunder/method that called us, at user code
+        )
+        return self._legacy_dict
+
+    @cached_property
+    def _legacy_dict(self) -> dict:
+        """Exactly the 0.3.0-0.6.x injected shape — {header, child_frame_id,
+        pose.pose, theta_degrees}, no twist, no covariance — whichever way the
+        instance was built, so legacy access is path-independent. The extra
+        data ``raw`` carries stays on ``raw``. Memoized: enumeration protocols
+        (dict(odom), {**odom}) hit the mapping once per key."""
+        base = self.raw if self.raw is not None else self._reconstructed_raw()
+        return {
+            "header": base["header"],
+            "child_frame_id": base["child_frame_id"],
+            "pose": {"pose": base["pose"]["pose"]},
+            "theta_degrees": self.theta_degrees,
+        }
+
+    def _reconstructed_raw(self) -> dict:
+        """Legacy-shape fallback for instances built without ``raw_source``
+        (hand-constructed); the quaternion carries yaw only."""
+        sec = int(self.stamp)
+        half = self.theta / 2.0
+        return {
+            "header": {
+                "stamp": {"sec": sec, "nanosec": int(round((self.stamp - sec) * 1e9))},
+                "frame_id": self.frame_id,
+            },
+            "child_frame_id": self.child_frame_id,
+            "pose": {
+                "pose": {
+                    "position": {"x": self.x, "y": self.y, "z": 0.0},
+                    "orientation": {"x": 0.0, "y": 0.0, "z": math.sin(half), "w": math.cos(half)},
+                }
+            },
+        }
+
+
+def _msg_to_raw(msg) -> dict:
+    """nav_msgs/Odometry message -> plain rosbridge-style dict. Duck-typed
+    (attribute access only) so this module stays ROS-free."""
+    pos = msg.pose.pose.position
+    ori = msg.pose.pose.orientation
+    twist = msg.twist.twist
+    return {
+        "header": {
+            "stamp": {"sec": msg.header.stamp.sec, "nanosec": msg.header.stamp.nanosec},
+            "frame_id": msg.header.frame_id,
+        },
+        "child_frame_id": msg.child_frame_id,
+        "pose": {
+            "pose": {
+                "position": {"x": pos.x, "y": pos.y, "z": pos.z},
+                "orientation": {"x": ori.x, "y": ori.y, "z": ori.z, "w": ori.w},
+            },
+            "covariance": list(msg.pose.covariance),
+        },
+        "twist": {
+            "twist": {
+                "linear": {"x": twist.linear.x, "y": twist.linear.y, "z": twist.linear.z},
+                "angular": {"x": twist.angular.x, "y": twist.angular.y, "z": twist.angular.z},
+            },
+            "covariance": list(msg.twist.covariance),
+        },
+    }

ros2_ws/src/brain/brain_client/brain_client/skills/robot_state.py

@@ -17,11 +17,13 @@
 import time
 
 import numpy as np
-from nav_msgs.msg import OccupancyGrid, Odometry
+from nav_msgs.msg import OccupancyGrid
+from nav_msgs.msg import Odometry as OdometryMsg
 from sensor_msgs.msg import BatteryState, JointState
 from std_msgs.msg import String
 
 from brain_client.common.geometry import quaternion_to_yaw
+from brain_client.skills.odometry import Odometry
 from brain_client.skills.types import InterfaceType, RobotStateType
 
 
@@ -89,7 +91,7 @@ def start_subscriptions(self) -> None:
         if self._odom_sub is not None:
             return
         feed_node = self._manipulation.node
-        self._odom_sub = feed_node.create_subscription(Odometry, "/odom", self._on_odom, 10)
+        self._odom_sub = feed_node.create_subscription(OdometryMsg, "/odom", self._on_odom, 10)
         self._map_sub = feed_node.create_subscription(OccupancyGrid, "/map", self._on_map, 1)
         self._head_position_sub = feed_node.create_subscription(
             String, self._head_current_position_topic, self._on_head_position, 10
@@ -112,7 +114,7 @@ def stop_subscriptions(self) -> None:
         self.last_joint_states = None
         self.last_battery = None
 
-    def _on_odom(self, msg: Odometry) -> None:
+    def _on_odom(self, msg: OdometryMsg) -> None:
         if self._active:
             self.last_odom = msg
 
@@ -230,26 +232,22 @@ def update_skill_robot_state(self, skill) -> None:
 
         if RobotStateType.LAST_ODOM in required_states:
             if self.last_odom is not None:
-                pos = self.last_odom.pose.pose.position
-                ori = self.last_odom.pose.pose.orientation
-                theta = quaternion_to_yaw(ori)
-                robot_state_to_inject[RobotStateType.LAST_ODOM.value] = {
-                    "header": {
-                        "stamp": {
-                            "sec": self.last_odom.header.stamp.sec,
-                            "nanosec": self.last_odom.header.stamp.nanosec,
-                        },
-                        "frame_id": self.last_odom.header.frame_id,
-                    },
-                    "child_frame_id": self.last_odom.child_frame_id,
-                    "pose": {
-                        "pose": {
-                            "position": {"x": pos.x, "y": pos.y, "z": pos.z},
-                            "orientation": {"x": ori.x, "y": ori.y, "z": ori.z, "w": ori.w},
-                        }
-                    },
-                    "theta_degrees": math.degrees(theta),
-                }
+                msg = self.last_odom
+                pos = msg.pose.pose.position
+                twist = msg.twist.twist
+                robot_state_to_inject[RobotStateType.LAST_ODOM.value] = Odometry(
+                    x=pos.x,
+                    y=pos.y,
+                    theta=quaternion_to_yaw(msg.pose.pose.orientation),
+                    linear_velocity=twist.linear.x,
+                    angular_velocity=twist.angular.z,
+                    stamp=msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9,
+                    frame_id=msg.header.frame_id,
+                    child_frame_id=msg.child_frame_id,
+                    # .raw is built lazily from the message on first access, so
+                    # this 50 Hz path pays nothing for the full-fidelity dict
+                    raw_source=msg,
+                )
             else:
                 self._warn_missing("LAST_ODOM")
 

ros2_ws/src/brain/brain_client/innate/__init__.py

@@ -8,6 +8,7 @@
     from innate.skills import head_emotion, navigate_to_position
 """
 
+from brain_client.skills.odometry import Odometry
 from brain_client.skills.types import (
     Interface,
     InterfaceType,
@@ -22,6 +23,7 @@
 __all__ = [
     "Interface",
     "InterfaceType",
+    "Odometry",
     "RobotState",
     "RobotStateType",
     "Skill",

workspace/innate_skills/move_straight.py

@@ -94,11 +94,7 @@ def cancel(self):
 
     def _position(self):
         """Current (x, y) from the odometry robot state, or None if absent."""
-        try:
-            p = self.odom["pose"]["pose"]["position"]
-            return (p["x"], p["y"])
-        except (TypeError, KeyError):
-            return None
+        return self.odom.position if self.odom is not None else None
 
     def _wait_for_position(self):
         """(x, y) once odometry arrives, or None after ODOM_WAIT_SEC."""

workspace/innate_skills/turn_in_place.py

@@ -102,10 +102,7 @@ def cancel(self):
 
     def _yaw(self):
         """Current heading in degrees from the odometry state, or None."""
-        try:
-            return float(self.odom["theta_degrees"])
-        except (TypeError, KeyError):
-            return None
+        return self.odom.theta_degrees if self.odom is not None else None
 
     def _wait_for_yaw(self):
         """Heading once odometry arrives, or None after ODOM_WAIT_SEC."""