github-actions[bot]
f8e13417f2
date: 2026-08-15T14:20:10 master commit: d48cfa730bf21593b684efeaf3fd8940695e1dea
48 lines
1.9 KiB
Python
48 lines
1.9 KiB
Python
import itertools
|
|
from openpilot.common.test import OpenpilotTestCase
|
|
from openpilot.common.parameterized import parameterized_class
|
|
|
|
from openpilot.cereal import log
|
|
|
|
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
|
|
from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
|
|
|
|
|
|
def desired_follow_distance(v_ego, v_lead, t_follow=None):
|
|
if t_follow is None:
|
|
t_follow = get_T_FOLLOW()
|
|
return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
|
|
|
|
def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0):
|
|
man = Maneuver(
|
|
'',
|
|
duration=t_end,
|
|
initial_speed=float(v_lead),
|
|
lead_relevancy=True,
|
|
initial_distance_lead=100,
|
|
speed_lead_values=[v_lead],
|
|
breakpoints=[0.],
|
|
e2e=e2e,
|
|
personality=personality,
|
|
)
|
|
valid, output = man.evaluate()
|
|
assert valid
|
|
return output[-1,2] - output[-1,1]
|
|
|
|
|
|
@parameterized_class(("e2e", "personality", "speed"), itertools.product(
|
|
[True, False], # e2e
|
|
[log.LongitudinalPersonality.relaxed, # personality
|
|
log.LongitudinalPersonality.standard,
|
|
log.LongitudinalPersonality.aggressive],
|
|
[0,10,35])) # speed
|
|
class TestFollowingDistance(OpenpilotTestCase):
|
|
def test_following_distance(self):
|
|
v_lead = float(self.speed)
|
|
simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e, personality=self.personality)
|
|
correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
|
|
err_ratio = 0.2 if self.e2e else 0.1
|
|
abs_err_margin = 0.5 if v_lead > 0.0 else 1.15
|
|
self.assertAlmostEqual(simulation_steady_state, correct_steady_state,
|
|
delta=err_ratio * correct_steady_state + abs_err_margin)
|