seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
sl-jetson 2026-03-03 00:42:27 -05:00
commit ffc69a05c0
8 changed files with 757 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
jetson/ros2_ws/src/saltybot_bridge_msgs/CMakeLists.txt Normal file
View File

@ -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()

11
jetson/ros2_ws/src/saltybot_bridge_msgs/msg/WheelTicks.msg Normal file
View File

@ -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

23
jetson/ros2_ws/src/saltybot_bridge_msgs/package.xml Normal file
View File

@ -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>

3
jetson/ros2_ws/src/saltybot_bringup/package.xml
View File

@ -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>

161
jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_wheel_odom.py Normal file
View File

@ -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))

210
jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/wheel_odom_node.py Normal file
View File

@ -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()

2
jetson/ros2_ws/src/saltybot_bringup/setup.py
View File

@ -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',
],
},
)

332
jetson/ros2_ws/src/saltybot_bringup/test/test_wheel_odom.py Normal file
View File

@ -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'])