first commit

utils/tests/visual_track_control/rate_limiter_test.py

@@ -0,0 +1,68 @@
+"""rate_limiter_test.py —— Clamp / SlewRateLimiter 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/rate_limiter_test.cpp。
+覆盖（物理追踪思路 §8、状态机设计 S6）：静态限幅、变化率限幅步进、
+首条指令从零渐变、异常节拍保护、Reset 归零。
+"""
+
+import pytest
+
+from visual_track_control.rate_limiter import SlewRateLimiter, clamp
+
+
+def test_clamp_clamps_value():
+    assert clamp(5.0, -2.0, 2.0) == 2.0
+    assert clamp(-5.0, -2.0, 2.0) == -2.0
+    assert clamp(1.5, -2.0, 2.0) == 1.5
+    # 边界值：落在区间内
+    assert clamp(2.0, -2.0, 2.0) == 2.0
+    assert clamp(-2.0, -2.0, 2.0) == -2.0
+
+
+def test_first_command_ramps_from_zero():
+    # S6：初始输出为 0，首条大指令只能按变化率渐变
+    lim = SlewRateLimiter(2.0)
+    assert lim.limit(10.0, 0.5) == 1.0    # 0 + 2×0.5
+    assert lim.limit(10.0, 0.5) == 2.0    # 继续渐变
+    assert lim.limit(10.0, 10.0) == 10.0  # 步长足够 → 到达目标
+
+
+def test_tracks_target_within_step():
+    lim = SlewRateLimiter(10.0)
+    # 步长 10×1=10，目标 3 在步长内 → 直接到达
+    assert lim.limit(3.0, 1.0) == 3.0
+    assert lim.limit(3.0, 1.0) == 3.0  # 已在目标：保持
+
+
+def test_negative_direction():
+    lim = SlewRateLimiter(2.0)
+    assert lim.limit(-10.0, 0.5) == -1.0
+    assert lim.limit(-10.0, 0.5) == -2.0
+
+
+def test_zero_or_negative_dt_holds():
+    lim = SlewRateLimiter(2.0)
+    lim.limit(10.0, 1.0)  # 输出 2.0
+    assert lim.limit(10.0, 0.0) == 2.0
+    assert lim.limit(10.0, -1.0) == 2.0
+
+
+def test_zero_rate_limit_freezes_output():
+    lim = SlewRateLimiter(0.0)
+    assert lim.limit(5.0, 1.0) == 0.0
+
+
+def test_reset_to_value():
+    lim = SlewRateLimiter(2.0)
+    lim.limit(10.0, 1.0)
+    lim.reset()
+    assert lim.last_output == 0.0
+    lim.reset(3.5)
+    assert lim.last_output == 3.5
+    # 复位后从新值继续渐变
+    assert lim.limit(10.0, 1.0) == 5.5  # 3.5 + 2×1
+
+
+def test_custom_initial_value():
+    lim = SlewRateLimiter(1.0, initial=5.0)
+    assert lim.limit(10.0, 1.0) == 6.0