| """Unit tests for the LDS-02 adapter (no ROS): python3 tests/test_adapter.py""" |
| import math |
| import os |
| import sys |
|
|
| import numpy as np |
|
|
| sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) |
| from lds02 import ScanCleaner, lds02_corrupt, sample_params |
| from lds02_adapter import FRONT_IDX, RAY_ANGLES, LidarAdapter, bin_scan |
|
|
|
|
| def scene(angles): |
| """Ranges seen from the origin: wall at 2 m everywhere, a 0.5 m object straight ahead (+-10 deg), one at 1 m on |
| the left (80..100 deg).""" |
| a = np.mod(angles, 2 * math.pi) |
| r = np.full(len(a), 2.0) |
| r[(a < math.radians(10)) | (a > math.radians(350))] = 0.5 |
| r[(a > math.radians(80)) & (a < math.radians(100))] = 1.0 |
| return r |
|
|
|
|
| def test_layouts(): |
| for n, amin, sign in ((230, 0.0, 1), (360, 0.0, 1), (460, 0.0, 1), (360, -math.pi, 1), (360, 0.0, -1), (455, 1.0, -1)): |
| inc = sign * 2 * math.pi / n |
| ang = amin + np.arange(n) * inc |
| out, seen = bin_scan(scene(ang), amin, inc) |
| ref = scene(RAY_ANGLES) |
| ok = np.mean(np.abs(out - ref) < 1e-6) |
| assert ok > 0.97, (n, amin, sign, ok) |
| assert out[0] == 0.5 and abs(out[90] - 1.0) < 1e-6 and out[180] == 2.0, (n, amin, sign) |
| print('layouts ok (230 / 360 / 460 points, offset start, clockwise)') |
|
|
|
|
| def test_invalid_and_hold(): |
| c = ScanCleaner() |
| s1 = np.full(360, 2.0); s1[40:45] = 0.25 |
| s2 = np.full(360, 2.0); s2[40:45] = 0.0; s2[300] = np.inf |
| c(s1, 0.0) |
| o, held = c(s2, 0.2) |
| assert abs(o[42] - 0.25) < 1e-6 and o[300] == 3.5 and held == 5 |
| o, _ = c(np.zeros(360), 1.0) |
| assert (o == 3.5).all() |
| print('invalid readings / near-field hold ok') |
|
|
|
|
| def test_sim_equivalence(): |
| """The simulator path (Gazebo 360 rays -> lds02_corrupt -> ScanCleaner, front beams by FRONT_GRID) and the robot |
| path (LaserScan -> bin_scan -> ScanCleaner) give identical frames for the same raw readings.""" |
| rng = np.random.default_rng(0) |
| p = sample_params(rng) |
| ad, cl = LidarAdapter(), ScanCleaner() |
| for k in range(12): |
| raw = lds02_corrupt(scene(RAY_ANGLES) - 0.05 * k, rng, p) |
| ad.push(raw, 0.0, 6.28 / 359, 0.2 * k) |
| sim, _ = cl(raw, 0.2 * k) |
| assert np.array_equal(ad.frames[-1], sim) |
| assert np.array_equal(ad.front_beams(0), sim[FRONT_IDX]) |
| x, v = ad.window() |
| assert x.shape == (20, 360) and v.sum() == 12 and x.min() >= 0.12 and x.max() <= 3.5 |
| print('sim / robot pipelines identical') |
|
|
|
|
| def test_timing(): |
| ad = LidarAdapter() |
| added = [ad.push(np.full(360, 2.0), 0.0, 6.28 / 359, t) for t in np.arange(0, 2.0, 0.1)] |
| assert sum(added) == 10 |
| ad = LidarAdapter() |
| added = [ad.push(np.full(360, 2.0), 0.0, 6.28 / 359, t) for t in np.arange(0, 2.0, 1 / 5.8)] |
| assert all(added) |
| print('frame timing ok (10 Hz -> every other scan, 5.8 Hz -> every scan)') |
|
|
|
|
| if __name__ == '__main__': |
| test_layouts() |
| test_invalid_and_hold() |
| test_sim_equivalence() |
| test_timing() |
| print('ALL TESTS PASSED') |
|
|
