diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/odometry.py b/ros2_ws/src/brain/brain_client/brain_client/skills/odometry.py new file mode 100644 index 000000000..cbc210cc8 --- /dev/null +++ b/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), + }, + } diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/robot_state.py b/ros2_ws/src/brain/brain_client/brain_client/skills/robot_state.py index 2c3df4d0b..72de974ac 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/robot_state.py +++ b/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") diff --git a/ros2_ws/src/brain/brain_client/innate/__init__.py b/ros2_ws/src/brain/brain_client/innate/__init__.py index 9c760f4df..2e6a9f4f5 100644 --- a/ros2_ws/src/brain/brain_client/innate/__init__.py +++ b/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", diff --git a/workspace/innate_skills/move_straight.py b/workspace/innate_skills/move_straight.py index 8f1108b2a..c461b0ec2 100644 --- a/workspace/innate_skills/move_straight.py +++ b/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.""" diff --git a/workspace/innate_skills/turn_in_place.py b/workspace/innate_skills/turn_in_place.py index ca54b643e..ff70cdd95 100644 --- a/workspace/innate_skills/turn_in_place.py +++ b/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."""