119 lines
3.6 KiB
Python
119 lines
3.6 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
|
|
Original concept ("Increased Stop Distance") by SpysyWeeb (github.com/SpysyWeeb)
|
|
"""
|
|
from types import SimpleNamespace
|
|
|
|
from iqdbc.car.interfaces import ACCEL_MIN
|
|
from iqpilot.selfdrive.iqmodeld.config import ModelConstants
|
|
from iqpilot.selfdrive.controls.lib.custom_stop_distance import (
|
|
CustomStopDistance,
|
|
MIN_ADJUSTED_D_REL,
|
|
)
|
|
|
|
|
|
def _build(distance):
|
|
c = CustomStopDistance.__new__(CustomStopDistance)
|
|
c.frame = 0
|
|
c.distance = float(distance)
|
|
return c
|
|
|
|
|
|
def _model_msg(stop_distance, end_velocity):
|
|
x = [0.0] * (ModelConstants.IDX_N - 1) + [stop_distance]
|
|
v = [0.0] * (ModelConstants.IDX_N - 1) + [end_velocity]
|
|
return SimpleNamespace(position=SimpleNamespace(x=x), velocity=SimpleNamespace(x=v))
|
|
|
|
|
|
def test_zero_distance_is_a_no_op():
|
|
c = _build(0)
|
|
lead = {'status': True, 'dRel': 10.0, 'vLead': 0.0}
|
|
assert c.apply_lead(dict(lead)) == lead
|
|
|
|
|
|
def test_positive_distance_reduces_reported_lead_distance():
|
|
c = _build(2)
|
|
lead = {'status': True, 'dRel': 10.0, 'vLead': 0.0}
|
|
out = c.apply_lead(dict(lead))
|
|
assert out['dRel'] == 8.0
|
|
|
|
|
|
def test_negative_distance_increases_reported_lead_distance():
|
|
c = _build(-2)
|
|
lead = {'status': True, 'dRel': 10.0, 'vLead': 0.0}
|
|
out = c.apply_lead(dict(lead))
|
|
assert out['dRel'] == 12.0
|
|
|
|
|
|
def test_positive_distance_never_reports_below_floor():
|
|
c = _build(2)
|
|
lead = {'status': True, 'dRel': 1.5, 'vLead': 0.0}
|
|
out = c.apply_lead(dict(lead))
|
|
assert out['dRel'] == MIN_ADJUSTED_D_REL
|
|
|
|
|
|
def test_positive_distance_never_reports_further_than_reality():
|
|
c = _build(2)
|
|
lead = {'status': True, 'dRel': 0.5, 'vLead': 0.0}
|
|
out = c.apply_lead(dict(lead))
|
|
assert out['dRel'] == 0.5
|
|
|
|
|
|
def test_offset_fades_out_as_lead_speeds_up():
|
|
c = _build(2)
|
|
lead = {'status': True, 'dRel': 10.0, 'vLead': 3.0}
|
|
out = c.apply_lead(dict(lead))
|
|
assert out['dRel'] == 10.0
|
|
|
|
|
|
def test_no_lead_is_untouched():
|
|
c = _build(2)
|
|
lead = {'status': False, 'dRel': 10.0, 'vLead': 0.0}
|
|
out = c.apply_lead(dict(lead))
|
|
assert out['dRel'] == 10.0
|
|
|
|
|
|
def test_e2e_negative_distance_is_a_no_op():
|
|
c = _build(-2)
|
|
a_target, should_stop = c.adjust_e2e_stop(-0.5, False, 0.2, _model_msg(3.0, 0.0))
|
|
assert (a_target, should_stop) == (-0.5, False)
|
|
|
|
|
|
def test_e2e_zero_distance_is_a_no_op():
|
|
c = _build(0)
|
|
a_target, should_stop = c.adjust_e2e_stop(-0.5, False, 0.2, _model_msg(3.0, 0.0))
|
|
assert (a_target, should_stop) == (-0.5, False)
|
|
|
|
|
|
def test_e2e_stop_sign_plans_are_untouched():
|
|
c = _build(2)
|
|
# model plan still moving at the end -> proceeding through (stop sign), not held
|
|
a_target, should_stop = c.adjust_e2e_stop(-0.5, False, 0.2, _model_msg(3.0, 5.0))
|
|
assert (a_target, should_stop) == (-0.5, False)
|
|
|
|
|
|
def test_e2e_holds_short_of_model_stop_when_already_stopped():
|
|
c = _build(2)
|
|
a_target, should_stop = c.adjust_e2e_stop(0.0, False, 0.1, _model_msg(stop_distance=3.0, end_velocity=0.0))
|
|
assert should_stop is True
|
|
|
|
|
|
def test_e2e_does_not_hold_once_past_offset_and_buffer():
|
|
c = _build(2)
|
|
a_target, should_stop = c.adjust_e2e_stop(0.0, False, 0.1, _model_msg(stop_distance=10.0, end_velocity=0.0))
|
|
assert should_stop is False
|
|
|
|
|
|
def test_e2e_deepens_braking_already_in_progress():
|
|
c = _build(2)
|
|
a_target, should_stop = c.adjust_e2e_stop(-0.5, False, 5.0, _model_msg(stop_distance=10.0, end_velocity=0.0))
|
|
assert a_target < -0.5
|
|
assert a_target >= ACCEL_MIN
|
|
|
|
|
|
def test_e2e_never_relaxes_braking():
|
|
c = _build(2)
|
|
a_target, should_stop = c.adjust_e2e_stop(0.0, False, 5.0, _model_msg(stop_distance=10.0, end_velocity=0.0))
|
|
assert a_target == 0.0
|