diff --git a/jetson/ros2_ws/src/saltybot_bridge_msgs/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_bridge_msgs/CMakeLists.txt
new file mode 100644
index 0000000..e9d57e9
--- /dev/null
+++ b/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()
diff --git a/jetson/ros2_ws/src/saltybot_bridge_msgs/msg/WheelTicks.msg b/jetson/ros2_ws/src/saltybot_bridge_msgs/msg/WheelTicks.msg
new file mode 100644
index 0000000..301aaa1
--- /dev/null
+++ b/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
diff --git a/jetson/ros2_ws/src/saltybot_bridge_msgs/package.xml b/jetson/ros2_ws/src/saltybot_bridge_msgs/package.xml
new file mode 100644
index 0000000..7d7ed20
--- /dev/null
+++ b/jetson/ros2_ws/src/saltybot_bridge_msgs/package.xml
@@ -0,0 +1,23 @@
+
+
+
+ saltybot_bridge_msgs
+ 0.1.0
+ STM32 bridge message definitions — wheel encoder ticks and low-level hardware telemetry (Issue #184)
+ sl-perception
+ MIT
+
+ ament_cmake
+ rosidl_default_generators
+
+ std_msgs
+ builtin_interfaces
+
+ rosidl_default_runtime
+
+ rosidl_interface_packages
+
+
+ ament_cmake
+
+
diff --git a/jetson/ros2_ws/src/saltybot_bringup/package.xml b/jetson/ros2_ws/src/saltybot_bringup/package.xml
index 3decbdc..06c4831 100644
--- a/jetson/ros2_ws/src/saltybot_bringup/package.xml
+++ b/jetson/ros2_ws/src/saltybot_bringup/package.xml
@@ -27,6 +27,9 @@
saltybot_perception
saltybot_scene_msgs
+
+ saltybot_bridge_msgs
+ nav_msgs
saltybot_uwb
ament_python
diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_wheel_odom.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_wheel_odom.py
new file mode 100644
index 0000000..839e9da
--- /dev/null
+++ b/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))
diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/wheel_odom_node.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/wheel_odom_node.py
new file mode 100644
index 0000000..6d915f8
--- /dev/null
+++ b/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()
diff --git a/jetson/ros2_ws/src/saltybot_bringup/setup.py b/jetson/ros2_ws/src/saltybot_bringup/setup.py
index b0cc59e..16cbe6b 100644
--- a/jetson/ros2_ws/src/saltybot_bringup/setup.py
+++ b/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',
],
},
)
diff --git a/jetson/ros2_ws/src/saltybot_bringup/test/test_wheel_odom.py b/jetson/ros2_ws/src/saltybot_bringup/test/test_wheel_odom.py
new file mode 100644
index 0000000..341fa2e
--- /dev/null
+++ b/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'])