updated metric script to show acceleration and remove dot plots

Author: pravshot

testing/test_comfort_metrics.py

@@ -6,6 +6,8 @@
 import json
 import matplotlib.pyplot as plt
 import numpy as np
+from matplotlib.collections import LineCollection
+from matplotlib.colors import Normalize
 
 CMAP = "RdYlGn"
 
@@ -21,7 +23,7 @@ def compute_safety_factor(value, safe_thresh, unsafe_thresh, flip=False):
         factor = 0.0
     else:
         factor = 1.0 - (abs_val - safe_thresh) / (unsafe_thresh - safe_thresh)
-    
+
     if flip:
         return 1.0 - factor
     return factor
@@ -40,7 +42,7 @@ def parse_behavior_log(filename):
     heading_rates = []
     pedestrian_times = []
     pedestrian_distances = []
-    
+
     with open(filename, 'r') as f:
         for line in f:
             try:
@@ -73,8 +75,8 @@ def parse_behavior_log(filename):
                             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):
@@ -111,69 +113,137 @@ def add_safety_colorbar(figure):
     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, 
+def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, time_accel, accel, 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')
+    # 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_acceleration(axs[0,2], time_accel, accel)
     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)
-    
+    plot_pedestrian_dist(axs[1,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)
-    axis.scatter(time, jerk, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black")
+    """
+    Plots vehicle jerk (rate of acceleration) vs. time as a colored line.
+    """
+    # compute safety scores
+    safety = np.vectorize(compute_safety_factor)(jerk, safe_thresh, unsafe_thresh)
+
+    # build line segments
+    points   = np.vstack([time, jerk]).T.reshape(-1,1,2)
+    segments = np.concatenate([points[:-1], points[1:]], axis=1)
+
+    # create colored LineCollection
+    norm = Normalize(vmin=0, vmax=1)
+    lc   = LineCollection(segments, cmap=CMAP, norm=norm, linewidth=1.5)
+    lc.set_array(safety[:-1])
+    axis.add_collection(lc)
 
+    # set limits & labels
+    axis.set_xlim(time.min(), time.max())
+    axis.set_ylim(jerk.min(), jerk.max())
     axis.set_xlabel("Time (s)")
     axis.set_ylabel("Jerk (m/s³)")
     axis.set_title("Vehicle Jerk Over Time")
     axis.grid(True)
 
+def plot_acceleration(axis, time, acceleration, safe_thresh=0.5, unsafe_thresh=1.5):
+    """
+    Plots vehicle acceleration vs. time as a colored line.
+    """
+    safety = np.vectorize(compute_safety_factor)(acceleration, safe_thresh, unsafe_thresh)
+
+    points   = np.vstack([time, acceleration]).T.reshape(-1,1,2)
+    segments = np.concatenate([points[:-1], points[1:]], axis=1)
+
+    norm = Normalize(vmin=0, vmax=1)
+    lc   = LineCollection(segments, cmap=CMAP, norm=norm, linewidth=1.5)
+    lc.set_array(safety[:-1])
+    axis.add_collection(lc)
+
+    axis.set_xlim(time.min(), time.max())
+    axis.set_ylim(acceleration.min(), acceleration.max())
+    axis.set_xlabel("Time (s)")
+    axis.set_ylabel("Acceleration (m/s²)")
+    axis.set_title("Vehicle Acceleration Over Time")
+    axis.grid(True)
+
 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)
-    axis.scatter(time, heading_acc, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black")
-    
+    """
+    Plots vehicle heading acceleration vs. time as a colored line.
+    """
+    safety = np.vectorize(compute_safety_factor)(heading_acc, safe_thresh, unsafe_thresh)
+
+    points   = np.vstack([time, heading_acc]).T.reshape(-1,1,2)
+    segments = np.concatenate([points[:-1], points[1:]], axis=1)
+
+    norm = Normalize(vmin=0, vmax=1)
+    lc   = LineCollection(segments, cmap=CMAP, norm=norm, linewidth=1.5)
+    lc.set_array(safety[:-1])
+    axis.add_collection(lc)
+
+    axis.set_xlim(time.min(), time.max())
+    axis.set_ylim(heading_acc.min(), heading_acc.max())
     axis.set_xlabel("Time (s)")
     axis.set_ylabel("Heading Acceleration (rad/s²)")
     axis.set_title("Vehicle Heading Acceleration Over Time")
     axis.grid(True)
 
 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)
-    axis.scatter(time, cte, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black")
-    
+    """
+    Plots vehicle cross track error vs. time as a colored line.
+    """
+    # compute safety scores for each point
+    safety = np.vectorize(compute_safety_factor)(cte, safe_thresh, unsafe_thresh)
+
+    points = np.vstack([time, cte]).T.reshape(-1,1,2)
+    segments = np.concatenate([points[:-1], points[1:]], axis=1)
+
+    lc = LineCollection(segments, cmap=CMAP, norm=Normalize(0,1))
+    lc.set_array(safety[:-1])
+    lc.set_linewidth(2.0)
+    axis.add_collection(lc)
+
+    # set axis limits
+    axis.set_xlim(time.min(), time.max())
+    axis.set_ylim(cte.min(), cte.max())
+
     axis.set_xlabel("Time (s)")
     axis.set_ylabel("Cross Track Error")
     axis.set_title("Vehicle Cross Track Error Over Time")
     axis.grid(True)
 
-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"""
-    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)
-    axis.scatter(y_actual, x_actual, c=safety_scores, cmap=CMAP, vmin=0, vmax=1, edgecolors="black")
-    
+def plot_position(axis, x_actual, y_actual, x_desired, y_desired,
+                  safe_thresh=1.0, unsafe_thresh=2.5):
+    """
+    Plots vehicle actual and desired positions
+    """
+    # compute positional error and safety at each point
+    pos_error = np.sqrt((x_desired - x_actual)**2 + (y_desired - y_actual)**2)
+    safety = np.vectorize(compute_safety_factor)(pos_error, safe_thresh, unsafe_thresh)
+
+    # actual path segments
+    actual_pts = np.vstack([y_actual, x_actual]).T.reshape(-1,1,2)
+    actual_segs = np.concatenate([actual_pts[:-1], actual_pts[1:]], axis=1)
+    norm = Normalize(vmin=0, vmax=1)
+    lc_actual = LineCollection(actual_segs, cmap=CMAP, norm=norm)
+    lc_actual.set_array(safety[:-1])
+    lc_actual.set_linewidth(2.0)
+    axis.add_collection(lc_actual)
+
+    # desired path as dashed gray line
+    axis.plot(y_desired, x_desired,
+              linestyle='--', linewidth=1.5, color='gray', label='Desired')
+
     axis.set_xlabel("Y Position (m)")
     axis.set_ylabel("X Position (m)")
     axis.set_title("Vehicle Position vs. Desired Position")
@@ -186,7 +256,7 @@ def plot_pedestrian_dist(axis, pedestrian_times, pedestrian_distances, safe_thre
         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")
@@ -196,41 +266,43 @@ def plot_pedestrian_dist(axis, pedestrian_times, pedestrian_distances, safe_thre
     if len(sys.argv) != 2:
         print("Usage: python test_comfort_metrics.py <log_directory>")
         sys.exit(1)
-    
+
     log_dir = sys.argv[1]
     behavior_file = os.path.join(log_dir, "behavior.json")
     tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv")
-    
+
     # 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)
-    
+
     # 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, 
+        plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, times, accelerations, 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)
+        fig, axs = plt.subplots(2, 2, figsize=(12, 4))
+        plot_jerk(axs[0, 0], time_jerk, jerk)
+        plot_heading_acceleration(axs[0, 1], time_heading_acc, heading_acc)
+        plot_acceleration(axs[1, 0], times, accelerations)
+        plot_pedestrian_dist(axs[1, 1], 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²")
     if len(ped_distances) > 0:
-        print("Minimum Pedestrian Distance:", np.min(ped_distances), "m")
+        print("Minimum Pedestrian Distance:", np.min(ped_distances), "m")