Merge pull request 'feat(perception): wheel encoder differential drive odometry (Issue #184)' (#318) from sl-perception/issue-184-wheel-odom into main

jetson/ros2_ws/src/saltybot_bridge_msgs/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.8)
+project(saltybot_bridge_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/WheelTicks.msg"
+  DEPENDENCIES std_msgs builtin_interfaces
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

jetson/ros2_ws/src/saltybot_bridge_msgs/msg/WheelTicks.msg

@@ -0,0 +1,11 @@
+# WheelTicks.msg — cumulative wheel encoder tick counts from STM32 (Issue #184)
+#
+# left_ticks  : cumulative left  encoder count (int32, wraps at ±2^31)
+# right_ticks : cumulative right encoder count (int32, wraps at ±2^31)
+#
+# Receivers must handle int32 rollover by computing delta relative to the
+# previous message value.  The wheel_odom_node does this via unwrap_delta().
+#
+std_msgs/Header header
+int32 left_ticks
+int32 right_ticks

jetson/ros2_ws/src/saltybot_bridge_msgs/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_bridge_msgs</name>
+  <version>0.1.0</version>
+  <description>STM32 bridge message definitions — wheel encoder ticks and low-level hardware telemetry (Issue #184)</description>
+  <maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <depend>builtin_interfaces</depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_bringup/package.xml

@@ -27,6 +27,9 @@
   <exec_depend>saltybot_perception</exec_depend>
   <!-- HSV color segmentation messages (Issue #274) -->
   <exec_depend>saltybot_scene_msgs</exec_depend>
+  <!-- Wheel encoder messages + odometry (Issue #184) -->
+  <exec_depend>saltybot_bridge_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
   <exec_depend>saltybot_uwb</exec_depend>
 
   <buildtool_depend>ament_python</buildtool_depend>

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_wheel_odom.py

@@ -0,0 +1,161 @@
+"""
+_wheel_odom.py — Differential drive wheel odometry kinematics (no ROS2 deps).
+
+Algorithm
+---------
+Given incremental left/right wheel displacements (metres) since the last update:
+
+    d_center = (d_left + d_right) / 2
+    d_theta  = (d_right − d_left) / wheel_base
+
+Pose is integrated using the midpoint Euler method (equivalent to the exact
+ICC solution for a circular arc, but simpler and numerically robust):
+
+    mid_theta = theta + d_theta / 2
+    x        += d_center · cos(mid_theta)
+    y        += d_center · sin(mid_theta)
+    theta    += d_theta
+
+theta is kept in (−π, π] after every step.
+
+Int32 rollover
+--------------
+STM32 encoder counters are int32 and wrap at ±2^31.  `unwrap_delta` handles
+this by detecting jumps larger than half the int32 range and adjusting by the
+full range:
+
+    if  delta >  2^30 : delta -= 2^31
+    if  delta < -2^30 : delta += 2^31
+
+This correctly handles up to ½ of the full int32 range per message interval —
+safely above any realistic tick rate at 50 Hz.
+
+Public API
+----------
+  WheelOdomState(x, y, theta)
+  unwrap_delta(current, prev)        -> int
+  ticks_to_meters(ticks, radius, ticks_per_rev) -> float
+  integrate_odom(x, y, theta, d_left_m, d_right_m, wheel_base) -> (x, y, theta)
+  normalize_angle(theta)             -> float
+  quat_from_yaw(yaw)                 -> (qx, qy, qz, qw)
+"""
+
+from __future__ import annotations
+
+import math
+from typing import NamedTuple, Tuple
+
+
+# ── Data types ────────────────────────────────────────────────────────────────
+
+class WheelOdomState(NamedTuple):
+    """Current dead-reckoning pose estimate."""
+    x:     float   # metres (world frame)
+    y:     float   # metres (world frame)
+    theta: float   # radians, kept in (−π, π]
+
+
+# ── Int32 rollover handling ───────────────────────────────────────────────────
+
+_INT32_HALF = 2 ** 31   # half of the full int32 value range (2^32) — detection threshold
+_INT32_FULL = 2 ** 32   # full int32 value range — adjustment amount
+
+
+def unwrap_delta(current: int, prev: int) -> int:
+    """
+    Compute signed tick delta handling int32 counter rollover.
+
+    Parameters
+    ----------
+    current : current tick count (int32, may have wrapped)
+    prev    : previous tick count
+
+    Returns
+    -------
+    Signed delta ticks, correct even across the ±2^31 rollover boundary.
+    """
+    delta = int(current) - int(prev)
+    if delta > _INT32_HALF:
+        delta -= _INT32_FULL
+    elif delta < -_INT32_HALF:
+        delta += _INT32_FULL
+    return delta
+
+
+# ── Unit conversion ───────────────────────────────────────────────────────────
+
+def ticks_to_meters(delta_ticks: int, wheel_radius: float, ticks_per_rev: int) -> float:
+    """
+    Convert encoder tick count to linear wheel displacement in metres.
+
+    Parameters
+    ----------
+    delta_ticks  : signed tick increment
+    wheel_radius : effective wheel radius (metres)
+    ticks_per_rev: encoder ticks per full wheel revolution
+
+    Returns
+    -------
+    Linear displacement (metres); positive = forward.
+    """
+    if ticks_per_rev <= 0:
+        return 0.0
+    return delta_ticks * (2.0 * math.pi * wheel_radius) / ticks_per_rev
+
+
+# ── Pose integration ──────────────────────────────────────────────────────────
+
+def normalize_angle(theta: float) -> float:
+    """Wrap angle to (−π, π]."""
+    return math.atan2(math.sin(theta), math.cos(theta))
+
+
+def integrate_odom(
+    x:          float,
+    y:          float,
+    theta:      float,
+    d_left_m:   float,
+    d_right_m:  float,
+    wheel_base: float,
+) -> Tuple[float, float, float]:
+    """
+    Integrate differential drive kinematics using the midpoint Euler method.
+
+    Parameters
+    ----------
+    x, y, theta  : current pose (metres, metres, radians)
+    d_left_m     : left  wheel displacement since last update (metres)
+    d_right_m    : right wheel displacement since last update (metres)
+    wheel_base   : centre-to-centre wheel separation (metres)
+
+    Returns
+    -------
+    (x, y, theta) updated pose
+    """
+    d_center = (d_left_m + d_right_m) / 2.0
+    d_theta  = (d_right_m - d_left_m) / wheel_base
+
+    mid_theta = theta + d_theta / 2.0
+    new_x     = x + d_center * math.cos(mid_theta)
+    new_y     = y + d_center * math.sin(mid_theta)
+    new_theta = normalize_angle(theta + d_theta)
+
+    return new_x, new_y, new_theta
+
+
+# ── Quaternion helper ─────────────────────────────────────────────────────────
+
+def quat_from_yaw(yaw: float) -> Tuple[float, float, float, float]:
+    """
+    Convert a yaw angle (rotation about Z) to a unit quaternion.
+
+    Parameters
+    ----------
+    yaw : rotation angle in radians
+
+    Returns
+    -------
+    (qx, qy, qz, qw)
+    """
+    half = yaw / 2.0
+    return (0.0, 0.0, math.sin(half), math.cos(half))

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/wheel_odom_node.py

@@ -0,0 +1,210 @@
+"""
+wheel_odom_node.py — Differential drive wheel encoder odometry (Issue #184).
+
+Subscribes to raw encoder tick counts from the STM32 bridge, integrates
+differential drive kinematics, and publishes nav_msgs/Odometry at 50 Hz.
+Optionally broadcasts the odom → base_link TF transform.
+
+Subscribes:
+  /saltybot/wheel_ticks      saltybot_bridge_msgs/WheelTicks   (RELIABLE)
+
+Publishes:
+  /odom_wheel                nav_msgs/Odometry   at publish_hz (default 50 Hz)
+
+TF broadcast (when publish_tf=true):
+  odom → base_link
+
+Parameters
+----------
+wheel_radius   float   0.034   Effective wheel radius (metres)
+wheel_base     float   0.160   Centre-to-centre wheel separation (metres)
+ticks_per_rev  int     1000    Encoder ticks per full wheel revolution
+publish_hz     float   50.0    Odometry publication rate (Hz)
+odom_frame_id  str     odom    Frame id for odometry header
+base_frame_id  str     base_link  Child frame id
+publish_tf     bool    true    Whether to broadcast odom → base_link TF
+"""
+
+from __future__ import annotations
+
+import math
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+import tf2_ros
+from geometry_msgs.msg import TransformStamped
+
+from nav_msgs.msg import Odometry
+from saltybot_bridge_msgs.msg import WheelTicks
+
+from ._wheel_odom import (
+    WheelOdomState,
+    unwrap_delta,
+    ticks_to_meters,
+    integrate_odom,
+    quat_from_yaw,
+)
+
+
+# Covariance matrices (6×6 diagonal, row-major).
+# Indices: 0=x, 7=y, 14=z, 21=roll, 28=pitch, 35=yaw
+_POSE_COV  = [0.0] * 36
+_TWIST_COV = [0.0] * 36
+_POSE_COV[0]   = 1e-3   # x
+_POSE_COV[7]   = 1e-3   # y
+_POSE_COV[35]  = 1e-3   # yaw
+_TWIST_COV[0]  = 1e-3   # vx
+_TWIST_COV[35] = 1e-3   # v_yaw
+
+
+class WheelOdomNode(Node):
+
+    def __init__(self) -> None:
+        super().__init__('wheel_odom_node')
+
+        self.declare_parameter('wheel_radius',  0.034)
+        self.declare_parameter('wheel_base',    0.160)
+        self.declare_parameter('ticks_per_rev', 1000)
+        self.declare_parameter('publish_hz',    50.0)
+        self.declare_parameter('odom_frame_id', 'odom')
+        self.declare_parameter('base_frame_id', 'base_link')
+        self.declare_parameter('publish_tf',    True)
+
+        self._radius    = float(self.get_parameter('wheel_radius').value)
+        self._base      = float(self.get_parameter('wheel_base').value)
+        self._ticks_rev = int(self.get_parameter('ticks_per_rev').value)
+        publish_hz      = float(self.get_parameter('publish_hz').value)
+        self._odom_fid  = self.get_parameter('odom_frame_id').value
+        self._base_fid  = self.get_parameter('base_frame_id').value
+        self._pub_tf    = bool(self.get_parameter('publish_tf').value)
+
+        # Running state
+        self._x     = 0.0
+        self._y     = 0.0
+        self._theta = 0.0
+        self._prev_left:  int | None = None
+        self._prev_right: int | None = None
+
+        # Velocity accumulation between timer callbacks
+        self._accum_d:     float = 0.0
+        self._accum_dtheta: float = 0.0
+
+        self._sub = self.create_subscription(
+            WheelTicks,
+            '/saltybot/wheel_ticks',
+            self._on_ticks,
+            QoSProfile(
+                reliability=ReliabilityPolicy.RELIABLE,
+                history=HistoryPolicy.KEEP_LAST,
+                depth=10,
+            ),
+        )
+        self._pub = self.create_publisher(Odometry, '/odom_wheel', 10)
+
+        if self._pub_tf:
+            self._tf_br = tf2_ros.TransformBroadcaster(self)
+        else:
+            self._tf_br = None
+
+        period = 1.0 / max(publish_hz, 1.0)
+        self._last_pub_time = self.get_clock().now()
+        self._timer = self.create_timer(period, self._on_timer)
+
+        self.get_logger().info(
+            f'wheel_odom_node ready — '
+            f'radius={self._radius}m base={self._base}m '
+            f'ticks_per_rev={self._ticks_rev} hz={publish_hz}'
+        )
+
+    # ── Tick callback (updates pose immediately) ──────────────────────────────
+
+    def _on_ticks(self, msg: WheelTicks) -> None:
+        if self._prev_left is None:
+            self._prev_left  = msg.left_ticks
+            self._prev_right = msg.right_ticks
+            return
+
+        dl = unwrap_delta(msg.left_ticks,  self._prev_left)
+        dr = unwrap_delta(msg.right_ticks, self._prev_right)
+        self._prev_left  = msg.left_ticks
+        self._prev_right = msg.right_ticks
+
+        d_left_m  = ticks_to_meters(dl, self._radius, self._ticks_rev)
+        d_right_m = ticks_to_meters(dr, self._radius, self._ticks_rev)
+
+        self._x, self._y, self._theta = integrate_odom(
+            self._x, self._y, self._theta,
+            d_left_m, d_right_m, self._base,
+        )
+
+        # Accumulate for velocity estimation in the timer
+        self._accum_d      += (d_left_m + d_right_m) / 2.0
+        self._accum_dtheta += (d_right_m - d_left_m) / self._base
+
+    # ── Timer callback (publishes at fixed rate) ──────────────────────────────
+
+    def _on_timer(self) -> None:
+        now = self.get_clock().now()
+        dt  = (now - self._last_pub_time).nanoseconds * 1e-9
+        self._last_pub_time = now
+
+        # Velocity from accumulated incremental motion over the timer period
+        vx    = self._accum_d      / dt if dt > 1e-6 else 0.0
+        omega = self._accum_dtheta / dt if dt > 1e-6 else 0.0
+        self._accum_d      = 0.0
+        self._accum_dtheta = 0.0
+
+        qx, qy, qz, qw = quat_from_yaw(self._theta)
+        stamp = now.to_msg()
+
+        # ── Odometry message ──────────────────────────────────────────────────
+        odom = Odometry()
+        odom.header.stamp    = stamp
+        odom.header.frame_id = self._odom_fid
+        odom.child_frame_id  = self._base_fid
+
+        odom.pose.pose.position.x  = self._x
+        odom.pose.pose.position.y  = self._y
+        odom.pose.pose.position.z  = 0.0
+        odom.pose.pose.orientation.x = qx
+        odom.pose.pose.orientation.y = qy
+        odom.pose.pose.orientation.z = qz
+        odom.pose.pose.orientation.w = qw
+        odom.pose.covariance = _POSE_COV
+
+        odom.twist.twist.linear.x  = vx
+        odom.twist.twist.angular.z = omega
+        odom.twist.covariance = _TWIST_COV
+
+        self._pub.publish(odom)
+
+        # ── TF broadcast ──────────────────────────────────────────────────────
+        if self._tf_br is not None:
+            tf_msg = TransformStamped()
+            tf_msg.header.stamp    = stamp
+            tf_msg.header.frame_id = self._odom_fid
+            tf_msg.child_frame_id  = self._base_fid
+            tf_msg.transform.translation.x = self._x
+            tf_msg.transform.translation.y = self._y
+            tf_msg.transform.translation.z = 0.0
+            tf_msg.transform.rotation.x = qx
+            tf_msg.transform.rotation.y = qy
+            tf_msg.transform.rotation.z = qz
+            tf_msg.transform.rotation.w = qw
+            self._tf_br.sendTransform(tf_msg)
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = WheelOdomNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_bringup/setup.py

@@ -47,6 +47,8 @@ setup(
             'terrain_roughness       = saltybot_bringup.terrain_rough_node:main',
             # Sky detector for outdoor navigation (Issue #307)
             'sky_detector            = saltybot_bringup.sky_detect_node:main',
+            # Wheel encoder differential drive odometry (Issue #184)
+            'wheel_odom              = saltybot_bringup.wheel_odom_node:main',
         ],
     },
 )

jetson/ros2_ws/src/saltybot_bringup/test/test_wheel_odom.py

@@ -0,0 +1,332 @@
+"""
+test_wheel_odom.py — Unit tests for wheel odometry kinematics (no ROS2 required).
+
+Covers:
+  unwrap_delta:
+    - normal positive delta
+    - normal negative delta
+    - zero delta
+    - positive rollover (counter crosses +2^31 boundary)
+    - negative rollover (counter crosses -2^31 boundary)
+    - near-zero after positive rollover
+    - large forward motion (just under rollover threshold)
+
+  ticks_to_meters:
+    - zero ticks → 0.0 m
+    - one full revolution → 2π * radius
+    - half revolution → π * radius
+    - negative ticks → negative displacement
+    - ticks_per_rev = 0 → 0.0 (guard)
+    - fractional result correctness
+
+  normalize_angle:
+    - 0 → 0
+    - π → π (or −π, both valid; atan2 returns in (−π, π])
+    - 2π → 0
+    - −π → −π (or π)
+    - 3π/2 → −π/2
+
+  integrate_odom — straight line:
+    - equal d_left == d_right → x increases, y unchanged, theta unchanged
+    - moving backward (negative equal displacements) → x decreases
+
+  integrate_odom — rotation:
+    - d_right > d_left → theta increases (left turn)
+    - d_left > d_right → theta decreases (right turn)
+    - spin in place (d_left = −d_right) → x,y unchanged, theta = d_theta
+
+  integrate_odom — circular motion:
+    - four identical quarter-turns return near-original position
+    - heading after 90° left turn ≈ π/2
+
+  integrate_odom — starting from non-zero pose:
+    - displacement is applied in the current heading direction
+
+  quat_from_yaw:
+    - yaw=0 → qw=1, qx=qy=qz=0
+    - yaw=π → qw≈0, qz≈1
+    - yaw=π/2 → qw=qz≈1/√2, qx=qy=0
+    - unit quaternion: qx²+qy²+qz²+qw²=1 for arbitrary yaw
+    - yaw=−π/2 → qz≈−1/√2
+
+  WheelOdomState:
+    - fields accessible by name
+    - immutable (NamedTuple)
+"""
+
+import sys
+import os
+import math
+
+import pytest
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
+
+from saltybot_bringup._wheel_odom import (
+    WheelOdomState,
+    unwrap_delta,
+    ticks_to_meters,
+    normalize_angle,
+    integrate_odom,
+    quat_from_yaw,
+)
+
+# ── Constants ─────────────────────────────────────────────────────────────────
+
+_R    = 0.034    # wheel radius (m)
+_BASE = 0.160    # wheel base (m)
+_TPR  = 1000     # ticks per rev
+
+
+# ── WheelOdomState ────────────────────────────────────────────────────────────
+
+class TestWheelOdomState:
+
+    def test_fields_accessible(self):
+        s = WheelOdomState(x=1.0, y=2.0, theta=0.5)
+        assert s.x     == pytest.approx(1.0)
+        assert s.y     == pytest.approx(2.0)
+        assert s.theta == pytest.approx(0.5)
+
+    def test_is_namedtuple(self):
+        s = WheelOdomState(0.0, 0.0, 0.0)
+        assert isinstance(s, tuple)
+
+
+# ── unwrap_delta ──────────────────────────────────────────────────────────────
+
+class TestUnwrapDelta:
+
+    def test_normal_positive(self):
+        assert unwrap_delta(100, 50) == 50
+
+    def test_normal_negative(self):
+        assert unwrap_delta(50, 100) == -50
+
+    def test_zero(self):
+        assert unwrap_delta(200, 200) == 0
+
+    def test_positive_rollover(self):
+        """Counter wraps from just below +2^31 to just above −2^31."""
+        MAX = 2 ** 31
+        prev    =  MAX - 10
+        current = -MAX + 5   # wrapped: total advance = 15 ticks
+        assert unwrap_delta(current, prev) == 15
+
+    def test_negative_rollover(self):
+        """Counter wraps from just above −2^31 to just below +2^31."""
+        MAX = 2 ** 31
+        prev    = -MAX + 10
+        current =  MAX - 5   # wrapped backward: delta = −15
+        assert unwrap_delta(current, prev) == -15
+
+    def test_large_forward_no_rollover(self):
+        """Delta just under the rollover threshold should not be treated as rollover."""
+        half = 2 ** 30
+        assert unwrap_delta(half - 1, 0) == half - 1
+
+    def test_symmetry(self):
+        assert unwrap_delta(300, 200) == -unwrap_delta(200, 300)
+
+
+# ── ticks_to_meters ───────────────────────────────────────────────────────────
+
+class TestTicksToMeters:
+
+    def test_zero_ticks(self):
+        assert ticks_to_meters(0, _R, _TPR) == pytest.approx(0.0)
+
+    def test_one_full_revolution(self):
+        expected = 2.0 * math.pi * _R
+        assert ticks_to_meters(_TPR, _R, _TPR) == pytest.approx(expected, rel=1e-9)
+
+    def test_half_revolution(self):
+        expected = math.pi * _R
+        assert ticks_to_meters(_TPR // 2, _R, _TPR) == pytest.approx(expected, rel=1e-4)
+
+    def test_negative_ticks(self):
+        d = ticks_to_meters(-_TPR, _R, _TPR)
+        assert d == pytest.approx(-(2.0 * math.pi * _R), rel=1e-9)
+
+    def test_ticks_per_rev_zero_returns_zero(self):
+        assert ticks_to_meters(100, _R, 0) == pytest.approx(0.0)
+
+    def test_proportional(self):
+        d1 = ticks_to_meters(100, _R, _TPR)
+        d2 = ticks_to_meters(200, _R, _TPR)
+        assert d2 == pytest.approx(2 * d1, rel=1e-9)
+
+
+# ── normalize_angle ───────────────────────────────────────────────────────────
+
+class TestNormalizeAngle:
+
+    def test_zero(self):
+        assert normalize_angle(0.0) == pytest.approx(0.0)
+
+    def test_two_pi_to_zero(self):
+        assert normalize_angle(2 * math.pi) == pytest.approx(0.0, abs=1e-10)
+
+    def test_three_half_pi_to_neg_half_pi(self):
+        assert normalize_angle(3 * math.pi / 2) == pytest.approx(-math.pi / 2, rel=1e-9)
+
+    def test_minus_three_half_pi_to_half_pi(self):
+        assert normalize_angle(-3 * math.pi / 2) == pytest.approx(math.pi / 2, rel=1e-9)
+
+    def test_pi_stays_in_range(self):
+        v = normalize_angle(math.pi)
+        assert -math.pi <= v <= math.pi
+
+    def test_large_angle_wraps(self):
+        v = normalize_angle(100 * math.pi + 0.1)
+        assert -math.pi < v <= math.pi
+        assert v == pytest.approx(0.1, rel=1e-6)
+
+
+# ── integrate_odom — straight line ───────────────────────────────────────────
+
+class TestIntegrateOdomStraight:
+
+    def test_straight_forward_x_increases(self):
+        d = 0.1
+        x, y, theta = integrate_odom(0.0, 0.0, 0.0, d, d, _BASE)
+        assert x == pytest.approx(d, rel=1e-9)
+        assert y == pytest.approx(0.0, abs=1e-12)
+        assert theta == pytest.approx(0.0, abs=1e-12)
+
+    def test_straight_backward_x_decreases(self):
+        d = -0.05
+        x, y, theta = integrate_odom(0.0, 0.0, 0.0, d, d, _BASE)
+        assert x == pytest.approx(d, rel=1e-9)
+        assert y == pytest.approx(0.0, abs=1e-12)
+
+    def test_straight_along_y_axis(self):
+        """Starting at theta=π/2, forward motion should increase y."""
+        d = 0.1
+        x, y, theta = integrate_odom(0.0, 0.0, math.pi / 2, d, d, _BASE)
+        assert x == pytest.approx(0.0, abs=1e-10)
+        assert y == pytest.approx(d, rel=1e-9)
+
+    def test_straight_accumulates(self):
+        """Multiple straight steps accumulate correctly."""
+        x, y, theta = 0.0, 0.0, 0.0
+        for _ in range(10):
+            x, y, theta = integrate_odom(x, y, theta, 0.01, 0.01, _BASE)
+        assert x == pytest.approx(0.10, rel=1e-6)
+        assert y == pytest.approx(0.0,  abs=1e-10)
+
+
+# ── integrate_odom — rotation ─────────────────────────────────────────────────
+
+class TestIntegrateOdomRotation:
+
+    def test_right_wheel_faster_turns_left(self):
+        """d_right > d_left → robot turns left → theta increases."""
+        x, y, theta = integrate_odom(0.0, 0.0, 0.0, 0.0, 0.01, _BASE)
+        assert theta > 0.0
+
+    def test_left_wheel_faster_turns_right(self):
+        """d_left > d_right → robot turns right → theta decreases."""
+        x, y, theta = integrate_odom(0.0, 0.0, 0.0, 0.01, 0.0, _BASE)
+        assert theta < 0.0
+
+    def test_spin_in_place_xy_unchanged(self):
+        """d_left = −d_right → pure rotation, position unchanged."""
+        d = 0.01
+        x, y, theta = integrate_odom(0.0, 0.0, 0.0, -d, d, _BASE)
+        assert x == pytest.approx(0.0, abs=1e-10)
+        assert y == pytest.approx(0.0, abs=1e-10)
+
+    def test_spin_in_place_theta_correct(self):
+        """Spinning one radian: d_theta = (d_right − d_left) / base."""
+        d_theta_target = 1.0
+        d = d_theta_target * _BASE / 2.0
+        x, y, theta = integrate_odom(0.0, 0.0, 0.0, -d, d, _BASE)
+        assert theta == pytest.approx(d_theta_target, rel=1e-9)
+
+    def test_90deg_left_turn_heading(self):
+        """Quarter arc left turn → theta ≈ π/2."""
+        # Arc length for 90° left turn on wheel base _BASE, radius R:
+        # r = R + base/2 for outer (right) wheel; r - base/2 for inner (left)
+        arc_radius = 0.5   # arbitrary turn radius
+        d_theta = math.pi / 2
+        d_right = (arc_radius + _BASE / 2) * d_theta
+        d_left  = (arc_radius - _BASE / 2) * d_theta
+        x, y, theta = integrate_odom(0.0, 0.0, 0.0, d_left, d_right, _BASE)
+        assert theta == pytest.approx(math.pi / 2, rel=1e-9)
+
+
+# ── integrate_odom — circular closure ────────────────────────────────────────
+
+class TestIntegrateOdomClosure:
+
+    def test_four_quarter_turns_return_to_origin(self):
+        """
+        Four 90° left arcs (same radius) should close into a full circle and
+        return approximately to the origin.
+        """
+        arc_r   = 0.30        # turning radius (m)
+        d_theta = math.pi / 2  # 90° per arc
+
+        d_right = (arc_r + _BASE / 2) * d_theta
+        d_left  = (arc_r - _BASE / 2) * d_theta
+
+        x, y, theta = 0.0, 0.0, 0.0
+        for _ in range(4):
+            x, y, theta = integrate_odom(x, y, theta, d_left, d_right, _BASE)
+
+        assert x     == pytest.approx(0.0, abs=1e-9)
+        assert y     == pytest.approx(0.0, abs=1e-9)
+        assert theta == pytest.approx(0.0, abs=1e-9)
+
+    def test_full_spin_in_place(self):
+        """Spinning 2π in small steps should return theta to 0."""
+        d_theta_step = math.pi / 18  # 10° per step
+        d            = d_theta_step * _BASE / 2.0
+        x, y, theta  = 0.0, 0.0, 0.0
+        for _ in range(36):          # 36 × 10° = 360°
+            x, y, theta = integrate_odom(x, y, theta, -d, d, _BASE)
+        assert theta == pytest.approx(0.0, abs=1e-9)
+
+
+# ── quat_from_yaw ─────────────────────────────────────────────────────────────
+
+class TestQuatFromYaw:
+
+    def test_zero_yaw(self):
+        qx, qy, qz, qw = quat_from_yaw(0.0)
+        assert qx == pytest.approx(0.0, abs=1e-12)
+        assert qy == pytest.approx(0.0, abs=1e-12)
+        assert qz == pytest.approx(0.0, abs=1e-12)
+        assert qw == pytest.approx(1.0)
+
+    def test_pi_yaw(self):
+        qx, qy, qz, qw = quat_from_yaw(math.pi)
+        assert qx == pytest.approx(0.0, abs=1e-10)
+        assert qy == pytest.approx(0.0, abs=1e-10)
+        assert abs(qz) == pytest.approx(1.0, rel=1e-9)
+        assert abs(qw) == pytest.approx(0.0, abs=1e-10)
+
+    def test_half_pi_yaw(self):
+        qx, qy, qz, qw = quat_from_yaw(math.pi / 2)
+        s = 1.0 / math.sqrt(2)
+        assert qx == pytest.approx(0.0, abs=1e-12)
+        assert qy == pytest.approx(0.0, abs=1e-12)
+        assert qz == pytest.approx(s, rel=1e-9)
+        assert qw == pytest.approx(s, rel=1e-9)
+
+    def test_neg_half_pi_yaw(self):
+        qx, qy, qz, qw = quat_from_yaw(-math.pi / 2)
+        s = 1.0 / math.sqrt(2)
+        assert qz == pytest.approx(-s, rel=1e-9)
+        assert qw == pytest.approx(s,  rel=1e-9)
+
+    @pytest.mark.parametrize('yaw', [0.0, 0.1, 0.5, math.pi, -math.pi / 3, 2.7])
+    def test_unit_quaternion(self, yaw):
+        qx, qy, qz, qw = quat_from_yaw(yaw)
+        norm_sq = qx**2 + qy**2 + qz**2 + qw**2
+        assert norm_sq == pytest.approx(1.0, rel=1e-9)
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])