mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 13:33:42 +08:00
firestarsdog
238 lines
11 KiB
Python
238 lines
11 KiB
Python
"""Pure offset selection and qualified raw/cluster coordinate conversion for SLC."""
|
|
|
|
import math
|
|
import struct
|
|
from dataclasses import dataclass
|
|
from enum import Enum
|
|
|
|
from openpilot.selfdrive.car.cruise import V_CRUISE_MIN, V_CRUISE_MAX, slc_target_kph
|
|
from openpilot.starpilot.speed_limits import acceptance as acc
|
|
|
|
|
|
class DomainStatus(Enum):
|
|
VALID = "valid"
|
|
ABSENT = "absent"
|
|
UNKNOWN = "unknown"
|
|
STALE = "stale"
|
|
UNAVAILABLE = "unavailable"
|
|
INVALID = "invalid"
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class SpeedPair:
|
|
status: DomainStatus
|
|
raw_mps: float | None = None
|
|
cluster_mps: float | None = None
|
|
session_id: str | None = None
|
|
timestamp_ns: int | None = None
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class OffsetBand:
|
|
lower_mps: float
|
|
upper_mps: float | None
|
|
offset_mps: float
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class OffsetSchedule:
|
|
bands: tuple[OffsetBand, ...]
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class DomainContext:
|
|
session_id: str
|
|
timestamp_ns: int
|
|
accepted_raw_mps: float
|
|
offset_mps: float
|
|
effective_cluster_mps: float
|
|
selected_raw_mps: float
|
|
selected_delta_mps: float
|
|
ego_raw_mps: float
|
|
ego_delta_mps: float
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class DomainResolution:
|
|
status: DomainStatus
|
|
context: DomainContext | None = None
|
|
reason: str = ""
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class PlannerCoordinate:
|
|
cap_mps: float | None
|
|
basis: str
|
|
cluster_target_mps: float | None = None
|
|
ego_delta_mps: float | None = None
|
|
|
|
|
|
def _number(value: object, *, positive: bool = False) -> bool:
|
|
if type(value) not in (int, float):
|
|
return False
|
|
assert isinstance(value, (int, float))
|
|
try:
|
|
return math.isfinite(value) and (value > 0 if positive else value >= 0)
|
|
except OverflowError:
|
|
return False
|
|
|
|
|
|
def _signed_number(value: object) -> bool:
|
|
if type(value) not in (int, float):
|
|
return False
|
|
assert isinstance(value, (int, float))
|
|
try:
|
|
return math.isfinite(value)
|
|
except OverflowError:
|
|
return False
|
|
|
|
|
|
def _pair_valid(pair: object, *, positive_raw: bool) -> bool:
|
|
if not isinstance(pair, SpeedPair) or not isinstance(pair.status, DomainStatus):
|
|
return False
|
|
if pair.status is not DomainStatus.VALID:
|
|
return pair.raw_mps is None and pair.cluster_mps is None
|
|
return (_number(pair.raw_mps, positive=positive_raw) and _number(pair.cluster_mps) and
|
|
pair.cluster_mps is not None and pair.raw_mps is not None and
|
|
type(pair.session_id) is str and bool(pair.session_id) and
|
|
type(pair.timestamp_ns) is int and pair.timestamp_ns >= 0)
|
|
|
|
|
|
def selected_pair_from_kph(raw_mps: float, cluster_kph: float, *, session_id: str, timestamp_ns: int) -> SpeedPair:
|
|
"""Convert a qualified dashboard-selected cluster value from km/h to m/s."""
|
|
if not _number(raw_mps, positive=True) or not _number(cluster_kph):
|
|
return SpeedPair(DomainStatus.INVALID)
|
|
return SpeedPair(DomainStatus.VALID, raw_mps, cluster_kph / 3.6, session_id, timestamp_ns)
|
|
|
|
|
|
def context_is_valid(context: object) -> bool:
|
|
if not isinstance(context, DomainContext):
|
|
return False
|
|
return (type(context.session_id) is str and bool(context.session_id) and type(context.timestamp_ns) is int and
|
|
context.timestamp_ns >= 0 and _number(context.accepted_raw_mps, positive=True) and
|
|
_signed_number(context.offset_mps) and _number(context.effective_cluster_mps, positive=True) and
|
|
math.isclose(context.effective_cluster_mps, context.accepted_raw_mps + context.offset_mps, rel_tol=0, abs_tol=1e-9) and
|
|
_number(context.selected_raw_mps, positive=True) and _number(context.selected_delta_mps) and
|
|
_number(context.ego_raw_mps) and _number(context.ego_delta_mps))
|
|
|
|
|
|
def _schedule_valid(schedule: object) -> bool:
|
|
if not isinstance(schedule, OffsetSchedule) or not isinstance(schedule.bands, tuple) or not schedule.bands:
|
|
return False
|
|
previous_upper = None
|
|
for i, band in enumerate(schedule.bands):
|
|
if not isinstance(band, OffsetBand) or not _number(band.lower_mps) or not _signed_number(band.offset_mps):
|
|
return False
|
|
if band.upper_mps is not None and (not _number(band.upper_mps, positive=True) or band.upper_mps <= band.lower_mps):
|
|
return False
|
|
if i and (previous_upper is None or band.lower_mps < previous_upper):
|
|
return False
|
|
previous_upper = band.upper_mps
|
|
return True
|
|
|
|
|
|
def offset_for_limit(speed_mps: float, schedule: OffsetSchedule) -> float | None:
|
|
if not _number(speed_mps, positive=True) or not _schedule_valid(schedule):
|
|
return None
|
|
for band in schedule.bands:
|
|
if band.lower_mps <= speed_mps and (band.upper_mps is None or speed_mps < band.upper_mps):
|
|
return band.offset_mps
|
|
return None
|
|
|
|
|
|
def adjusted_limit(speed_mps: float, schedule: OffsetSchedule, *, minimum_target_mps: float = 0.0) -> float | None:
|
|
"""Apply the offset within the ordinary cruise-speed bounds."""
|
|
offset = offset_for_limit(speed_mps, schedule)
|
|
if offset is None or not _number(minimum_target_mps):
|
|
return None
|
|
value = speed_mps + offset
|
|
if offset < 0 and value <= minimum_target_mps:
|
|
value = speed_mps
|
|
return min(max(value, V_CRUISE_MIN / 3.6), V_CRUISE_MAX / 3.6) if _number(value, positive=True) else None
|
|
|
|
|
|
def set_speed_matches(target_mps: float, raw_kph: float, cluster_kph: float | None, is_metric: bool | None) -> bool:
|
|
"""Compare the SET values Card would actually write, including display-unit rounding."""
|
|
if type(is_metric) is not bool or not all(_number(v, positive=True) for v in (target_mps, raw_kph, cluster_kph)):
|
|
return False
|
|
if not V_CRUISE_MIN / 3.6 <= target_mps <= V_CRUISE_MAX / 3.6:
|
|
return False
|
|
assert cluster_kph is not None
|
|
# Card receives targetMps as Float32; transport can move a half-unit rounding boundary.
|
|
target_mps = struct.unpack('f', struct.pack('f', target_mps))[0]
|
|
target_kph = slc_target_kph(target_mps, is_metric)
|
|
return (math.isclose(target_kph, raw_kph, rel_tol=0, abs_tol=0.001) and
|
|
math.isclose(target_kph, cluster_kph, rel_tol=0, abs_tol=0.001))
|
|
|
|
|
|
def resolve(decision: acc.Decision, schedule: OffsetSchedule, selected: SpeedPair, ego: SpeedPair) -> DomainResolution:
|
|
"""Bind an explicit offset and contemporaneous speed pairs to an acceptance output."""
|
|
if (not isinstance(decision, acc.Decision) or not isinstance(decision.state, acc.State) or decision.errors or
|
|
not _schedule_valid(schedule) or not _pair_valid(selected, positive_raw=True) or
|
|
not _pair_valid(ego, positive_raw=False) or
|
|
type(decision.state.session_id) is not str or not decision.state.session_id or
|
|
type(decision.state.last_timestamp_ns) is not int or decision.state.last_timestamp_ns < 0 or
|
|
not isinstance(decision.authority, acc.Authority) or
|
|
not acc.authority_is_valid(decision.authority) or not isinstance(decision.policy, acc.Policy) or
|
|
any(type(value) is not bool for value in (decision.policy.confirm_lower, decision.policy.confirm_higher,
|
|
decision.policy.fallback_previous, decision.policy.display_only)) or
|
|
not acc.observation_is_valid(decision.state.observation, session_id=decision.state.session_id)):
|
|
return DomainResolution(DomainStatus.INVALID, reason="invalid acceptance, schedule or speed pair")
|
|
accepted = decision.state.accepted
|
|
if accepted is not None and (not isinstance(accepted, acc.AcceptedLimit) or
|
|
type(accepted.session_id) is not str or not accepted.session_id or
|
|
type(accepted.accepted_at_ns) is not int or accepted.accepted_at_ns < 0 or
|
|
not acc.observation_is_valid(acc.Observation(acc.ObservationKind.VALID, accepted.candidate),
|
|
session_id=decision.state.session_id)):
|
|
return DomainResolution(DomainStatus.INVALID, reason="invalid accepted limit")
|
|
if selected.status is DomainStatus.INVALID or ego.status is DomainStatus.INVALID:
|
|
return DomainResolution(DomainStatus.INVALID, reason="invalid speed coordinate")
|
|
if ((selected.status is DomainStatus.VALID and (selected.session_id != decision.state.session_id or
|
|
selected.timestamp_ns != decision.state.last_timestamp_ns)) or
|
|
(ego.status is DomainStatus.VALID and (ego.session_id != decision.state.session_id or
|
|
ego.timestamp_ns != decision.state.last_timestamp_ns))):
|
|
return DomainResolution(DomainStatus.INVALID, reason="speed pair session or timestamp mismatch")
|
|
if decision.control_target_mps is None or selected.status is not DomainStatus.VALID or ego.status is not DomainStatus.VALID:
|
|
return DomainResolution(DomainStatus.UNAVAILABLE, reason="target or coordinate evidence unavailable")
|
|
target = decision.control_target_mps
|
|
if (not _number(target, positive=True) or accepted is None or
|
|
accepted.candidate.speed_mps != target or not decision.authority.can_control or
|
|
decision.policy.display_only or
|
|
(decision.state.observation.kind is not acc.ObservationKind.VALID and not acc.previous_limit_is_qualified(decision))):
|
|
return DomainResolution(DomainStatus.INVALID, reason="accepted target mismatch")
|
|
assert selected.raw_mps is not None and selected.cluster_mps is not None
|
|
assert ego.raw_mps is not None and ego.cluster_mps is not None
|
|
offset = offset_for_limit(target, schedule)
|
|
if offset is None:
|
|
return DomainResolution(DomainStatus.UNAVAILABLE, reason="no configured offset band")
|
|
ego_delta = max(0.0, ego.cluster_mps - ego.raw_mps)
|
|
effective = adjusted_limit(target, schedule, minimum_target_mps=ego_delta)
|
|
if effective is None:
|
|
return DomainResolution(DomainStatus.INVALID, reason="nonpositive effective target")
|
|
offset = effective - target
|
|
return DomainResolution(DomainStatus.VALID, DomainContext(
|
|
decision.state.session_id, decision.state.last_timestamp_ns, target, offset, effective,
|
|
selected.raw_mps, max(0.0, selected.cluster_mps - selected.raw_mps),
|
|
ego.raw_mps, ego_delta))
|
|
|
|
|
|
def to_planner_coordinate(context: DomainContext, contribution_raw_mps: float | None, basis: str) -> PlannerCoordinate:
|
|
"""Apply a raw override in cluster space, then remove the ego delta exactly once."""
|
|
if (not context_is_valid(context) or basis not in ("none", "adoption_clear", "pedal", "persistent") or
|
|
(basis in ("none", "adoption_clear")) != (contribution_raw_mps is None)):
|
|
return PlannerCoordinate(None, "invalid")
|
|
if contribution_raw_mps is not None and not _number(contribution_raw_mps, positive=True):
|
|
return PlannerCoordinate(None, "invalid")
|
|
cluster_target = context.effective_cluster_mps
|
|
if basis == "pedal":
|
|
if contribution_raw_mps != context.ego_raw_mps:
|
|
return PlannerCoordinate(None, "invalid")
|
|
cluster_target = max(cluster_target, context.ego_raw_mps + context.ego_delta_mps)
|
|
elif basis == "persistent":
|
|
assert contribution_raw_mps is not None
|
|
cluster_target = max(cluster_target, contribution_raw_mps + context.selected_delta_mps)
|
|
cap = cluster_target - context.ego_delta_mps
|
|
if not _number(cap, positive=True):
|
|
return PlannerCoordinate(None, "nonpositive", cluster_target, context.ego_delta_mps)
|
|
return PlannerCoordinate(cap, basis, cluster_target, context.ego_delta_mps)
|