"""Analyze path_heading from log"""
|
from math import atan2, pi
|
|
with open('controller_log.csv') as f:
|
lines = [l.strip() for l in f.readlines()[1:]]
|
|
filtered = [l.split(',') for l in lines if len(l.split(',')) >= 9]
|
|
print("Analysis at key time points:")
|
for target_t in [10.0, 20.0, 30.0]:
|
rows = [p for p in filtered if abs(float(p[0]) - target_t) < 0.05]
|
if rows:
|
r = rows[0]
|
t = float(r[0])
|
x, y = float(r[1]), float(r[2])
|
heading = float(r[3])
|
tx, ty = float(r[4]), float(r[5])
|
xte = float(r[8])
|
heading_err = float(r[9])
|
|
path_heading = atan2(ty-y, tx-x)
|
|
print(f"\nt={t:.1f}s:")
|
print(f" pos=({x:.3f}, {y:.3f}), heading={heading:.3f} rad = {heading*180/pi:.1f} deg")
|
print(f" target=({tx:.3f}, {ty:.3f})")
|
print(f" path_heading={path_heading:.3f} rad = {path_heading*180/pi:.1f} deg")
|
print(f" xte={xte:.3f}, heading_err={heading_err:.3f}")
|
print(f" => desired_heading = path_heading + xte_correction")
|
print(f" = {path_heading:.3f} + (-1.2 * {xte:.3f})")
|
print(f" = {path_heading - 1.2*xte:.3f} rad = {(path_heading - 1.2*xte)*180/pi:.1f} deg")
|
