import csv, json, math
|
|
with open('example_path.json') as f:
|
path = json.load(f)
|
start = tuple(path[0])
|
|
rows = []
|
with open('controller_log.csv') as f:
|
reader = csv.DictReader(f)
|
for r in reader:
|
rows.append(r)
|
|
first_follow_idx = None
|
for i, r in enumerate(rows):
|
if r.get('stage','') == 'follow_path':
|
first_follow_idx = i
|
break
|
|
if first_follow_idx is not None:
|
print('Found follow_path at row', first_follow_idx, 't=', rows[first_follow_idx]['t'])
|
for j in range(max(0, first_follow_idx-5), first_follow_idx+5):
|
r = rows[j]
|
x = float(r['x']); y = float(r['y']); heading = float(r['heading'])
|
dx = start[0]-x; dy = start[1]-y
|
desired = math.atan2(dy, dx)
|
h_err = (desired - heading + math.pi) % (2*math.pi) - math.pi
|
print(j, r['t'], r['stage'], r['status'], f"pos=({x:.3f},{y:.3f})", f"dist={(math.hypot(dx,dy)):.3f}", f"h_err={h_err:.3f}")
|
else:
|
print('No follow_path in log. Scanning for closest approach to start...')
|
min_dist = float('inf')
|
min_idx = None
|
for i, r in enumerate(rows):
|
x = float(r['x']); y = float(r['y']);
|
d = math.hypot(start[0]-x, start[1]-y)
|
if d < min_dist:
|
min_dist = d; min_idx = i
|
print('min dist to start =', min_dist, 'at row', min_idx, 't=', rows[min_idx]['t'])
|
print('Nearby log lines:')
|
for j in range(max(0, min_idx-10), min_idx+11):
|
r = rows[j]
|
x = float(r['x']); y = float(r['y']); heading = float(r['heading'])
|
dx = start[0]-x; dy = start[1]-y
|
desired = math.atan2(dy, dx)
|
h_err = (desired - heading + math.pi) % (2*math.pi) - math.pi
|
print(j, r['t'], r['stage'], r['status'], f"pos=({x:.3f},{y:.3f})", f"dist={(math.hypot(dx,dy)):.3f}", f"h_err={h_err:.3f}")
|
|
# also print counts of key statuses
|
from collections import Counter
|
cnt = Counter((r['stage'], r['status']) for r in rows)
|
print('\nStage/Status counts (top 20):')
|
for k,v in cnt.most_common(20):
|
print(k, v)
|
