From 7c48e5679929c28ae1a6ab14d90189c2604d4c9b Mon Sep 17 00:00:00 2001 From: pravshot Date: Tue, 18 Feb 2025 18:29:41 -0600 Subject: [PATCH 01/10] initial plot metrics --- testing/test_comfort_metrics.py | 64 +++++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 testing/test_comfort_metrics.py diff --git a/testing/test_comfort_metrics.py b/testing/test_comfort_metrics.py new file mode 100644 index 000000000..158e33c3d --- /dev/null +++ b/testing/test_comfort_metrics.py @@ -0,0 +1,64 @@ +import json +import sys +import matplotlib.pyplot as plt + +def parse_log(filename): + """Reads a JSON log file and extracts time, acceleration, and heading_rate""" + times = [] + accelerations = [] + heading_rates = [] + + with open(filename, 'r') as f: + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError as e: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + # Only add if all required fields are available + if t is not None and acceleration is not None and heading_rate is not None: + times.append(t) + accelerations.append(acceleration) + heading_rates.append(heading_rate) + return times, accelerations, heading_rates + +def plot_metrics(times, accelerations, heading_rates): + """Creates two subplots: + - Instantaneous acceleration vs. time + - Heading rate vs. time + """ + plt.figure(figsize=(12, 8)) + # Plot acceleration + plt.subplot(2, 1, 1) + plt.plot(times, accelerations, 'b-', marker='o', label="Acceleration (m/s²)") + plt.xlabel("Time (s)") + plt.ylabel("Acceleration (m/s²)") + plt.title("Vehicle Acceleration Over Time") + plt.legend() + plt.grid(True) + + # Plot heading rate + plt.subplot(2, 1, 2) + plt.plot(times, heading_rates, 'r-', marker='x', label="Heading Rate (rad/s)") + plt.xlabel("Time (s)") + plt.ylabel("Heading Rate (rad/s)") + plt.title("Vehicle Heading Rate Over Time") + plt.legend() + plt.grid(True) + + plt.tight_layout() + plt.show() + +def main(): + if len(sys.argv) != 2: + print("Usage: python test_comfort_metrics.py .json") + sys.exit(1) + + filename = sys.argv[1] + times, accelerations, heading_rates = parse_log(filename) + plot_metrics(times, accelerations, heading_rates) \ No newline at end of file From 3fcd14a5b50ac03eed439d80cd74bc056f83c7eb Mon Sep 17 00:00:00 2001 From: Praveen Kalva Date: Wed, 19 Feb 2025 02:10:41 +0000 Subject: [PATCH 02/10] added safety factor scale --- testing/test_comfort_metrics.py | 69 +++++++++++++++++++++++---------- 1 file changed, 49 insertions(+), 20 deletions(-) diff --git a/testing/test_comfort_metrics.py b/testing/test_comfort_metrics.py index 158e33c3d..69372bcca 100644 --- a/testing/test_comfort_metrics.py +++ b/testing/test_comfort_metrics.py @@ -1,6 +1,14 @@ import json import sys import matplotlib.pyplot as plt +import matplotlib.cm as cm +import matplotlib.colors as mcolors +import numpy as np + +ACC_SAFE = 0.8 # |acceleration| <= 2.0 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 3.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.1 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.3 rad/s is unsafe def parse_log(filename): """Reads a JSON log file and extracts time, acceleration, and heading_rate""" @@ -25,40 +33,61 @@ def parse_log(filename): times.append(t) accelerations.append(acceleration) heading_rates.append(heading_rate) - return times, accelerations, heading_rates + + return np.array(times), np.array(accelerations), np.array(heading_rates) + +def compute_safety_factor(value, safe_thresh, unsafe_thresh): + """Computes a safety factor between 0 and 1 to be used by plots""" + abs_val = abs(value) + if abs_val <= safe_thresh: + return 1.0 + elif abs_val >= unsafe_thresh: + return 0.0 + else: + return 1.0 - (abs_val - safe_thresh) / (unsafe_thresh - safe_thresh) def plot_metrics(times, accelerations, heading_rates): """Creates two subplots: - Instantaneous acceleration vs. time - Heading rate vs. time """ - plt.figure(figsize=(12, 8)) - # Plot acceleration - plt.subplot(2, 1, 1) - plt.plot(times, accelerations, 'b-', marker='o', label="Acceleration (m/s²)") - plt.xlabel("Time (s)") - plt.ylabel("Acceleration (m/s²)") - plt.title("Vehicle Acceleration Over Time") - plt.legend() - plt.grid(True) + # Compute safety factors for acceleration and heading rate + acc_safety = np.vectorize(compute_safety_factor)(accelerations, ACC_SAFE, ACC_UNSAFE) + hr_safety = np.vectorize(compute_safety_factor)(heading_rates, HR_SAFE, HR_UNSAFE) - # Plot heading rate - plt.subplot(2, 1, 2) - plt.plot(times, heading_rates, 'r-', marker='x', label="Heading Rate (rad/s)") - plt.xlabel("Time (s)") - plt.ylabel("Heading Rate (rad/s)") - plt.title("Vehicle Heading Rate Over Time") - plt.legend() - plt.grid(True) + fig, axs = plt.subplots(2, 1, figsize=(12, 8)) + cmap = cm.get_cmap('RdYlGn') + # Plot acceleration with scatter coloring + sc1 = axs[0].scatter(times, accelerations, c=acc_safety, cmap=cmap, vmin=0, vmax=1, marker='o') + axs[0].plot(times, accelerations, color='gray', alpha=0.5, linestyle='--') + axs[0].set_xlabel("Time (s)") + axs[0].set_ylabel("Acceleration (m/s²)") + axs[0].set_title("Vehicle Acceleration Over Time") + axs[0].grid(True) + cbar1 = fig.colorbar(sc1, ax=axs[0]) + cbar1.set_label("Safety Factor (1=safe, 0=unsafe)") + + # Plot heading rate with scatter coloring + sc2 = axs[1].scatter(times, heading_rates, c=hr_safety, cmap=cmap, vmin=0, vmax=1, marker='x') + axs[1].plot(times, heading_rates, color='gray', alpha=0.5, linestyle='--') + axs[1].set_xlabel("Time (s)") + axs[1].set_ylabel("Heading Rate (rad/s)") + axs[1].set_title("Vehicle Heading Rate Over Time") + axs[1].grid(True) + cbar2 = fig.colorbar(sc2, ax=axs[1]) + cbar2.set_label("Safety Factor (1=safe, 0=unsafe)") plt.tight_layout() plt.show() -def main(): +if __name__=='__main__': if len(sys.argv) != 2: print("Usage: python test_comfort_metrics.py .json") sys.exit(1) filename = sys.argv[1] times, accelerations, heading_rates = parse_log(filename) - plot_metrics(times, accelerations, heading_rates) \ No newline at end of file + plot_metrics(times, accelerations, heading_rates) + + print("Max (abs) acceleration:", np.max(np.abs(accelerations))) + print("Max (abs) heading rate:", np.max(np.abs(heading_rates))) \ No newline at end of file From 331e40ab35829562a5af9f9adf005db082ee6a63 Mon Sep 17 00:00:00 2001 From: pravshot Date: Mon, 24 Feb 2025 15:14:58 -0600 Subject: [PATCH 03/10] added cross track error --- testing/test_comfort_metrics.py | 139 +++++++++++++++++++++----------- 1 file changed, 94 insertions(+), 45 deletions(-) diff --git a/testing/test_comfort_metrics.py b/testing/test_comfort_metrics.py index 69372bcca..e186e5d13 100644 --- a/testing/test_comfort_metrics.py +++ b/testing/test_comfort_metrics.py @@ -1,17 +1,16 @@ import json import sys +import os import matplotlib.pyplot as plt -import matplotlib.cm as cm -import matplotlib.colors as mcolors import numpy as np -ACC_SAFE = 0.8 # |acceleration| <= 2.0 m/s² is safe -ACC_UNSAFE = 2.0 # |acceleration| >= 3.0 m/s² is unsafe -HR_SAFE = 0.05 # |heading_rate| <= 0.1 rad/s is safe -HR_UNSAFE = 0.2 # |heading_rate| >= 0.3 rad/s is unsafe +# Safety thresholds (not used in the current plots) +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe -def parse_log(filename): - """Reads a JSON log file and extracts time, acceleration, and heading_rate""" +def parse_behavior_log(filename): times = [] accelerations = [] heading_rates = [] @@ -20,7 +19,7 @@ def parse_log(filename): for line in f: try: entry = json.loads(line) - except json.JSONDecodeError as e: + except json.JSONDecodeError: print(f"Skipping invalid JSON line: {line.strip()}") continue if "vehicle" in entry: @@ -37,7 +36,7 @@ def parse_log(filename): return np.array(times), np.array(accelerations), np.array(heading_rates) def compute_safety_factor(value, safe_thresh, unsafe_thresh): - """Computes a safety factor between 0 and 1 to be used by plots""" + """Computes a safety factor between 0 and 1 (for coloring in plots)""" abs_val = abs(value) if abs_val <= safe_thresh: return 1.0 @@ -46,48 +45,98 @@ def compute_safety_factor(value, safe_thresh, unsafe_thresh): else: return 1.0 - (abs_val - safe_thresh) / (unsafe_thresh - safe_thresh) -def plot_metrics(times, accelerations, heading_rates): - """Creates two subplots: - - Instantaneous acceleration vs. time - - Heading rate vs. time - """ - # Compute safety factors for acceleration and heading rate - acc_safety = np.vectorize(compute_safety_factor)(accelerations, ACC_SAFE, ACC_UNSAFE) - hr_safety = np.vectorize(compute_safety_factor)(heading_rates, HR_SAFE, HR_UNSAFE) +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte): + """Plots jerk, heading acceleration, and cross-track error in subplots.""" + fig, axs = plt.subplots(3, 1, figsize=(10, 12), sharex=True) + fig.subplots_adjust(hspace=0.4) - fig, axs = plt.subplots(2, 1, figsize=(12, 8)) - cmap = cm.get_cmap('RdYlGn') - # Plot acceleration with scatter coloring - sc1 = axs[0].scatter(times, accelerations, c=acc_safety, cmap=cmap, vmin=0, vmax=1, marker='o') - axs[0].plot(times, accelerations, color='gray', alpha=0.5, linestyle='--') - axs[0].set_xlabel("Time (s)") - axs[0].set_ylabel("Acceleration (m/s²)") - axs[0].set_title("Vehicle Acceleration Over Time") + # Jerk plot + axs[0].plot(time_jerk, jerk, marker='o', linestyle='-', color='blue') + axs[0].set_ylabel("Jerk (m/s³)") + axs[0].set_title("Vehicle Jerk Over Time") axs[0].grid(True) - cbar1 = fig.colorbar(sc1, ax=axs[0]) - cbar1.set_label("Safety Factor (1=safe, 0=unsafe)") - # Plot heading rate with scatter coloring - sc2 = axs[1].scatter(times, heading_rates, c=hr_safety, cmap=cmap, vmin=0, vmax=1, marker='x') - axs[1].plot(times, heading_rates, color='gray', alpha=0.5, linestyle='--') - axs[1].set_xlabel("Time (s)") - axs[1].set_ylabel("Heading Rate (rad/s)") - axs[1].set_title("Vehicle Heading Rate Over Time") + # Heading acceleration plot + axs[1].plot(time_heading_acc, heading_acc, marker='x', linestyle='-', color='orange') + axs[1].set_ylabel("Heading Acceleration (rad/s²)") + axs[1].set_title("Vehicle Heading Acceleration Over Time") axs[1].grid(True) - cbar2 = fig.colorbar(sc2, ax=axs[1]) - cbar2.set_label("Safety Factor (1=safe, 0=unsafe)") - plt.tight_layout() + # Cross track error plot + axs[2].plot(cte_time, cte, marker='s', linestyle='-', color='green') + axs[2].set_xlabel("Time (s)") + axs[2].set_ylabel("Cross Track Error") + axs[2].set_title("Vehicle Cross Track Error Over Time") + axs[2].grid(True) + + plt.show() + +def compute_derivative(times, values): + """ + Computes the derivative of array with respect to time. + Returns the time array and derivative values. + """ + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def parse_tracker_csv(filename): + """ + - Crosstrack error time (from column index 18) + - Crosstrack error (from column index 20) + """ + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + cte_time = data[:, 18] + cte = data[:, 20] + return cte_time, cte + +def plot_jerk(time, jerk): + """Plots vehicle jerk (rate of acceleration) vs. time""" + plt.figure(figsize=(12, 6)) + plt.plot(time, jerk, marker='o', linestyle='-', color='blue') + plt.xlabel("Time (s)") + plt.ylabel("Jerk (m/s³)") + plt.title("Vehicle Jerk Over Time") + plt.grid(True) + plt.show() + +def plot_heading_acceleration(time, heading_acc): + """Plots vehicle heading acceleration vs. time""" + plt.figure(figsize=(12, 6)) + plt.plot(time, heading_acc, marker='x', linestyle='-', color='orange') + plt.xlabel("Time (s)") + plt.ylabel("Heading Acceleration (rad/s²)") + plt.title("Vehicle Heading Acceleration Over Time") + plt.grid(True) + plt.show() + +def plot_crosstrack_error(time, cte): + """Plots vehicle cross track error vs. time""" + plt.figure(figsize=(12, 6)) + plt.plot(time, cte, marker='s', linestyle='-', color='green') + plt.xlabel("Time (s)") + plt.ylabel("Cross Track Error") + plt.title("Vehicle Cross Track Error Over Time") + plt.grid(True) plt.show() if __name__=='__main__': if len(sys.argv) != 2: - print("Usage: python test_comfort_metrics.py .json") + print("Usage: python test_comfort_metrics.py ") sys.exit(1) - - filename = sys.argv[1] - times, accelerations, heading_rates = parse_log(filename) - plot_metrics(times, accelerations, heading_rates) - - print("Max (abs) acceleration:", np.max(np.abs(accelerations))) - print("Max (abs) heading rate:", np.max(np.abs(heading_rates))) \ No newline at end of file + + log_dir = sys.argv[1] + behavior_file = os.path.join(log_dir, "behavior.json") + tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + + times, accelerations, heading_rates = parse_behavior_log(behavior_file) + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + cte_time, cte = parse_tracker_csv(tracker_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte) + + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) + print("Max (abs) cross track error:", np.max(np.abs(cte))) \ No newline at end of file From 630933a85b9e9724f603aba3c31442f4c13136ae Mon Sep 17 00:00:00 2001 From: pravshot Date: Mon, 24 Feb 2025 16:07:29 -0600 Subject: [PATCH 04/10] added position graph --- testing/test_comfort_metrics.py | 104 ++++++++++++++++++++------------ 1 file changed, 67 insertions(+), 37 deletions(-) diff --git a/testing/test_comfort_metrics.py b/testing/test_comfort_metrics.py index e186e5d13..db80a3c88 100644 --- a/testing/test_comfort_metrics.py +++ b/testing/test_comfort_metrics.py @@ -10,6 +10,16 @@ HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe +def compute_safety_factor(value, safe_thresh, unsafe_thresh): + """Computes a safety factor between 0 and 1 (for coloring in plots)""" + abs_val = abs(value) + if abs_val <= safe_thresh: + return 1.0 + elif abs_val >= unsafe_thresh: + return 0.0 + else: + return 1.0 - (abs_val - safe_thresh) / (unsafe_thresh - safe_thresh) + def parse_behavior_log(filename): times = [] accelerations = [] @@ -35,39 +45,55 @@ def parse_behavior_log(filename): return np.array(times), np.array(accelerations), np.array(heading_rates) -def compute_safety_factor(value, safe_thresh, unsafe_thresh): - """Computes a safety factor between 0 and 1 (for coloring in plots)""" - abs_val = abs(value) - if abs_val <= safe_thresh: - return 1.0 - elif abs_val >= unsafe_thresh: - return 0.0 - else: - return 1.0 - (abs_val - safe_thresh) / (unsafe_thresh - safe_thresh) +def parse_tracker_csv(filename): + """ + - Crosstrack error time (from column index 18) + - Crosstrack error (from column index 20) + - X position actual (from column index 2) + - Y position actual (from column index 5) + - X position desired (from column index 11) + - Y position desired (from column index 14) + """ + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + cte_time = data[:, 18] + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + x_desired, y_desired = data[:, 11], data[:, 14] + return cte_time, cte, x_actual, y_actual, x_desired, y_desired + -def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte): +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): """Plots jerk, heading acceleration, and cross-track error in subplots.""" - fig, axs = plt.subplots(3, 1, figsize=(10, 12), sharex=True) - fig.subplots_adjust(hspace=0.4) + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.4, wspace=0.3) # Jerk plot - axs[0].plot(time_jerk, jerk, marker='o', linestyle='-', color='blue') - axs[0].set_ylabel("Jerk (m/s³)") - axs[0].set_title("Vehicle Jerk Over Time") - axs[0].grid(True) + axs[0,0].plot(time_jerk, jerk, marker='o', linestyle='-', color='blue') + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].grid(True) # Heading acceleration plot - axs[1].plot(time_heading_acc, heading_acc, marker='x', linestyle='-', color='orange') - axs[1].set_ylabel("Heading Acceleration (rad/s²)") - axs[1].set_title("Vehicle Heading Acceleration Over Time") - axs[1].grid(True) + axs[0,1].plot(time_heading_acc, heading_acc, marker='x', linestyle='-', color='orange') + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].set_title("Vehicle Heading Acceleration Over Time") + axs[0,1].grid(True) # Cross track error plot - axs[2].plot(cte_time, cte, marker='s', linestyle='-', color='green') - axs[2].set_xlabel("Time (s)") - axs[2].set_ylabel("Cross Track Error") - axs[2].set_title("Vehicle Cross Track Error Over Time") - axs[2].grid(True) + axs[1,0].plot(cte_time, cte, marker='s', linestyle='-', color='green') + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("Cross Track Error") + axs[1,0].set_title("Vehicle Cross Track Error Over Time") + axs[1,0].grid(True) + + # Position plot + axs[1,1].plot(x_actual, y_actual, marker='o', linestyle='-', color='blue', label='Actual') + axs[1,1].plot(x_desired, y_desired, marker='x', linestyle='-', color='orange', label='Desired') + axs[1,1].set_xlabel("X Position (m)") + axs[1,1].set_ylabel("Y Position (m)") + axs[1,1].set_title("Vehicle Position") + axs[1,1].legend() + axs[1,1].grid(True) plt.show() @@ -81,16 +107,6 @@ def compute_derivative(times, values): derivative = dv / dt return times[1:], derivative -def parse_tracker_csv(filename): - """ - - Crosstrack error time (from column index 18) - - Crosstrack error (from column index 20) - """ - data = np.genfromtxt(filename, delimiter=',', skip_header=1) - cte_time = data[:, 18] - cte = data[:, 20] - return cte_time, cte - def plot_jerk(time, jerk): """Plots vehicle jerk (rate of acceleration) vs. time""" plt.figure(figsize=(12, 6)) @@ -121,6 +137,19 @@ def plot_crosstrack_error(time, cte): plt.grid(True) plt.show() +def plot_position(time, x_actual, y_actual, x_desired, y_desired): + """Plots vehicle actual and desired positions vs. time""" + plt.figure(figsize=(12, 6)) + plt.plot(x_actual, y_actual, marker='o', linestyle='-', color='blue', label='Actual') + plt.plot(x_desired, y_desired, marker='x', linestyle='-', color='orange', label='Desired') + plt.xlabel("X Position (m)") + plt.ylabel("Y Position (m)") + plt.title("Vehicle Position Over Time") + plt.legend() + plt.grid(True) + plt.show() + + if __name__=='__main__': if len(sys.argv) != 2: print("Usage: python test_comfort_metrics.py ") @@ -133,9 +162,10 @@ def plot_crosstrack_error(time, cte): times, accelerations, heading_rates = parse_behavior_log(behavior_file) time_jerk, jerk = compute_derivative(times, accelerations) time_heading_acc, heading_acc = compute_derivative(times, heading_rates) - cte_time, cte = parse_tracker_csv(tracker_file) - plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte) + cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) print("Max (abs) jerk:", np.max(np.abs(jerk))) print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) From a1fae811241aac27454bd8b3064eae63762bfc75 Mon Sep 17 00:00:00 2001 From: Jugal Date: Mon, 24 Feb 2025 16:07:41 -0600 Subject: [PATCH 05/10] Create plot.py my version of plots --- testing/plot.py | 207 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 207 insertions(+) create mode 100644 testing/plot.py diff --git a/testing/plot.py b/testing/plot.py new file mode 100644 index 000000000..66f951496 --- /dev/null +++ b/testing/plot.py @@ -0,0 +1,207 @@ +import os +import numpy as np +import pandas as pd +import matplotlib.pyplot as plt +import json +import argparse +import sys + +# Safety thresholds (not used in the current plots) +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe + +def plot_cross_track_error(folder, df): + t = df['curr pt[0] vehicle time'].tolist() + cte = df['crosstrack error'].tolist() + + rmse_cte = np.sqrt(np.mean(np.array(cte)**2)) + print(f'RMSE (cte): {rmse_cte}') + + plt.figure() + plt.plot(t, cte) + plt.xlabel('$t$ (s)') + plt.ylabel('Crosstrack Error (m)') + plt.title(f'Crosstrack Error over Time (RMSE: {rmse_cte:.2f})') + plt.grid(True) + + plots_folder = os.path.join(folder, 'plots') + os.makedirs(plots_folder, exist_ok=True) + plt.savefig(os.path.join(plots_folder, 'cte.png'), dpi=600) + plt.show() + +def plot_x_vs_xd(folder, df): + t = df['curr pt[0] vehicle time'].tolist() + x = df['curr pt[0]'].tolist() + xd = df['desired pt[0]'].tolist() + + plt.figure() + plt.plot(t, x, label='Actual x') + plt.plot(t, xd, label='Desired x', linestyle='--') + + plt.xlabel('$t$ (s)') + plt.ylabel('X Coordinate (m)') + plt.title('Actual vs Desired X Coordinate Over Time') + plt.legend() + plt.grid(True) + + plots_folder = os.path.join(folder, 'plots') + os.makedirs(plots_folder, exist_ok=True) + plt.savefig(os.path.join(plots_folder, 'x_vs_xd.png'), dpi=600) + plt.show() + +def plot_y_vs_yd(folder, df): + t = df['curr pt[0] vehicle time'].tolist() + y = df['curr pt[1]'].tolist() + yd = df['desired pt[1]'].tolist() + + plt.figure() + plt.plot(t, y, label='Actual y') + plt.plot(t, yd, label='Desired y', linestyle='--') + + plt.xlabel('$t$ (s)') + plt.ylabel('Y Coordinate (m)') + plt.title('Actual vs Desired Y Coordinate Over Time') + plt.legend() + plt.grid(True) + + plots_folder = os.path.join(folder, 'plots') + os.makedirs(plots_folder, exist_ok=True) + plt.savefig(os.path.join(plots_folder, 'y_vs_yd.png'), dpi=600) + plt.show() + +def plot_x_y_vs_xd_yd(folder, df): + x = df['curr pt[0]'].tolist() + y = df['curr pt[1]'].tolist() + xd = df['desired pt[0]'].tolist() + yd = df['desired pt[1]'].tolist() + + plt.figure() + plt.plot(x, y, label='Actual Path') + plt.plot(xd, yd, label='Desired Path', linestyle='--') + + plt.xlabel('X Coordinate (m)') + plt.ylabel('Y Coordinate (m)') + plt.title('Actual vs Desired Path') + plt.legend() + plt.grid(True) + + plots_folder = os.path.join(folder, 'plots') + os.makedirs(plots_folder, exist_ok=True) + plt.savefig(os.path.join(plots_folder, 'x_y_vs_xd_yd.png'), dpi=600) + plt.show() + +def parse_behavior_log(filename): + times = [] + accelerations = [] + heading_rates = [] + + with open(filename, 'r') as f: + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + # Only add if all required fields are available + if t is not None and acceleration is not None and heading_rate is not None: + times.append(t) + accelerations.append(acceleration) + heading_rates.append(heading_rate) + + return np.array(times), np.array(accelerations), np.array(heading_rates) + +def compute_safety_factor(value, safe_thresh, unsafe_thresh): + """Computes a safety factor between 0 and 1 (for coloring in plots)""" + abs_val = abs(value) + if abs_val <= safe_thresh: + return 1.0 + elif abs_val >= unsafe_thresh: + return 0.0 + else: + return 1.0 - (abs_val - safe_thresh) / (unsafe_thresh - safe_thresh) + +def compute_derivative(times, values): + """ + Computes the derivative of array with respect to time. + Returns the time array and derivative values. + """ + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def parse_tracker_csv(filename): + """ + - Crosstrack error time (from column index 18) + - Crosstrack error (from column index 20) + """ + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + cte_time = data[:, 18] + cte = data[:, 20] + return cte_time, cte + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte): + """Plots jerk, heading acceleration, and cross-track error in subplots.""" + fig, axs = plt.subplots(3, 1, figsize=(10, 12), sharex=True) + fig.subplots_adjust(hspace=0.4) + + # Jerk plot + axs[0].plot(time_jerk, jerk, marker='o', linestyle='-', color='blue') + axs[0].set_ylabel("Jerk (m/s³)") + axs[0].set_title("Vehicle Jerk Over Time") + axs[0].grid(True) + + # Heading acceleration plot + axs[1].plot(time_heading_acc, heading_acc, marker='x', linestyle='-', color='orange') + axs[1].set_ylabel("Heading Acceleration (rad/s²)") + axs[1].set_title("Vehicle Heading Acceleration Over Time") + axs[1].grid(True) + + # Cross track error plot + axs[2].plot(cte_time, cte, marker='s', linestyle='-', color='green') + axs[2].set_xlabel("Time (s)") + axs[2].set_ylabel("Cross Track Error") + axs[2].set_title("Vehicle Cross Track Error Over Time") + axs[2].grid(True) + + plt.show() + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Plot Crosstrack Error and Trajectory from a specified log folder.') + parser.add_argument('log_folder', type=str, help='Path to the log folder containing the log files') + args = parser.parse_args() + + # CSV and JSON files from the folder + csv_file = os.path.join(args.log_folder, 'PurePursuitTrajectoryTracker_debug.csv') + behavior_file = os.path.join(args.log_folder, 'behavior.json') + + if not os.path.isfile(csv_file): + raise FileNotFoundError(f"CSV file not found in folder: {args.log_folder}") + + # Reading CSV (PurePursuitTrajectoryTracker_debug.csv) + df = pd.read_csv(csv_file) + + # Plotting Crosstrack Error and Trajectory-related metrics + plot_cross_track_error(args.log_folder, df) + plot_x_vs_xd(args.log_folder, df) + plot_y_vs_yd(args.log_folder, df) + plot_x_y_vs_xd_yd(args.log_folder, df) + + # Parsing behavior JSON and computing additional metrics + times, accelerations, heading_rates = parse_behavior_log(behavior_file) + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + cte_time, cte = parse_tracker_csv(csv_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte) + + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) + print("Max (abs) cross track error:", np.max(np.abs(cte))) From 49d1bc66139eda2d4ea1f62e4ee1c186449cd567 Mon Sep 17 00:00:00 2001 From: pravshot Date: Tue, 25 Feb 2025 14:53:36 -0600 Subject: [PATCH 06/10] updated graphs with comfort/safety scale --- testing/test_comfort_metrics.py | 161 ++++++++++++++++---------------- 1 file changed, 79 insertions(+), 82 deletions(-) diff --git a/testing/test_comfort_metrics.py b/testing/test_comfort_metrics.py index db80a3c88..b65491e66 100644 --- a/testing/test_comfort_metrics.py +++ b/testing/test_comfort_metrics.py @@ -2,16 +2,13 @@ import sys import os import matplotlib.pyplot as plt +import matplotlib.colors as mcolors import numpy as np -# Safety thresholds (not used in the current plots) -ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe -ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe -HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe -HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe +CMAP = "RdYlGn" def compute_safety_factor(value, safe_thresh, unsafe_thresh): - """Computes a safety factor between 0 and 1 (for coloring in plots)""" + """Computes a safety factor between 0(unsafe) and 1(safe)""" abs_val = abs(value) if abs_val <= safe_thresh: return 1.0 @@ -47,7 +44,7 @@ def parse_behavior_log(filename): def parse_tracker_csv(filename): """ - - Crosstrack error time (from column index 18) + - vehicle time (from column index 18) - Crosstrack error (from column index 20) - X position actual (from column index 2) - Y position actual (from column index 5) @@ -55,46 +52,27 @@ def parse_tracker_csv(filename): - Y position desired (from column index 14) """ data = np.genfromtxt(filename, delimiter=',', skip_header=1) - cte_time = data[:, 18] + vehicle_time = data[:, 18] cte = data[:, 20] x_actual, y_actual = data[:, 2], data[:, 5] x_desired, y_desired = data[:, 11], data[:, 14] - return cte_time, cte, x_actual, y_actual, x_desired, y_desired + return vehicle_time, cte, x_actual, y_actual, x_desired, y_desired - -def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, vehicle_time, cte, x_actual, y_actual, x_desired, y_desired): """Plots jerk, heading acceleration, and cross-track error in subplots.""" fig, axs = plt.subplots(2, 2, figsize=(12, 8)) - fig.subplots_adjust(hspace=0.4, wspace=0.3) - - # Jerk plot - axs[0,0].plot(time_jerk, jerk, marker='o', linestyle='-', color='blue') - axs[0,0].set_ylabel("Jerk (m/s³)") - axs[0,0].set_title("Vehicle Jerk Over Time") - axs[0,0].grid(True) - - # Heading acceleration plot - axs[0,1].plot(time_heading_acc, heading_acc, marker='x', linestyle='-', color='orange') - axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") - axs[0,1].set_title("Vehicle Heading Acceleration Over Time") - axs[0,1].grid(True) - - # Cross track error plot - axs[1,0].plot(cte_time, cte, marker='s', linestyle='-', color='green') - axs[1,0].set_xlabel("Time (s)") - axs[1,0].set_ylabel("Cross Track Error") - axs[1,0].set_title("Vehicle Cross Track Error Over Time") - axs[1,0].grid(True) - - # Position plot - axs[1,1].plot(x_actual, y_actual, marker='o', linestyle='-', color='blue', label='Actual') - axs[1,1].plot(x_desired, y_desired, marker='x', linestyle='-', color='orange', label='Desired') - axs[1,1].set_xlabel("X Position (m)") - axs[1,1].set_ylabel("Y Position (m)") - axs[1,1].set_title("Vehicle Position") - axs[1,1].legend() - axs[1,1].grid(True) + fig.subplots_adjust(hspace=0.375, wspace=0.35) + plot_jerk(axs[0,0], time_jerk, jerk) + plot_heading_acceleration(axs[0,1], time_heading_acc, heading_acc) + plot_crosstrack_error(axs[1,0], vehicle_time, cte) + plot_position(axs[1,1], x_actual, y_actual, x_desired, y_desired) + + cbar_ax = fig.add_axes([0.92, 0.2, 0.02, 0.6]) # Position for the colorbar + sm = plt.cm.ScalarMappable(cmap=CMAP) + cbar = fig.colorbar(sm, cax=cbar_ax) + cbar.set_label("Comfort/Safety Level") + plt.show() def compute_derivative(times, values): @@ -107,47 +85,65 @@ def compute_derivative(times, values): derivative = dv / dt return times[1:], derivative -def plot_jerk(time, jerk): +def plot_jerk(axis, time, jerk, safe_thresh=1.0, unsafe_thresh=2.5): """Plots vehicle jerk (rate of acceleration) vs. time""" - plt.figure(figsize=(12, 6)) - plt.plot(time, jerk, marker='o', linestyle='-', color='blue') - plt.xlabel("Time (s)") - plt.ylabel("Jerk (m/s³)") - plt.title("Vehicle Jerk Over Time") - plt.grid(True) - plt.show() + safety_scores = np.vectorize(compute_safety_factor)(jerk, safe_thresh, unsafe_thresh) + + axis.plot(time, jerk, color="black", linewidth=0.8, alpha=0.5) + scatter = axis.scatter(time, jerk, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") -def plot_heading_acceleration(time, heading_acc): - """Plots vehicle heading acceleration vs. time""" - plt.figure(figsize=(12, 6)) - plt.plot(time, heading_acc, marker='x', linestyle='-', color='orange') - plt.xlabel("Time (s)") - plt.ylabel("Heading Acceleration (rad/s²)") - plt.title("Vehicle Heading Acceleration Over Time") - plt.grid(True) - plt.show() + axis.set_xlabel("Time (s)") + axis.set_ylabel("Jerk (m/s³)") + axis.set_title("Vehicle Jerk Over Time") + axis.grid(True) + # cbar = plt.colorbar(scatter, ax=axis) + # cbar.set_label("Comfort Level") -def plot_crosstrack_error(time, cte): - """Plots vehicle cross track error vs. time""" - plt.figure(figsize=(12, 6)) - plt.plot(time, cte, marker='s', linestyle='-', color='green') - plt.xlabel("Time (s)") - plt.ylabel("Cross Track Error") - plt.title("Vehicle Cross Track Error Over Time") - plt.grid(True) - plt.show() -def plot_position(time, x_actual, y_actual, x_desired, y_desired): +def plot_heading_acceleration(axis, time, heading_acc, safe_thresh=0.0075, unsafe_thresh=0.25): + """Plots vehicle heading acceleration vs. time""" + safety_scores = np.vectorize(compute_safety_factor)(heading_acc, safe_thresh, unsafe_thresh) + + axis.plot(time, heading_acc, color="black", linewidth=0.8, alpha=0.5) + scatter = axis.scatter(time, heading_acc, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") + + axis.set_xlabel("Time (s)") + axis.set_ylabel("Heading Acceleration (rad/s²)") + axis.set_title("Vehicle Heading Acceleration Over Time") + axis.grid(True) + # cbar = plt.colorbar(scatter, ax=axis) + # cbar.set_label("Comfort Level") + +def plot_crosstrack_error(axis, time, cte, safe_thresh=0.1, unsafe_thresh=0.4): + """Plots vehicle cross track error vs. time""" + safety_scores = np.vectorize(compute_safety_factor)(cte, safe_thresh, unsafe_thresh) + + axis.plot(time, cte, color="black", linewidth=0.8, alpha=0.5) + scatter = axis.scatter(time, cte, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") + + axis.set_xlabel("Time (s)") + axis.set_ylabel("Cross Track Error") + axis.set_title("Vehicle Cross Track Error Over Time") + axis.grid(True) + # cbar = plt.colorbar(scatter, ax=axis) + # cbar.set_label("Safety Level") + +def plot_position(axis, x_actual, y_actual, x_desired, y_desired, safe_thresh=1, unsafe_thresh=2.5): """Plots vehicle actual and desired positions vs. time""" - plt.figure(figsize=(12, 6)) - plt.plot(x_actual, y_actual, marker='o', linestyle='-', color='blue', label='Actual') - plt.plot(x_desired, y_desired, marker='x', linestyle='-', color='orange', label='Desired') - plt.xlabel("X Position (m)") - plt.ylabel("Y Position (m)") - plt.title("Vehicle Position Over Time") - plt.legend() - plt.grid(True) - plt.show() + position_error = np.sqrt((x_desired - x_actual) ** 2 + (y_desired - y_actual) ** 2) + safety_scores = np.vectorize(compute_safety_factor)(position_error, safe_thresh, unsafe_thresh) + + axis.plot(y_desired, x_desired, marker='.', linestyle=':', color='blue', label='Desired') + axis.plot(y_actual, x_actual, color="black", linewidth=0.8, alpha=0.5) + scatter = axis.scatter(y_actual, x_actual, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") + + axis.set_xlabel("Y Position (m)") + axis.set_ylabel("X Position (m)") + axis.set_title("Vehicle Position vs. Desired Position") + axis.legend() + axis.grid(True) + # cbar = plt.colorbar(scatter, ax=axis) + # cbar.set_label("Safety Level") if __name__=='__main__': @@ -160,13 +156,14 @@ def plot_position(time, x_actual, y_actual, x_desired, y_desired): tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") times, accelerations, heading_rates = parse_behavior_log(behavior_file) + vehicle_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + # calculate derivatives time_jerk, jerk = compute_derivative(times, accelerations) time_heading_acc, heading_acc = compute_derivative(times, heading_rates) - cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) - - plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, vehicle_time, cte, x_actual, y_actual, x_desired, y_desired) - print("Max (abs) jerk:", np.max(np.abs(jerk))) - print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) - print("Max (abs) cross track error:", np.max(np.abs(cte))) \ No newline at end of file + print("RMS Jerk:", np.sqrt(np.mean(jerk**2)), "m/s³") + print("RMS Heading Acceleration:", np.sqrt(np.mean(heading_acc**2)), "rad/s²") + print("RMS Cross Track Error:", np.sqrt(np.mean(cte**2)), "m") + print("RMS Position Error:", np.sqrt(np.mean((x_actual - x_desired)**2 + (y_actual - y_desired)**2)), 'm') \ No newline at end of file From 176817fda79e739373c62cb99da359ea3ac2b785 Mon Sep 17 00:00:00 2001 From: pravshot Date: Tue, 25 Feb 2025 14:55:09 -0600 Subject: [PATCH 07/10] removed plot.py file --- testing/plot.py | 207 ------------------------------------------------ 1 file changed, 207 deletions(-) delete mode 100644 testing/plot.py diff --git a/testing/plot.py b/testing/plot.py deleted file mode 100644 index 66f951496..000000000 --- a/testing/plot.py +++ /dev/null @@ -1,207 +0,0 @@ -import os -import numpy as np -import pandas as pd -import matplotlib.pyplot as plt -import json -import argparse -import sys - -# Safety thresholds (not used in the current plots) -ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe -ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe -HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe -HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe - -def plot_cross_track_error(folder, df): - t = df['curr pt[0] vehicle time'].tolist() - cte = df['crosstrack error'].tolist() - - rmse_cte = np.sqrt(np.mean(np.array(cte)**2)) - print(f'RMSE (cte): {rmse_cte}') - - plt.figure() - plt.plot(t, cte) - plt.xlabel('$t$ (s)') - plt.ylabel('Crosstrack Error (m)') - plt.title(f'Crosstrack Error over Time (RMSE: {rmse_cte:.2f})') - plt.grid(True) - - plots_folder = os.path.join(folder, 'plots') - os.makedirs(plots_folder, exist_ok=True) - plt.savefig(os.path.join(plots_folder, 'cte.png'), dpi=600) - plt.show() - -def plot_x_vs_xd(folder, df): - t = df['curr pt[0] vehicle time'].tolist() - x = df['curr pt[0]'].tolist() - xd = df['desired pt[0]'].tolist() - - plt.figure() - plt.plot(t, x, label='Actual x') - plt.plot(t, xd, label='Desired x', linestyle='--') - - plt.xlabel('$t$ (s)') - plt.ylabel('X Coordinate (m)') - plt.title('Actual vs Desired X Coordinate Over Time') - plt.legend() - plt.grid(True) - - plots_folder = os.path.join(folder, 'plots') - os.makedirs(plots_folder, exist_ok=True) - plt.savefig(os.path.join(plots_folder, 'x_vs_xd.png'), dpi=600) - plt.show() - -def plot_y_vs_yd(folder, df): - t = df['curr pt[0] vehicle time'].tolist() - y = df['curr pt[1]'].tolist() - yd = df['desired pt[1]'].tolist() - - plt.figure() - plt.plot(t, y, label='Actual y') - plt.plot(t, yd, label='Desired y', linestyle='--') - - plt.xlabel('$t$ (s)') - plt.ylabel('Y Coordinate (m)') - plt.title('Actual vs Desired Y Coordinate Over Time') - plt.legend() - plt.grid(True) - - plots_folder = os.path.join(folder, 'plots') - os.makedirs(plots_folder, exist_ok=True) - plt.savefig(os.path.join(plots_folder, 'y_vs_yd.png'), dpi=600) - plt.show() - -def plot_x_y_vs_xd_yd(folder, df): - x = df['curr pt[0]'].tolist() - y = df['curr pt[1]'].tolist() - xd = df['desired pt[0]'].tolist() - yd = df['desired pt[1]'].tolist() - - plt.figure() - plt.plot(x, y, label='Actual Path') - plt.plot(xd, yd, label='Desired Path', linestyle='--') - - plt.xlabel('X Coordinate (m)') - plt.ylabel('Y Coordinate (m)') - plt.title('Actual vs Desired Path') - plt.legend() - plt.grid(True) - - plots_folder = os.path.join(folder, 'plots') - os.makedirs(plots_folder, exist_ok=True) - plt.savefig(os.path.join(plots_folder, 'x_y_vs_xd_yd.png'), dpi=600) - plt.show() - -def parse_behavior_log(filename): - times = [] - accelerations = [] - heading_rates = [] - - with open(filename, 'r') as f: - for line in f: - try: - entry = json.loads(line) - except json.JSONDecodeError: - print(f"Skipping invalid JSON line: {line.strip()}") - continue - if "vehicle" in entry: - t = entry.get("time") - vehicle_data = entry["vehicle"].get("data", {}) - acceleration = vehicle_data.get("acceleration") - heading_rate = vehicle_data.get("heading_rate") - # Only add if all required fields are available - if t is not None and acceleration is not None and heading_rate is not None: - times.append(t) - accelerations.append(acceleration) - heading_rates.append(heading_rate) - - return np.array(times), np.array(accelerations), np.array(heading_rates) - -def compute_safety_factor(value, safe_thresh, unsafe_thresh): - """Computes a safety factor between 0 and 1 (for coloring in plots)""" - abs_val = abs(value) - if abs_val <= safe_thresh: - return 1.0 - elif abs_val >= unsafe_thresh: - return 0.0 - else: - return 1.0 - (abs_val - safe_thresh) / (unsafe_thresh - safe_thresh) - -def compute_derivative(times, values): - """ - Computes the derivative of array with respect to time. - Returns the time array and derivative values. - """ - dt = np.diff(times) - dv = np.diff(values) - derivative = dv / dt - return times[1:], derivative - -def parse_tracker_csv(filename): - """ - - Crosstrack error time (from column index 18) - - Crosstrack error (from column index 20) - """ - data = np.genfromtxt(filename, delimiter=',', skip_header=1) - cte_time = data[:, 18] - cte = data[:, 20] - return cte_time, cte - -def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte): - """Plots jerk, heading acceleration, and cross-track error in subplots.""" - fig, axs = plt.subplots(3, 1, figsize=(10, 12), sharex=True) - fig.subplots_adjust(hspace=0.4) - - # Jerk plot - axs[0].plot(time_jerk, jerk, marker='o', linestyle='-', color='blue') - axs[0].set_ylabel("Jerk (m/s³)") - axs[0].set_title("Vehicle Jerk Over Time") - axs[0].grid(True) - - # Heading acceleration plot - axs[1].plot(time_heading_acc, heading_acc, marker='x', linestyle='-', color='orange') - axs[1].set_ylabel("Heading Acceleration (rad/s²)") - axs[1].set_title("Vehicle Heading Acceleration Over Time") - axs[1].grid(True) - - # Cross track error plot - axs[2].plot(cte_time, cte, marker='s', linestyle='-', color='green') - axs[2].set_xlabel("Time (s)") - axs[2].set_ylabel("Cross Track Error") - axs[2].set_title("Vehicle Cross Track Error Over Time") - axs[2].grid(True) - - plt.show() - -if __name__ == '__main__': - parser = argparse.ArgumentParser(description='Plot Crosstrack Error and Trajectory from a specified log folder.') - parser.add_argument('log_folder', type=str, help='Path to the log folder containing the log files') - args = parser.parse_args() - - # CSV and JSON files from the folder - csv_file = os.path.join(args.log_folder, 'PurePursuitTrajectoryTracker_debug.csv') - behavior_file = os.path.join(args.log_folder, 'behavior.json') - - if not os.path.isfile(csv_file): - raise FileNotFoundError(f"CSV file not found in folder: {args.log_folder}") - - # Reading CSV (PurePursuitTrajectoryTracker_debug.csv) - df = pd.read_csv(csv_file) - - # Plotting Crosstrack Error and Trajectory-related metrics - plot_cross_track_error(args.log_folder, df) - plot_x_vs_xd(args.log_folder, df) - plot_y_vs_yd(args.log_folder, df) - plot_x_y_vs_xd_yd(args.log_folder, df) - - # Parsing behavior JSON and computing additional metrics - times, accelerations, heading_rates = parse_behavior_log(behavior_file) - time_jerk, jerk = compute_derivative(times, accelerations) - time_heading_acc, heading_acc = compute_derivative(times, heading_rates) - cte_time, cte = parse_tracker_csv(csv_file) - - plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte) - - print("Max (abs) jerk:", np.max(np.abs(jerk))) - print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) - print("Max (abs) cross track error:", np.max(np.abs(cte))) From c14d7af6bafc17259b69c15af7ffb441b0c759ce Mon Sep 17 00:00:00 2001 From: pravshot Date: Tue, 25 Feb 2025 17:57:46 -0600 Subject: [PATCH 08/10] added pedestrian distance metric and cleaned code --- testing/test_comfort_metrics.py | 161 ++++++++++++++++++++++---------- 1 file changed, 113 insertions(+), 48 deletions(-) diff --git a/testing/test_comfort_metrics.py b/testing/test_comfort_metrics.py index b65491e66..1baf26a58 100644 --- a/testing/test_comfort_metrics.py +++ b/testing/test_comfort_metrics.py @@ -1,27 +1,46 @@ -import json import sys import os +sys.path.append(os.getcwd()) + +from GEMstack.state import AgentEnum +import json import matplotlib.pyplot as plt -import matplotlib.colors as mcolors import numpy as np CMAP = "RdYlGn" -def compute_safety_factor(value, safe_thresh, unsafe_thresh): - """Computes a safety factor between 0(unsafe) and 1(safe)""" +def compute_safety_factor(value, safe_thresh, unsafe_thresh, flip=False): + """ + Computes a safety factor between 0(unsafe) and 1(safe) + If flip is True, the threshold bounds are reversed. + """ abs_val = abs(value) if abs_val <= safe_thresh: - return 1.0 + factor = 1.0 elif abs_val >= unsafe_thresh: - return 0.0 + factor = 0.0 else: - return 1.0 - (abs_val - safe_thresh) / (unsafe_thresh - safe_thresh) + factor = 1.0 - (abs_val - safe_thresh) / (unsafe_thresh - safe_thresh) + + if flip: + return 1.0 - factor + return factor def parse_behavior_log(filename): + """ + Parses the behavior log file and extracts the following data: + - vehicle time + - vehicle acceleration + - vehicle heading rate + - pedestrian time + - pedestrian distance + """ times = [] accelerations = [] heading_rates = [] - + pedestrian_times = [] + pedestrian_distances = [] + with open(filename, 'r') as f: for line in f: try: @@ -29,21 +48,38 @@ def parse_behavior_log(filename): except json.JSONDecodeError: print(f"Skipping invalid JSON line: {line.strip()}") continue + # Process vehicle state data if "vehicle" in entry: t = entry.get("time") vehicle_data = entry["vehicle"].get("data", {}) acceleration = vehicle_data.get("acceleration") heading_rate = vehicle_data.get("heading_rate") - # Only add if all required fields are available - if t is not None and acceleration is not None and heading_rate is not None: + # Only add if all fields are available + if None not in (t, acceleration, heading_rate): times.append(t) accelerations.append(acceleration) heading_rates.append(heading_rate) + # Process agent state data + if "agents" in entry: + for agent in entry["agents"].values(): + agent_data = agent.get("data", {}) + # Check if the agent is a pedestrian + if agent_data.get("type") == AgentEnum.PEDESTRIAN.value: + t = entry.get("time") + pose = agent_data.get("pose", {}) + x_agent = pose.get("x") + y_agent = pose.get("y") + if None not in (t, x_agent, y_agent): + pedestrian_times.append(t) + dist = np.sqrt(x_agent**2 + y_agent**2) + pedestrian_distances.append(dist) - return np.array(times), np.array(accelerations), np.array(heading_rates) + return (np.array(times), np.array(accelerations), np.array(heading_rates), + np.array(pedestrian_times), np.array(pedestrian_distances)) def parse_tracker_csv(filename): """ + Parses the pure pursuit tracker log file and extracts the following data: - vehicle time (from column index 18) - Crosstrack error (from column index 20) - X position actual (from column index 2) @@ -58,23 +94,6 @@ def parse_tracker_csv(filename): x_desired, y_desired = data[:, 11], data[:, 14] return vehicle_time, cte, x_actual, y_actual, x_desired, y_desired -def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, vehicle_time, cte, x_actual, y_actual, x_desired, y_desired): - """Plots jerk, heading acceleration, and cross-track error in subplots.""" - fig, axs = plt.subplots(2, 2, figsize=(12, 8)) - fig.subplots_adjust(hspace=0.375, wspace=0.35) - - plot_jerk(axs[0,0], time_jerk, jerk) - plot_heading_acceleration(axs[0,1], time_heading_acc, heading_acc) - plot_crosstrack_error(axs[1,0], vehicle_time, cte) - plot_position(axs[1,1], x_actual, y_actual, x_desired, y_desired) - - cbar_ax = fig.add_axes([0.92, 0.2, 0.02, 0.6]) # Position for the colorbar - sm = plt.cm.ScalarMappable(cmap=CMAP) - cbar = fig.colorbar(sm, cax=cbar_ax) - cbar.set_label("Comfort/Safety Level") - - plt.show() - def compute_derivative(times, values): """ Computes the derivative of array with respect to time. @@ -85,48 +104,66 @@ def compute_derivative(times, values): derivative = dv / dt return times[1:], derivative +def add_safety_colorbar(figure): + """Adds a colorbar to the right of the figure""" + cbar_ax = figure.add_axes([0.92, 0.2, 0.02, 0.6]) + sm = plt.cm.ScalarMappable(cmap=CMAP) + cbar = figure.colorbar(sm, cax=cbar_ax) + cbar.set_label("Comfort/Safety Level") + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, vehicle_time, cte, + x_actual, y_actual, x_desired, y_desired, pedestrian_times, pedestrian_distances): + """Plots all metrics in 2x3 grid""" + fig, axs = plt.subplots(2, 3, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.375, wspace=0.35) + axs[1,2].axis('off') + + plot_jerk(axs[0,0], time_jerk, jerk) + plot_heading_acceleration(axs[0,1], time_heading_acc, heading_acc) + plot_crosstrack_error(axs[1,0], vehicle_time, cte) + plot_position(axs[1,1], x_actual, y_actual, x_desired, y_desired) + plot_pedestrian_dist(axs[0,2], pedestrian_times, pedestrian_distances) + + # Colorbar on the right side + add_safety_colorbar(fig) + + plt.show() + def plot_jerk(axis, time, jerk, safe_thresh=1.0, unsafe_thresh=2.5): """Plots vehicle jerk (rate of acceleration) vs. time""" safety_scores = np.vectorize(compute_safety_factor)(jerk, safe_thresh, unsafe_thresh) axis.plot(time, jerk, color="black", linewidth=0.8, alpha=0.5) - scatter = axis.scatter(time, jerk, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") + axis.scatter(time, jerk, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") axis.set_xlabel("Time (s)") axis.set_ylabel("Jerk (m/s³)") axis.set_title("Vehicle Jerk Over Time") axis.grid(True) - # cbar = plt.colorbar(scatter, ax=axis) - # cbar.set_label("Comfort Level") - def plot_heading_acceleration(axis, time, heading_acc, safe_thresh=0.0075, unsafe_thresh=0.25): """Plots vehicle heading acceleration vs. time""" safety_scores = np.vectorize(compute_safety_factor)(heading_acc, safe_thresh, unsafe_thresh) axis.plot(time, heading_acc, color="black", linewidth=0.8, alpha=0.5) - scatter = axis.scatter(time, heading_acc, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") + axis.scatter(time, heading_acc, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") axis.set_xlabel("Time (s)") axis.set_ylabel("Heading Acceleration (rad/s²)") axis.set_title("Vehicle Heading Acceleration Over Time") axis.grid(True) - # cbar = plt.colorbar(scatter, ax=axis) - # cbar.set_label("Comfort Level") def plot_crosstrack_error(axis, time, cte, safe_thresh=0.1, unsafe_thresh=0.4): """Plots vehicle cross track error vs. time""" safety_scores = np.vectorize(compute_safety_factor)(cte, safe_thresh, unsafe_thresh) axis.plot(time, cte, color="black", linewidth=0.8, alpha=0.5) - scatter = axis.scatter(time, cte, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") + axis.scatter(time, cte, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") axis.set_xlabel("Time (s)") axis.set_ylabel("Cross Track Error") axis.set_title("Vehicle Cross Track Error Over Time") axis.grid(True) - # cbar = plt.colorbar(scatter, ax=axis) - # cbar.set_label("Safety Level") def plot_position(axis, x_actual, y_actual, x_desired, y_desired, safe_thresh=1, unsafe_thresh=2.5): """Plots vehicle actual and desired positions vs. time""" @@ -135,16 +172,24 @@ def plot_position(axis, x_actual, y_actual, x_desired, y_desired, safe_thresh=1, axis.plot(y_desired, x_desired, marker='.', linestyle=':', color='blue', label='Desired') axis.plot(y_actual, x_actual, color="black", linewidth=0.8, alpha=0.5) - scatter = axis.scatter(y_actual, x_actual, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") + axis.scatter(y_actual, x_actual, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") axis.set_xlabel("Y Position (m)") axis.set_ylabel("X Position (m)") axis.set_title("Vehicle Position vs. Desired Position") axis.legend() axis.grid(True) - # cbar = plt.colorbar(scatter, ax=axis) - # cbar.set_label("Safety Level") +def plot_pedestrian_dist(axis, pedestrian_times, pedestrian_distances, safe_thresh=5.0, unsafe_thresh=2.0): + """Plots pedestrian distance to vehicle vs. time""" + safety_scores = np.vectorize(compute_safety_factor)(pedestrian_distances, safe_thresh, unsafe_thresh, flip=True) + axis.plot(pedestrian_times, pedestrian_distances, color="black", linewidth=0.8, alpha=0.5) + axis.scatter(pedestrian_times, pedestrian_distances, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") + + axis.set_xlabel("Time (s)") + axis.set_ylabel("Pedestrian Distance (m)") + axis.set_title("Pedestrian Distance Over Time") + axis.grid(True) if __name__=='__main__': if len(sys.argv) != 2: @@ -155,15 +200,35 @@ def plot_position(axis, x_actual, y_actual, x_desired, y_desired, safe_thresh=1, behavior_file = os.path.join(log_dir, "behavior.json") tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") - times, accelerations, heading_rates = parse_behavior_log(behavior_file) - vehicle_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) - # calculate derivatives + # if behavior.json doesn't exist, print error and exit + if not os.path.exists(behavior_file): + print("Error: behavior.json file is missing in log folder.") + sys.exit(1) + + # Parse behavior log file and compute metrics + times, accelerations, heading_rates, ped_times, ped_distances = parse_behavior_log(behavior_file) time_jerk, jerk = compute_derivative(times, accelerations) time_heading_acc, heading_acc = compute_derivative(times, heading_rates) - plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, vehicle_time, cte, x_actual, y_actual, x_desired, y_desired) - + # Pure pursuit tracker file exists: parse and plot all metrics + if os.path.exists(tracker_file): + vehicle_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, vehicle_time, cte, + x_actual, y_actual, x_desired, y_desired, ped_times, ped_distances) + + print("RMS Cross Track Error:", np.sqrt(np.mean(cte**2)), "m") + print("RMS Position Error:", np.sqrt(np.mean((x_actual - x_desired)**2 + (y_actual - y_desired)**2)), 'm') + # Pure pursuit tracker file is missing: plot only behavior.json metrics + else: + print("Tracker file is missing. Skipping cross track error and vehicle position plots.") + # Plot only jerk, heading acceleration, and pedestrian distance + fig, axs = plt.subplots(1, 3, figsize=(12, 4)) + plot_jerk(axs[0], time_jerk, jerk) + plot_heading_acceleration(axs[1], time_heading_acc, heading_acc) + plot_pedestrian_dist(axs[2], ped_times, ped_distances) + add_safety_colorbar(fig) + plt.show() + print("RMS Jerk:", np.sqrt(np.mean(jerk**2)), "m/s³") print("RMS Heading Acceleration:", np.sqrt(np.mean(heading_acc**2)), "rad/s²") - print("RMS Cross Track Error:", np.sqrt(np.mean(cte**2)), "m") - print("RMS Position Error:", np.sqrt(np.mean((x_actual - x_desired)**2 + (y_actual - y_desired)**2)), 'm') \ No newline at end of file + print("Minimum Pedestrian Distance:", np.min(ped_distances), "m") \ No newline at end of file From ec44350e9cc3dc0ecec3e5c3ef52f0d7059ae821 Mon Sep 17 00:00:00 2001 From: pravshot Date: Tue, 25 Feb 2025 18:27:28 -0600 Subject: [PATCH 09/10] check for 0 pedestrians in log --- testing/test_comfort_metrics.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/testing/test_comfort_metrics.py b/testing/test_comfort_metrics.py index 1baf26a58..f0b162fb3 100644 --- a/testing/test_comfort_metrics.py +++ b/testing/test_comfort_metrics.py @@ -182,9 +182,10 @@ def plot_position(axis, x_actual, y_actual, x_desired, y_desired, safe_thresh=1, def plot_pedestrian_dist(axis, pedestrian_times, pedestrian_distances, safe_thresh=5.0, unsafe_thresh=2.0): """Plots pedestrian distance to vehicle vs. time""" - safety_scores = np.vectorize(compute_safety_factor)(pedestrian_distances, safe_thresh, unsafe_thresh, flip=True) - axis.plot(pedestrian_times, pedestrian_distances, color="black", linewidth=0.8, alpha=0.5) - axis.scatter(pedestrian_times, pedestrian_distances, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") + if len(pedestrian_times) > 0: + safety_scores = np.vectorize(compute_safety_factor)(pedestrian_distances, safe_thresh, unsafe_thresh, flip=True) + axis.plot(pedestrian_times, pedestrian_distances, color="black", linewidth=0.8, alpha=0.5) + axis.scatter(pedestrian_times, pedestrian_distances, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black") axis.set_xlabel("Time (s)") axis.set_ylabel("Pedestrian Distance (m)") @@ -231,4 +232,5 @@ def plot_pedestrian_dist(axis, pedestrian_times, pedestrian_distances, safe_thre print("RMS Jerk:", np.sqrt(np.mean(jerk**2)), "m/s³") print("RMS Heading Acceleration:", np.sqrt(np.mean(heading_acc**2)), "rad/s²") - print("Minimum Pedestrian Distance:", np.min(ped_distances), "m") \ No newline at end of file + if len(ped_distances) > 0: + print("Minimum Pedestrian Distance:", np.min(ped_distances), "m") \ No newline at end of file From 534436ff76ea3d38c1fb40f3d130677c60511c03 Mon Sep 17 00:00:00 2001 From: pravshot Date: Tue, 25 Feb 2025 18:39:40 -0600 Subject: [PATCH 10/10] added documentation --- ...afety and Comfort Metrics Documentation.md | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 docs/Safety and Comfort Metrics Documentation.md diff --git a/docs/Safety and Comfort Metrics Documentation.md b/docs/Safety and Comfort Metrics Documentation.md new file mode 100644 index 000000000..ea0e2f22f --- /dev/null +++ b/docs/Safety and Comfort Metrics Documentation.md @@ -0,0 +1,39 @@ +# Test Safety and Comfort Metrics Documentation + +## Files/Scripts +- `testing/test_comfort_metrics.py` + +## Purpose + +This script analyzes log files and reports vehicle comfort and pedestrian safety with plots. It extracts: + +- **Vehicle Data:** Time, acceleration, heading rate from `behavior.json`. +- **Pedestrian Data:** Time and pedestrian distance to car from `behavior.json`. +- **Pure Pursuit Tracker Data (Optional):** Vehicle time, cross-track error, and position (actual vs. desired) from `PurePursuitTrajectoryTracker_debug.csv`. + +The script will include 5 plots: +- Vehicle jerk vs. time. +- Vehicle heading acceleration vs. time. +- Vehicle cross-track error vs. time. +- Vehicle actual vs. desired position. +- Pedestrian distance vs. time. + +The script also prints key metrics: +- RMS Jerk +- RMS Heading acceleration +- RMS Cross track error +- RMS Position error +- Minimum pedestrian distance to car + +## Usage + +1. **Check log directory:** + - Ensure your log directory contains `behavior.json` (required). + - Optionally include `PurePursuitTrajectoryTracker_debug.csv` (if missing, some plots are skipped). + +2. **Run the script:** + + ```bash + python test_comfort_metrics.py + ``` + Replace `` with the path to the folder containing the log files. \ No newline at end of file