"""
|
Check trajectory with approach path visualization
|
"""
|
import json
|
|
# Load controller log
|
lines = []
|
with open('controller_log.csv', 'r') as f:
|
for line in f:
|
lines.append(line.strip())
|
|
# Find reached_start marker
|
reached_start_idx = None
|
for i, line in enumerate(lines):
|
if 'reached_start' in line:
|
reached_start_idx = i
|
print(f"Found reached_start at line {i+1}")
|
break
|
|
if reached_start_idx is None:
|
print("No reached_start found")
|
exit(1)
|
|
# Parse and display trajectory
|
print("\n=== TRAJECTORY ANALYSIS WITH APPROACH PATH ===\n")
|
|
# Show first 20 frames after reached_start
|
print("First 20 frames after reached_start (now following combined path):")
|
for i in range(reached_start_idx + 1, min(reached_start_idx + 21, len(lines))):
|
parts = lines[i].split(',')
|
if len(parts) >= 8:
|
t = float(parts[0])
|
x, y = float(parts[1]), float(parts[2])
|
heading = float(parts[3])
|
target_x, target_y = float(parts[4]), float(parts[5])
|
target_idx = int(parts[7])
|
xte = float(parts[8]) if len(parts) > 8 else 0.0
|
h_err = float(parts[9]) if len(parts) > 9 else 0.0
|
print(f"t={t:6.3f} pos=({x:6.3f},{y:6.3f}) h={heading:5.3f} target=({target_x:6.3f},{target_y:6.3f}) idx={target_idx:3d} xte={xte:6.3f} h_err={h_err:6.3f}")
|
|
# Sample every 10 seconds
|
print("\n\nPosition every 10 seconds:")
|
for target_t in [10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110]:
|
for line in lines[reached_start_idx+1:]:
|
parts = line.split(',')
|
if len(parts) >= 8:
|
t = float(parts[0])
|
if abs(t - target_t) < 0.1: # within 0.1s
|
x, y = float(parts[1]), float(parts[2])
|
heading = float(parts[3])
|
print(f"t={t:6.1f}s: pos=({x:7.3f},{y:7.3f}) heading={heading:6.3f}")
|
break
|
|
# Check if completed
|
print("\n\nCompletion status:")
|
last_line = lines[-1]
|
parts = last_line.split(',')
|
if len(parts) >= 14:
|
t = float(parts[0])
|
x, y = float(parts[1]), float(parts[2])
|
stage = parts[13]
|
print(f"Last frame: t={t:.1f}s, pos=({x:.3f},{y:.3f}), stage={stage}")
|
if t >= 119.0:
|
print("⚠️ TIMEOUT - did not complete in 120s")
|
else:
|
print("✓ Completed successfully")
|
|
# Load paths and check indices
|
print("\n\n=== PATH INFORMATION ===")
|
with open('example_path.json', 'r') as f:
|
work_path = json.load(f)
|
|
print(f"Work path: {len(work_path)} points ({len(work_path)-1} segments)")
|
print(f"Work path range: x=[{min(p[0] for p in work_path):.1f}, {max(p[0] for p in work_path):.1f}], "
|
f"y=[{min(p[1] for p in work_path):.1f}, {max(p[1] for p in work_path):.1f}]")
|
|
# Estimate approach path length
|
print(f"\nNote: Combined path includes approach path + work path")
|
print(f"Check simulation_results.png to see both paths visualized")
|
