You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
59 lines
1.8 KiB
Python
59 lines
1.8 KiB
Python
from pathlib import Path
|
|
import sys
|
|
|
|
import numpy as np
|
|
|
|
|
|
PROJECT_ROOT = Path(__file__).resolve().parents[1]
|
|
if str(PROJECT_ROOT) not in sys.path:
|
|
sys.path.insert(0, str(PROJECT_ROOT))
|
|
|
|
from guidance import ScreenGuidanceController
|
|
|
|
|
|
def test_guidance_reset_reinitializes_smoothed_point():
|
|
ctrl = ScreenGuidanceController()
|
|
ctrl.smoothed_point = np.array([100.0, 200.0], dtype=np.float32)
|
|
ctrl.smoothed_cmd = np.array([0.7, -0.4], dtype=np.float32)
|
|
|
|
ctrl.reset_for_target_switch(np.array([500.0, 300.0], dtype=np.float32))
|
|
|
|
assert np.allclose(ctrl.smoothed_point, np.array([500.0, 300.0], dtype=np.float32))
|
|
assert np.allclose(ctrl.smoothed_cmd, np.zeros(2, dtype=np.float32))
|
|
|
|
|
|
def test_guidance_reset_can_damp_instead_of_zero_when_requested():
|
|
ctrl = ScreenGuidanceController()
|
|
ctrl.smoothed_cmd = np.array([0.8, -0.6], dtype=np.float32)
|
|
|
|
ctrl.reset_for_target_switch(np.array([10.0, 20.0], dtype=np.float32), cmd_damp=0.25)
|
|
|
|
assert np.allclose(ctrl.smoothed_cmd, np.array([0.2, -0.15], dtype=np.float32))
|
|
|
|
|
|
def test_guidance_override_box_takes_priority_over_stale_locked_box():
|
|
ctrl = ScreenGuidanceController()
|
|
|
|
state = ctrl.update(
|
|
frame_id=1,
|
|
frame_w=1000,
|
|
frame_h=600,
|
|
confirmed=True,
|
|
locked_box=np.array([50.0, 50.0, 150.0, 150.0], dtype=np.float32),
|
|
pred_box=np.array([60.0, 60.0, 160.0, 160.0], dtype=np.float32),
|
|
override_box=np.array([800.0, 100.0, 900.0, 200.0], dtype=np.float32),
|
|
target_id=1,
|
|
target_track_score=0.55,
|
|
klt_valid=True,
|
|
klt_quality=0.9,
|
|
miss_streak=5,
|
|
vx=0.0,
|
|
vy=0.0,
|
|
)
|
|
|
|
assert state["status"] == "REACQ"
|
|
assert 845.0 <= state["aim_x"] <= 855.0
|
|
assert 145.0 <= state["aim_y"] <= 155.0
|
|
assert state["box_w"] == 100.0
|
|
assert state["box_h"] == 100.0
|