mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-07-26 03:42:05 +08:00
infiniteCable2
239 lines
8.7 KiB
Python
239 lines
8.7 KiB
Python
#!/usr/bin/env python3
|
|
import argparse
|
|
from collections.abc import Iterable
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
|
|
from cereal import log
|
|
from openpilot.selfdrive.locationd.curvatured import CurvatureDLookup
|
|
from openpilot.tools.lib.logreader import LogReader, ReadMode
|
|
|
|
|
|
def speed_label(speed_idx: int) -> str:
|
|
speed_kph = CurvatureDLookup.SPEED_ANCHORS[speed_idx] * 3.6
|
|
return f"{speed_kph:.0f} km/h"
|
|
|
|
|
|
def curvature_bucket_label(curvature_idx: int) -> str:
|
|
low = CurvatureDLookup.CURVATURE_BUCKET_EDGES[curvature_idx]
|
|
high = CurvatureDLookup.CURVATURE_BUCKET_EDGES[curvature_idx + 1]
|
|
return f"{low:.2e} .. {high:.2e}"
|
|
|
|
|
|
def iter_param_entries(init_data) -> Iterable:
|
|
params = getattr(init_data, "params", None)
|
|
if params is None:
|
|
return []
|
|
|
|
entries = getattr(params, "entries", None)
|
|
if entries is None:
|
|
return []
|
|
|
|
return entries
|
|
|
|
|
|
def decode_cached_param(init_data):
|
|
for entry in iter_param_entries(init_data):
|
|
if getattr(entry, "key", None) != "LiveCurvatureParameters":
|
|
continue
|
|
|
|
raw_value = bytes(entry.value)
|
|
result = {
|
|
"byte_len": len(raw_value),
|
|
"decoded": None,
|
|
"error": None,
|
|
"redacted": False,
|
|
}
|
|
|
|
if len(raw_value) == 0:
|
|
result["redacted"] = True
|
|
return result
|
|
|
|
try:
|
|
with log.Event.from_bytes(raw_value) as evt:
|
|
result["decoded"] = evt.liveCurvatureParameters
|
|
except Exception as e:
|
|
result["error"] = repr(e)
|
|
|
|
return result
|
|
|
|
return None
|
|
|
|
|
|
def message_summary(msg) -> dict:
|
|
payload = msg.liveCurvatureParameters
|
|
return {
|
|
"valid": bool(msg.valid),
|
|
"live_valid": bool(payload.liveValid),
|
|
"version": int(payload.version),
|
|
"use_params": bool(payload.useParams),
|
|
"cal_perc": int(payload.calPerc),
|
|
"total_points": int(payload.totalBucketPoints),
|
|
"bucket_speed": int(payload.bucketSpeed),
|
|
"bucket_curvature": int(payload.bucketCurvature),
|
|
"bucket_points": int(payload.currentBucketPoints),
|
|
"current_bias": float(payload.currentBias),
|
|
"current_correction": float(payload.currentCorrection),
|
|
}
|
|
|
|
|
|
def print_bucket_details(counts: np.ndarray, biases: np.ndarray, corrections: np.ndarray,
|
|
fit_valid: np.ndarray, speed_idx: int, focus_idx: int | None = None) -> None:
|
|
print(" buckets:")
|
|
for curvature_idx in range(len(CurvatureDLookup.CURVATURE_BUCKET_CENTERS)):
|
|
marker = "*" if focus_idx is not None and curvature_idx == focus_idx else " "
|
|
bucket_range = curvature_bucket_label(curvature_idx)
|
|
bucket_points = int(round(float(counts[speed_idx, curvature_idx])))
|
|
bucket_bias = float(biases[speed_idx, curvature_idx])
|
|
bucket_corr = float(corrections[speed_idx, curvature_idx])
|
|
bucket_valid = bool(fit_valid[speed_idx, curvature_idx])
|
|
print(
|
|
f" {marker} idx={curvature_idx} range={bucket_range} "
|
|
f"points={bucket_points} fitValid={bucket_valid} "
|
|
f"bias={bucket_bias:.8f} corr={bucket_corr:.8f}"
|
|
)
|
|
|
|
|
|
def print_message_summary(title: str, payload) -> None:
|
|
has_debug_arrays = len(list(payload.counts)) == CurvatureDLookup.total_size() and len(list(payload.biases)) == CurvatureDLookup.total_size()
|
|
counts = CurvatureDLookup.unflatten_bucket(list(payload.counts)) if has_debug_arrays else np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32)
|
|
biases = CurvatureDLookup.unflatten_bucket(list(payload.biases)) if has_debug_arrays else np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32)
|
|
corrections = CurvatureDLookup.unflatten_bucket(list(payload.corrections))
|
|
fit_valid = CurvatureDLookup.unflatten_bucket(list(payload.fitValid), dtype=bool)
|
|
|
|
print(title)
|
|
print(f" version: {int(payload.version)}")
|
|
print(f" liveValid: {bool(payload.liveValid)}")
|
|
print(f" useParams: {bool(payload.useParams)}")
|
|
print(f" calPerc: {int(payload.calPerc)}")
|
|
print(f" totalBucketPoints: {int(payload.totalBucketPoints)}")
|
|
print(f" currentCorrection: {float(payload.currentCorrection):.8f}")
|
|
print(f" currentBias: {float(payload.currentBias):.8f}")
|
|
print(f" currentBucket: ({int(payload.bucketSpeed)}, {int(payload.bucketCurvature)})")
|
|
if not has_debug_arrays:
|
|
print(" debugArrays: omitted")
|
|
|
|
speed_entries = []
|
|
for speed_idx in range(len(CurvatureDLookup.SPEED_ANCHORS)):
|
|
speed_counts = counts[speed_idx]
|
|
speed_valid = CurvatureDLookup.speed_curve_valid(counts, speed_idx) if has_debug_arrays else bool(np.any(fit_valid[speed_idx]))
|
|
valid_bucket_count = int(np.count_nonzero(speed_counts >= CurvatureDLookup.MIN_BUCKET_POINTS)) if has_debug_arrays else int(np.count_nonzero(fit_valid[speed_idx]))
|
|
total_points = int(round(float(speed_counts.sum()))) if has_debug_arrays else int(np.count_nonzero(fit_valid[speed_idx]))
|
|
if total_points == 0 and not speed_valid:
|
|
continue
|
|
|
|
best_idx = int(np.argmax(speed_counts)) if total_points > 0 else 0
|
|
best_points = int(round(float(speed_counts[best_idx])))
|
|
best_bias = float(biases[speed_idx, best_idx])
|
|
best_corr = float(corrections[speed_idx, best_idx])
|
|
best_bucket = curvature_bucket_label(best_idx)
|
|
fit_points = int(np.count_nonzero(fit_valid[speed_idx]))
|
|
focus_idx = best_idx
|
|
if int(payload.bucketSpeed) == speed_idx and int(payload.bucketCurvature) >= 0:
|
|
focus_idx = int(payload.bucketCurvature)
|
|
speed_entries.append((speed_idx, focus_idx,
|
|
f" {speed_label(speed_idx)}: valid={speed_valid} total={total_points} "
|
|
f"validBuckets={valid_bucket_count} fitPoints={fit_points} "
|
|
f"topBucket={best_idx} ({best_bucket}) points={best_points} "
|
|
f"bias={best_bias:.8f} corr={best_corr:.8f}")
|
|
)
|
|
|
|
if speed_entries:
|
|
print(" speed anchors:")
|
|
for speed_idx, focus_idx, line in speed_entries:
|
|
print(line)
|
|
if has_debug_arrays:
|
|
print_bucket_details(counts, biases, corrections, fit_valid, speed_idx, focus_idx)
|
|
|
|
|
|
def main() -> None:
|
|
parser = argparse.ArgumentParser(
|
|
description="Dump liveCurvatureParameters from a route, segment, local log file, or URL using the local schema."
|
|
)
|
|
parser.add_argument(
|
|
"route_or_segment_name",
|
|
help="Route id, segment range, local rlog/qlog path, or comma URL accepted by LogReader",
|
|
)
|
|
parser.add_argument(
|
|
"--segment",
|
|
help="Optional segment selector to append to a bare route id, e.g. 0, -1, or 2:6",
|
|
)
|
|
parser.add_argument(
|
|
"--mode",
|
|
choices=[m.value for m in ReadMode],
|
|
default=ReadMode.AUTO.value,
|
|
help="LogReader mode: r=rlog, q=qlog, a=auto, i=auto interactive",
|
|
)
|
|
parser.add_argument("--limit", type=int, default=5, help="How many liveCurvatureParameters events to sample")
|
|
args = parser.parse_args()
|
|
|
|
target = args.route_or_segment_name.strip()
|
|
is_local_path = Path(target).exists()
|
|
if args.segment and not is_local_path and "://" not in target:
|
|
selector = args.segment.strip().lstrip("/")
|
|
if "/q" not in target and "/r" not in target and "/a" not in target and "/i" not in target:
|
|
target = f"{target.rstrip('/')}/{selector}"
|
|
|
|
lr = LogReader(target, default_mode=ReadMode(args.mode))
|
|
|
|
init_data = None
|
|
curvature_msgs = []
|
|
valid_count = 0
|
|
live_valid_count = 0
|
|
|
|
for msg in lr:
|
|
which = msg.which()
|
|
if which == "initData" and init_data is None:
|
|
init_data = msg.initData
|
|
elif which == "liveCurvatureParameters":
|
|
curvature_msgs.append(msg)
|
|
valid_count += int(bool(msg.valid))
|
|
live_valid_count += int(bool(msg.liveCurvatureParameters.liveValid))
|
|
|
|
print(f"liveCurvatureParameters events: {len(curvature_msgs)}")
|
|
if curvature_msgs:
|
|
print(f"transport valid count: {valid_count}")
|
|
print(f"payload liveValid count: {live_valid_count}")
|
|
|
|
first = message_summary(curvature_msgs[0])
|
|
last = message_summary(curvature_msgs[-1])
|
|
print("first event:")
|
|
for key, value in first.items():
|
|
print(f" {key}: {value}")
|
|
print("last event:")
|
|
for key, value in last.items():
|
|
print(f" {key}: {value}")
|
|
|
|
sample_indices = np.linspace(0, len(curvature_msgs) - 1, min(args.limit, len(curvature_msgs)), dtype=int)
|
|
printed = set()
|
|
for idx in sample_indices:
|
|
idx = int(idx)
|
|
if idx in printed:
|
|
continue
|
|
printed.add(idx)
|
|
print_message_summary(f"sample event #{idx}", curvature_msgs[idx].liveCurvatureParameters)
|
|
else:
|
|
print("No liveCurvatureParameters events found.")
|
|
|
|
if init_data is None:
|
|
print("No initData found.")
|
|
return
|
|
|
|
cached = decode_cached_param(init_data)
|
|
if cached is None:
|
|
print("initData: no LiveCurvatureParameters param entry found.")
|
|
return
|
|
|
|
print(f"initData cache bytes: {cached['byte_len']}")
|
|
if cached["redacted"]:
|
|
print("initData cache: redacted (DONT_LOG)")
|
|
elif cached["decoded"] is not None:
|
|
print_message_summary("decoded initData cache", cached["decoded"])
|
|
else:
|
|
print(f"initData cache decode error: {cached['error']}")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|