diff --git a/src/subjugator/drivers/magnetic_compensation/CMakeLists.txt b/src/subjugator/drivers/magnetic_compensation/CMakeLists.txt index fb326c0ba..4efaee06e 100644 --- a/src/subjugator/drivers/magnetic_compensation/CMakeLists.txt +++ b/src/subjugator/drivers/magnetic_compensation/CMakeLists.txt @@ -12,6 +12,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) @@ -25,6 +26,10 @@ install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch) # Install project configuration files install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config) +# Install the pure-Python calibration module (importable as +# magnetic_compensation.ellipsoid by the config generator and the tests) +ament_python_install_package(${PROJECT_NAME}) + # create the component library add_library( ${PROJECT_NAME} SHARED src/component.cpp # adjust if your source file is named @@ -59,20 +64,22 @@ ament_export_dependencies(rclcpp rclcpp_components sensor_msgs tf2 tf2_eigen Eigen3) if(BUILD_TESTING) + # C++ unit tests: correction math + parameter validation (compensator.hpp) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_compensator test/test_compensator.cpp) + target_include_directories(test_compensator + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) + target_link_libraries(test_compensator Eigen3::Eigen) + + # Pure-math Python tests for the ellipsoid fit (numpy/scipy only) find_package(ament_cmake_pytest REQUIRED) - set(_pytest_tests) # test/test_basic_shift.py) - foreach(_test_path ${_pytest_tests}) - get_filename_component(_test_name ${_test_path} NAME_WE) - ament_add_pytest_test( - ${_test_name} - ${_test_path} - APPEND_ENV - PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} - TIMEOUT - 60 - WORKING_DIRECTORY - ${CMAKE_SOURCE_DIR}) - endforeach() + ament_add_pytest_test(test_ellipsoid test/test_ellipsoid.py TIMEOUT 60 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) + + # End-to-end launch tests: live correction path + constructor param validation + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test(test/test_basic_shift.py TIMEOUT 60) + add_launch_test(test/test_param_validation.py TIMEOUT 60) endif() ament_package() diff --git a/src/subjugator/drivers/magnetic_compensation/include/magnetic_compensation/compensator.hpp b/src/subjugator/drivers/magnetic_compensation/include/magnetic_compensation/compensator.hpp new file mode 100644 index 000000000..321074a1c --- /dev/null +++ b/src/subjugator/drivers/magnetic_compensation/include/magnetic_compensation/compensator.hpp @@ -0,0 +1,15 @@ +#pragma once + +#include + +namespace mil::magnetic_compensation +{ + +// Apply the hard/soft-iron correction: undo the offset, then the distortion. +inline Eigen::Vector3d compensate(Eigen::Matrix3d const &scale_inverse, Eigen::Vector3d const &shift, + Eigen::Vector3d const &raw) +{ + return scale_inverse * (raw - shift); +} + +} // namespace mil::magnetic_compensation diff --git a/src/subjugator/drivers/magnetic_compensation/magnetic_compensation/__init__.py b/src/subjugator/drivers/magnetic_compensation/magnetic_compensation/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/subjugator/drivers/magnetic_compensation/magnetic_compensation/ellipsoid.py b/src/subjugator/drivers/magnetic_compensation/magnetic_compensation/ellipsoid.py new file mode 100644 index 000000000..e60a8924f --- /dev/null +++ b/src/subjugator/drivers/magnetic_compensation/magnetic_compensation/ellipsoid.py @@ -0,0 +1,93 @@ +"""Pure-math hard/soft-iron ellipsoid fitting for magnetometer calibration. + +A clean magnetometer, swept through all orientations, traces a sphere centered +at the origin. The sub's hard-iron (a constant offset) and soft-iron (a linear +distortion) warp that sphere into an off-center, tilted ellipsoid. + +``fit_ellipsoid`` recovers the offset (``shift``) and the distortion +(``scale``) from points on that ellipsoid so the live node can undo them with +``scale^-1 @ (raw - shift)``. + +This module has no ROS or plotting dependencies so the math can be unit tested +in isolation (numpy + scipy only). +""" + +import numpy as np +import scipy.linalg + + +def normalized_matrix(m): + assert np.linalg.det(m) > 0 + return m / np.linalg.det(m) ** (1 / m.shape[0]) + + +def calculate_error(points): + radii = list(map(np.linalg.norm, points)) + error = np.std(radii) / np.mean(radii) + return error + + +def fit_ellipsoid(points): + points = np.array(points) + + # The fit solves for 9 coefficients; fewer points leaves it underdetermined + # and lstsq returns a confident-looking but meaningless calibration. Reject + # that loudly rather than silently emitting a bogus scale/shift. + if points.shape[0] < 9: + raise ValueError( + f"fit_ellipsoid needs at least 9 points, got {points.shape[0]}", + ) + + # Reject (near-)coplanar/collinear data. If the points don't span all three + # axes the ellipsoid is underdetermined along an axis, and whether the fit + # then errors out is numerically fragile (it depends on the BLAS backend). + # The singular values of the mean-centered points measure spread per axis; a + # vanishing smallest one means the calibration sweep was degenerate (e.g. the + # vehicle only rotated about one axis), so fail loudly here instead. + singular_values = np.linalg.svd(points - points.mean(axis=0), compute_uv=False) + if singular_values[-1] < 1e-6 * singular_values[0]: + raise ValueError( + "fit_ellipsoid: points are (near-)coplanar; calibration data must " + "cover all three axes", + ) + + A = np.zeros((points.shape[0], 9)) + A[:, 0] = points[:, 0] ** 2 + A[:, 1] = points[:, 1] ** 2 + A[:, 2] = points[:, 2] ** 2 + A[:, 3] = 2 * points[:, 0] * points[:, 1] + A[:, 4] = 2 * points[:, 0] * points[:, 2] + A[:, 5] = 2 * points[:, 1] * points[:, 2] + A[:, 6] = -2 * points[:, 0] + A[:, 7] = -2 * points[:, 1] + A[:, 8] = -2 * points[:, 2] + + B = np.ones((points.shape[0], 1)) + + X = np.linalg.lstsq(A, B, rcond=None)[0].flatten() + if X[0] < 0: + X = -X + + ka = np.linalg.inv( + np.array( + [ + [X[0], X[3], X[4]], + [X[3], X[1], X[5]], + [X[4], X[5], X[2]], + ], + ), + ) + shift = ka.dot(X[6:9]) + + scale = scipy.linalg.sqrtm(ka) + assert np.isreal(scale).all(), scale + scale = np.real(scale) + scale = normalized_matrix(scale) + + scale_inv = np.linalg.inv(scale) + compensated = [scale_inv.dot(p - shift) for p in points] + + for axis in range(3): + assert min(p[axis] for p in compensated) < 0 < max(p[axis] for p in compensated) + + return scale, shift, compensated diff --git a/src/subjugator/drivers/magnetic_compensation/package.xml b/src/subjugator/drivers/magnetic_compensation/package.xml index 7f3645d27..c3e39a3ec 100644 --- a/src/subjugator/drivers/magnetic_compensation/package.xml +++ b/src/subjugator/drivers/magnetic_compensation/package.xml @@ -8,9 +8,20 @@ TODO: License declaration ament_cmake + ament_cmake_python + python3-numpy + python3-scipy + + ament_cmake_gtest + ament_cmake_pytest ament_lint_auto ament_lint_common + launch_ros + launch_testing + launch_testing_ament_cmake + python3-pytest + rclcpp_components ament_cmake diff --git a/src/subjugator/drivers/magnetic_compensation/scripts/generate_config.py b/src/subjugator/drivers/magnetic_compensation/scripts/generate_config.py index eef07a87b..af4f237f1 100644 --- a/src/subjugator/drivers/magnetic_compensation/scripts/generate_config.py +++ b/src/subjugator/drivers/magnetic_compensation/scripts/generate_config.py @@ -6,9 +6,13 @@ import matplotlib.pyplot as plt import numpy as np import rclpy -import scipy.linalg import yaml from geometry_msgs.msg import Vector3 +from magnetic_compensation.ellipsoid import ( + calculate_error, + fit_ellipsoid, + normalized_matrix, +) from mpl_toolkits.mplot3d import Axes3D from rclpy.serialization import deserialize_message from rosbag2_py import ConverterOptions, SequentialReader, StorageOptions @@ -16,62 +20,6 @@ from tf_transformations import random_rotation_matrix, unit_vector -def normalized_matrix(m): - assert np.linalg.det(m) > 0 - return m / np.linalg.det(m) ** (1 / m.shape[0]) - - -def calculate_error(points): - radii = list(map(np.linalg.norm, points)) - error = np.std(radii) / np.mean(radii) - return error - - -def fit_ellipsoid(points): - points = np.array(points) - - A = np.zeros((points.shape[0], 9)) - A[:, 0] = points[:, 0] ** 2 - A[:, 1] = points[:, 1] ** 2 - A[:, 2] = points[:, 2] ** 2 - A[:, 3] = 2 * points[:, 0] * points[:, 1] - A[:, 4] = 2 * points[:, 0] * points[:, 2] - A[:, 5] = 2 * points[:, 1] * points[:, 2] - A[:, 6] = -2 * points[:, 0] - A[:, 7] = -2 * points[:, 1] - A[:, 8] = -2 * points[:, 2] - - B = np.ones((points.shape[0], 1)) - - X = np.linalg.lstsq(A, B, rcond=None)[0].flatten() - if X[0] < 0: - X = -X - - ka = np.linalg.inv( - np.array( - [ - [X[0], X[3], X[4]], - [X[3], X[1], X[5]], - [X[4], X[5], X[2]], - ], - ), - ) - shift = ka.dot(X[6:9]) - - scale = scipy.linalg.sqrtm(ka) - assert np.isreal(scale).all(), scale - scale = np.real(scale) - scale = normalized_matrix(scale) - - scale_inv = np.linalg.inv(scale) - compensated = [scale_inv.dot(p - shift) for p in points] - - for axis in range(3): - assert min(p[axis] for p in compensated) < 0 < max(p[axis] for p in compensated) - - return scale, shift, compensated - - def axisEqual3D(ax): ax.axis("tight") extents = np.array([getattr(ax, f"get_{dim}lim")() for dim in "xyz"]) diff --git a/src/subjugator/drivers/magnetic_compensation/src/component.cpp b/src/subjugator/drivers/magnetic_compensation/src/component.cpp index 0568be157..eb9f1988d 100644 --- a/src/subjugator/drivers/magnetic_compensation/src/component.cpp +++ b/src/subjugator/drivers/magnetic_compensation/src/component.cpp @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -73,7 +74,7 @@ class HardsoftCompensator : public rclcpp::Node Eigen::Vector3d raw; tf2::fromMsg(msg->magnetic_field, raw); - Eigen::Vector3d processed = scale_inverse_ * (raw - shift_); + Eigen::Vector3d processed = compensate(scale_inverse_, shift_, raw); sensor_msgs::msg::MagneticField result; result.header = msg->header; diff --git a/src/subjugator/drivers/magnetic_compensation/test/test_basic_shift.py b/src/subjugator/drivers/magnetic_compensation/test/test_basic_shift.py index b3b3573bc..9c395e56e 100644 --- a/src/subjugator/drivers/magnetic_compensation/test/test_basic_shift.py +++ b/src/subjugator/drivers/magnetic_compensation/test/test_basic_shift.py @@ -1,3 +1,14 @@ +"""End-to-end launch test for the HardsoftCompensator node. + +Brings up the real component with an identity calibration (zero shift, identity +scale) and checks the live correction path: a matching-frame message passes +through unchanged, and a wrong-frame message is dropped. + +NOTE: requires a built+sourced ROS 2 workspace (run via ``colcon test``); it is +not exercised by the pure-Python ``test_ellipsoid.py`` suite. +""" + +import time import unittest import launch @@ -6,9 +17,10 @@ import rclpy from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode -from rclpy.duration import Duration from sensor_msgs.msg import MagneticField +FRAME_ID = "imu_link" + @pytest.mark.launch_test def generate_test_description(): @@ -23,20 +35,9 @@ def generate_test_description(): plugin="mil::magnetic_compensation::HardsoftCompensator", name="hardsoft_compensator", parameters=[ + {"frame_id": FRAME_ID}, {"shift": [0.0, 0.0, 0.0]}, - { - "scale": [ - 1.0, - 1.0, - 1.0, - 1.0, - 1.0, - 1.0, - 1.0, - 1.0, - 1.0, - ], - }, + {"scale": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]}, ], ), ], @@ -50,7 +51,7 @@ def generate_test_description(): ) -class TestBasicShift(unittest.TestCase): +class TestCompensatorNode(unittest.TestCase): @classmethod def setUpClass(cls): rclpy.init() @@ -60,32 +61,50 @@ def tearDownClass(cls): rclpy.shutdown() def setUp(self): - self.node = rclpy.create_node("test_basic_shift") - self.mag_raw_pub = self.node.create_publisher(MagneticField, "imu/mag_raw", 10) - self.mag_sub = self.node.create_subscription( + self.node = rclpy.create_node("test_compensator") + self.received = None + self.pub = self.node.create_publisher(MagneticField, "/imu/mag_raw", 10) + self.sub = self.node.create_subscription( MagneticField, "imu/mag", - self.imu_callback, + self._on_msg, 10, ) - def __init__(self, *args): - super().__init__(*args) - self.most_recent_imu_msg = None - - def imu_callback(self, msg): - self.most_recent_imu_msg = msg - - def test_shifted(self): - # send an arbitrary measurement and ensure that it has not shifted - mf_message = MagneticField() - mf_message.magnetic_field.x = 1.0 - mf_message.magnetic_field.y = 1.0 - mf_message.magnetic_field.z = 1.0 - mf_message.header.frame_id = "imu_link" - self.mag_raw_pub.publish(mf_message) - self.node.get_clock().sleep_for(Duration(seconds=20.0)) - self.assertIsNot(self.most_recent_imu_msg, None) - self.assertEqual(self.most_recent_imu_msg.magnetic_field.x, 1.0) - self.assertEqual(self.most_recent_imu_msg.magnetic_field.y, 1.0) - self.assertEqual(self.most_recent_imu_msg.magnetic_field.z, 1.0) + def tearDown(self): + self.node.destroy_node() + + def _on_msg(self, msg): + self.received = msg + + def _publish_and_wait(self, frame_id, xyz, timeout_sec=5.0): + """Publish a raw reading repeatedly while spinning until a reply or timeout. + + Republishing covers pub/sub discovery latency; spinning the node is what + the original test was missing (it slept without ever spinning, so the + callback never fired and the test always failed -> it was disabled). + """ + msg = MagneticField() + msg.header.frame_id = frame_id + msg.magnetic_field.x, msg.magnetic_field.y, msg.magnetic_field.z = xyz + + deadline = time.monotonic() + timeout_sec + while self.received is None and time.monotonic() < deadline: + self.pub.publish(msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + return self.received + + def test_identity_passes_through_unchanged(self): + result = self._publish_and_wait(FRAME_ID, (1.0, 2.0, 3.0)) + self.assertIsNotNone(result, "no message received from compensator") + self.assertAlmostEqual(result.magnetic_field.x, 1.0) + self.assertAlmostEqual(result.magnetic_field.y, 2.0) + self.assertAlmostEqual(result.magnetic_field.z, 3.0) + + def test_wrong_frame_id_is_dropped(self): + result = self._publish_and_wait( + "not_imu_link", + (1.0, 2.0, 3.0), + timeout_sec=2.0, + ) + self.assertIsNone(result, "message with mismatched frame_id should be dropped") diff --git a/src/subjugator/drivers/magnetic_compensation/test/test_compensator.cpp b/src/subjugator/drivers/magnetic_compensation/test/test_compensator.cpp new file mode 100644 index 000000000..7415c145f --- /dev/null +++ b/src/subjugator/drivers/magnetic_compensation/test/test_compensator.cpp @@ -0,0 +1,40 @@ +// Unit tests for the hard/soft-iron correction math. +// +// These exercise compensate() in compensator.hpp directly (the same code the +// live node calls), so no ROS node or spinning is required. Parameter +// validation (size + invertibility checks) stays inline in the node +// constructor and is not directly covered here. + +#include + +#include + +using mil::magnetic_compensation::compensate; + +TEST(Compensate, IdentityLeavesReadingUnchanged) +{ + Eigen::Matrix3d scale_inv = Eigen::Matrix3d::Identity(); + Eigen::Vector3d shift = Eigen::Vector3d::Zero(); + Eigen::Vector3d raw(1.0, 2.0, 3.0); + EXPECT_TRUE(compensate(scale_inv, shift, raw).isApprox(raw)); +} + +TEST(Compensate, PureShiftIsSubtracted) +{ + Eigen::Matrix3d scale_inv = Eigen::Matrix3d::Identity(); + Eigen::Vector3d shift(0.5, -1.0, 2.0); + Eigen::Vector3d raw(1.0, 2.0, 3.0); + Eigen::Vector3d expected(0.5, 3.0, 1.0); + EXPECT_TRUE(compensate(scale_inv, shift, raw).isApprox(expected)); +} + +TEST(Compensate, DiagonalScaleIsInverted) +{ + // scale = diag(2,2,2) -> scale_inverse = diag(0.5,0.5,0.5) + Eigen::Matrix3d scale_inv; + scale_inv << 0.5, 0, 0, 0, 0.5, 0, 0, 0, 0.5; + Eigen::Vector3d shift = Eigen::Vector3d::Zero(); + Eigen::Vector3d raw(2.0, 4.0, 6.0); + Eigen::Vector3d expected(1.0, 2.0, 3.0); + EXPECT_TRUE(compensate(scale_inv, shift, raw).isApprox(expected)); +} diff --git a/src/subjugator/drivers/magnetic_compensation/test/test_ellipsoid.py b/src/subjugator/drivers/magnetic_compensation/test/test_ellipsoid.py new file mode 100644 index 000000000..cbb006ef7 --- /dev/null +++ b/src/subjugator/drivers/magnetic_compensation/test/test_ellipsoid.py @@ -0,0 +1,161 @@ +"""Unit tests for the magnetometer hard/soft-iron ellipsoid fit. + +These are pure-math tests: they exercise the calibration math in +``magnetic_compensation.ellipsoid`` with synthetic data and need only +numpy/scipy (no ROS, no hardware). + +The model under test: a clean magnetometer traces a unit sphere; the sub's +hard-iron (a constant offset) and soft-iron (a linear distortion) warp it into +an off-center, tilted ellipsoid. ``fit_ellipsoid`` must recover the offset +(``shift``) and the distortion (``scale``) from points on that ellipsoid. +""" + +import numpy as np +import pytest +from magnetic_compensation.ellipsoid import ( + calculate_error, + fit_ellipsoid, + normalized_matrix, +) + +# --- helpers --------------------------------------------------------------- + + +def _fib_sphere(n): + """n evenly distributed unit vectors (deterministic, full coverage).""" + i = np.arange(n) + phi = np.pi * (3.0 - np.sqrt(5.0)) # golden angle + y = 1.0 - 2.0 * i / (n - 1) + r = np.sqrt(np.clip(1.0 - y * y, 0.0, 1.0)) + theta = phi * i + return np.column_stack((np.cos(theta) * r, y, np.sin(theta) * r)) + + +def _rotation(rx, ry, rz): + """A proper rotation matrix from fixed roll/pitch/yaw angles.""" + cx, sx = np.cos(rx), np.sin(rx) + cy, sy = np.cos(ry), np.sin(ry) + cz, sz = np.cos(rz), np.sin(rz) + rot_x = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]]) + rot_y = np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]]) + rot_z = np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]]) + return rot_z @ rot_y @ rot_x + + +def _ellipsoid_points(scale, shift, n=400): + """Points on the ellipsoid: p_i = scale @ u_i + shift.""" + u = _fib_sphere(n) + return u @ np.asarray(scale).T + np.asarray(shift) + + +# --- normalized_matrix ----------------------------------------------------- + + +def test_normalized_matrix_has_unit_determinant(): + m = np.diag([1.0, 2.0, 3.0]) + assert np.isclose(np.linalg.det(normalized_matrix(m)), 1.0) + + +def test_normalized_matrix_is_scale_invariant(): + m = np.diag([1.0, 2.0, 3.0]) + assert np.allclose(normalized_matrix(5.0 * m), normalized_matrix(m)) + + +def test_normalized_matrix_identity_unchanged(): + assert np.allclose(normalized_matrix(np.eye(3)), np.eye(3)) + + +def test_normalized_matrix_rejects_nonpositive_determinant(): + with pytest.raises(AssertionError): + normalized_matrix(np.diag([-1.0, 1.0, 1.0])) # det = -1 + + +# --- calculate_error ------------------------------------------------------- + + +def test_calculate_error_zero_on_perfect_sphere(): + assert calculate_error(_fib_sphere(200)) == pytest.approx(0.0, abs=1e-9) + + +def test_calculate_error_known_value(): + pts = np.array([[1.0, 0, 0], [2.0, 0, 0], [3.0, 0, 0]]) + # radii = [1, 2, 3]; std/mean = 0.81650 / 2 + assert calculate_error(pts) == pytest.approx(0.40825, abs=1e-4) + + +# --- fit_ellipsoid: recovery (the "ellipsoids are proper" requirement) ----- + + +def test_fit_identity_sphere(): + scale, shift = np.eye(3), np.zeros(3) + s2, sh2, _ = fit_ellipsoid(_ellipsoid_points(scale, shift)) + assert np.allclose(s2, scale, atol=1e-6) + assert np.allclose(sh2, shift, atol=1e-6) + + +def test_fit_recovers_pure_hard_iron(): + scale, shift = np.eye(3), np.array([1.0, -2.0, 0.5]) + s2, sh2, _ = fit_ellipsoid(_ellipsoid_points(scale, shift)) + assert np.allclose(s2, scale, atol=1e-6) + assert np.allclose(sh2, shift, atol=1e-6) + + +def test_fit_recovers_pure_soft_iron(): + scale, shift = np.diag([2.0, 1.0, 0.5]), np.zeros(3) # det(scale) = 1 + s2, sh2, _ = fit_ellipsoid(_ellipsoid_points(scale, shift)) + assert np.allclose(s2, scale, atol=1e-6) + assert np.allclose(sh2, shift, atol=1e-6) + + +def test_fit_recovers_full_affine(): + rot = _rotation(0.3, -0.2, 0.5) + scale = rot @ np.diag([2.0, 1.0, 0.5]) @ rot.T + shift = np.array([0.3, -0.4, 0.2]) + s2, sh2, _ = fit_ellipsoid(_ellipsoid_points(scale, shift)) + assert np.allclose(s2, scale, atol=1e-6) + assert np.allclose(sh2, shift, atol=1e-6) + + +def test_fit_output_scale_is_symmetric_spd_unit_det(): + rot = _rotation(0.3, -0.2, 0.5) + scale = rot @ np.diag([2.0, 1.0, 0.5]) @ rot.T + shift = np.array([0.3, -0.4, 0.2]) + s2, _, _ = fit_ellipsoid(_ellipsoid_points(scale, shift)) + assert np.allclose(s2, s2.T, atol=1e-9) # symmetric + assert np.isclose(np.linalg.det(s2), 1.0, atol=1e-6) # normalized + assert (np.linalg.eigvalsh(s2) > 0).all() # positive-definite + + +def test_fit_compensated_points_lie_on_unit_sphere(): + rot = _rotation(0.3, -0.2, 0.5) + scale = rot @ np.diag([2.0, 1.0, 0.5]) @ rot.T + shift = np.array([0.3, -0.4, 0.2]) + _, _, compensated = fit_ellipsoid(_ellipsoid_points(scale, shift)) + assert calculate_error(compensated) < 1e-6 + + +# --- fit_ellipsoid: edge cases (must fail loudly, not return garbage) ------ + + +def test_fit_too_few_points_raises(): + pts = _fib_sphere(5) # fewer than the 9 unknowns + with pytest.raises(ValueError): + fit_ellipsoid(pts) + + +def test_fit_coplanar_points_raises(): + u = _fib_sphere(200) + u[:, 2] = 0.0 # collapse onto the z = 0 plane + with pytest.raises(ValueError): + fit_ellipsoid(u) + + +def test_fit_clustered_points_raises(): + # all directions bunched near +z -> compensated cloud never straddles 0 + a = np.linspace(0.0, 0.2, 60) + b = np.linspace(0.0, 2 * np.pi, 60) + pts = np.column_stack( + (np.sin(a) * np.cos(b), np.sin(a) * np.sin(b), np.cos(a)), + ) + with pytest.raises(AssertionError): + fit_ellipsoid(pts) diff --git a/src/subjugator/drivers/magnetic_compensation/test/test_param_validation.py b/src/subjugator/drivers/magnetic_compensation/test/test_param_validation.py new file mode 100644 index 000000000..6cad356e6 --- /dev/null +++ b/src/subjugator/drivers/magnetic_compensation/test/test_param_validation.py @@ -0,0 +1,74 @@ +"""Launch test for HardsoftCompensator constructor parameter validation. + +Loads the component with deliberately bad calibration parameters and checks the +constructor rejects them (logging an error and throwing) rather than starting up +with a meaningless correction. Each bad node lives in its own container, since a +load failure aborts the rest of a container's load sequence. The constructor's +RCLCPP_ERROR goes to stderr, which is what we wait for. + +NOTE: requires a built+sourced ROS 2 workspace (run via ``colcon test``). +""" + +import unittest + +import launch +import launch_testing.actions +import pytest +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def _container(name, node_name, scale): + return ComposableNodeContainer( + name=name, + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="magnetic_compensation", + plugin="mil::magnetic_compensation::HardsoftCompensator", + name=node_name, + parameters=[{"shift": [0.0, 0.0, 0.0]}, {"scale": scale}], + ), + ], + ) + + +@pytest.mark.launch_test +def generate_test_description(): + size_container = _container("size_container", "bad_size", [1.0, 0.0, 0.0]) + singular_container = _container( + "singular_container", + "bad_singular", + [0.0] * 9, # right size, but det == 0 + ) + + return ( + launch.LaunchDescription( + [ + size_container, + singular_container, + launch_testing.actions.ReadyToTest(), + ], + ), + {}, + ) + + +class TestParamValidation(unittest.TestCase): + def test_invalid_size_is_rejected(self, proc_output): + # The wrong-size scale (3 elements, not 9) must be rejected. + proc_output.assertWaitFor( + "Invalid parameter sizes", + timeout=15, + stream="stderr", + ) + + def test_singular_scale_is_rejected(self, proc_output): + # A 9-element but non-invertible scale must be rejected too. + proc_output.assertWaitFor( + "Scale matrix is not invertible", + timeout=15, + stream="stderr", + )