|
| 1 | +import json |
| 2 | +import sys |
| 3 | +import os |
| 4 | +import matplotlib.pyplot as plt |
| 5 | +import numpy as np |
| 6 | + |
| 7 | +# Safety thresholds (not used in the current plots) |
| 8 | +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe |
| 9 | +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe |
| 10 | +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe |
| 11 | +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe |
| 12 | + |
| 13 | +def parse_behavior_log(filename): |
| 14 | + times = [] |
| 15 | + accelerations = [] |
| 16 | + heading_rates = [] |
| 17 | + |
| 18 | + with open(filename, 'r') as f: |
| 19 | + first_time = None # Store the first timestamp for shifting |
| 20 | + for line in f: |
| 21 | + try: |
| 22 | + entry = json.loads(line) |
| 23 | + except json.JSONDecodeError: |
| 24 | + print(f"Skipping invalid JSON line: {line.strip()}") |
| 25 | + continue |
| 26 | + if "vehicle" in entry: |
| 27 | + t = entry.get("time") |
| 28 | + vehicle_data = entry["vehicle"].get("data", {}) |
| 29 | + acceleration = vehicle_data.get("acceleration") |
| 30 | + heading_rate = vehicle_data.get("heading_rate") |
| 31 | + if t is not None and acceleration is not None and heading_rate is not None: |
| 32 | + if first_time is None: |
| 33 | + first_time = t # Set the first timestamp |
| 34 | + times.append(t - first_time) # Shift time to start from 0 |
| 35 | + accelerations.append(acceleration) |
| 36 | + heading_rates.append(heading_rate) |
| 37 | + |
| 38 | + return np.array(times), np.array(accelerations), np.array(heading_rates) |
| 39 | + |
| 40 | +def parse_tracker_csv(filename): |
| 41 | + data = np.genfromtxt(filename, delimiter=',', skip_header=1) |
| 42 | + first_time = data[0, 18] # Normalize time to start from 0 |
| 43 | + cte_time = data[:, 18] - first_time |
| 44 | + cte = data[:, 20] |
| 45 | + x_actual, y_actual = data[:, 2], data[:, 5] |
| 46 | + x_desired, y_desired = data[:, 11], data[:, 14] |
| 47 | + return cte_time, cte, x_actual, y_actual, x_desired, y_desired |
| 48 | + |
| 49 | +def compute_derivative(times, values): |
| 50 | + dt = np.diff(times) |
| 51 | + dv = np.diff(values) |
| 52 | + derivative = dv / dt |
| 53 | + return times[1:], derivative |
| 54 | + |
| 55 | +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): |
| 56 | + global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) |
| 57 | + global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) |
| 58 | + |
| 59 | + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) |
| 60 | + fig.subplots_adjust(hspace=0.4, wspace=0.3) |
| 61 | + |
| 62 | + axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') |
| 63 | + axs[0,0].set_ylabel("Jerk (m/s³)") |
| 64 | + axs[0,0].set_title("Vehicle Jerk Over Time") |
| 65 | + axs[0,0].grid(True) |
| 66 | + axs[0,0].set_xlim(global_min_time, global_max_time) |
| 67 | + |
| 68 | + axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') |
| 69 | + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") |
| 70 | + axs[0,1].set_title("Vehicle Heading Acceleration Over Time") |
| 71 | + axs[0,1].grid(True) |
| 72 | + axs[0,1].set_xlim(global_min_time, global_max_time) |
| 73 | + |
| 74 | + axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') |
| 75 | + axs[1,0].set_xlabel("Time (s)") |
| 76 | + axs[1,0].set_ylabel("Cross Track Error") |
| 77 | + axs[1,0].set_title("Vehicle Cross Track Error Over Time") |
| 78 | + axs[1,0].grid(True) |
| 79 | + axs[1,0].set_xlim(global_min_time, global_max_time) |
| 80 | + |
| 81 | + axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') |
| 82 | + axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') |
| 83 | + axs[1,1].set_xlabel("X Position (m)") |
| 84 | + axs[1,1].set_ylabel("Y Position (m)") |
| 85 | + axs[1,1].set_title("Vehicle Position") |
| 86 | + axs[1,1].legend() |
| 87 | + axs[1,1].grid(True) |
| 88 | + |
| 89 | + plt.show() |
| 90 | + |
| 91 | +if __name__=='__main__': |
| 92 | + if len(sys.argv) != 2: |
| 93 | + print("Usage: python test_comfort_metrics.py <log_directory>") |
| 94 | + sys.exit(1) |
| 95 | + |
| 96 | + log_dir = sys.argv[1] |
| 97 | + behavior_file = os.path.join(log_dir, "behavior.json") |
| 98 | + tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") |
| 99 | + #tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") |
| 100 | + |
| 101 | + times, accelerations, heading_rates = parse_behavior_log(behavior_file) |
| 102 | + time_jerk, jerk = compute_derivative(times, accelerations) |
| 103 | + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) |
| 104 | + |
| 105 | + cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) |
| 106 | + |
| 107 | + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) |
| 108 | + |
| 109 | + print("Max (abs) jerk:", np.max(np.abs(jerk))) |
| 110 | + print("Avg jerk:", np.mean(np.abs(jerk))) |
| 111 | + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) |
| 112 | + print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) |
| 113 | + print("Max (abs) cross track error:", np.max(np.abs(cte))) |
| 114 | + print("Avg cross track error:", np.mean(np.abs(cte))) |
| 115 | + |
0 commit comments