Infra a auto plot

GEMstack/onboard/execution/entrypoint.py

@@ -122,17 +122,19 @@ def caution_callback(k,variant):
 
     #configure logging
     if log_settings:
-        topfolder = log_settings.get('log','logs')
+        topfolder = log_settings.get('folder','logs')
         prefix = log_settings.get('prefix','')
         from datetime import datetime
         default_suffix = datetime.now().strftime('%Y-%m-%d_%H-%M-%S')
         suffix = log_settings.get('suffix',default_suffix)
         logfolder = os.path.join(topfolder,prefix+suffix)
         print(EXECUTION_PREFIX,"Logging to",logfolder)
+        auto_plot = log_settings.get("auto_plot", False)
         os.makedirs(logfolder,exist_ok=True)
 
         #configure logging for components
         mission_executor.set_log_folder(logfolder)
+        mission_executor.set_auto_plot(auto_plot)
         #configure ROS logging
         log_topics = log_settings.get('ros_topics',[])
         rosbag_options = log_settings.get('rosbag_options','')

GEMstack/onboard/execution/execution.py

@@ -424,6 +424,9 @@ def add_pipeline(self,name : str, perception : Dict[str,ComponentExecutor], plan
 
     def set_log_folder(self,folder : str):
         self.logging_manager.set_log_folder(folder)
+
+    def set_auto_plot(self,enabled : bool):
+        self.logging_manager.set_auto_plot(enabled)
 
     def log_vehicle_interface(self,enabled=True):
         """Indicates that the vehicle interface should be logged"""

GEMstack/onboard/execution/logging.py

@@ -1,6 +1,6 @@
 from __future__ import annotations
 from ..component import Component
-from ...utils import serialization,logging,config,settings
+from ...utils import serialization,logging,config,settings,log_plot
 from typing import List,Optional,Dict,Set,Any
 import time
 import datetime
@@ -27,6 +27,10 @@ def __init__(self):
         self.vehicle_time = None
         self.start_vehicle_time = None
         self.debug_messages = {}
+        self.auto_plot = False
+
+    def set_auto_plot(self, auto_plot : bool) -> None:
+        self.auto_plot = auto_plot
 
     def logging(self) -> bool:
         return self.log_folder is not None
@@ -264,6 +268,8 @@ def log_component_stderr(self, component : str, msg : List[str]) -> None:
 
     def close(self):
         self.dump_debug()
+        if self.auto_plot:
+            log_plot.main(self.log_folder)
         self.debug_messages = {}
         if self.rosbag_process is not None:
             out,err = self.rosbag_process.communicate()  # Will block 

GEMstack/utils/log_plot.py

@@ -0,0 +1,148 @@
+import matplotlib.pyplot as plt
+import pandas as pd
+import os
+import numpy as np
+
+# Most of the code in this file is copied from ./tuning_logs/log_plot.py by Mikayel Aramyan (mikayel2)
+
+def create_plot(t, actual, desired, xlabel, ylabel, title, legend, save_path=None):
+    plt.figure()
+    plt.plot(t, actual)
+    plt.plot(t, desired)
+    overshoot = np.max(np.array(actual) - np.array(desired))
+    rmse = np.sqrt(np.mean((np.array(actual) - np.array(desired))**2))
+    print(f'{title} - Maximum Overshoot: {overshoot}, RMSE: {rmse}')
+    plt.xlabel(xlabel)
+    plt.ylabel(ylabel)
+    plt.title(f'{title} (Max Overshoot: {overshoot:.2f}, RMSE: {rmse:.2f})')
+    plt.legend(legend)
+    plt.grid(True)
+    if save_path:
+        plt.savefig(save_path, dpi=600)
+        plt.close()
+
+def create_error_plot(t, error, xlabel, ylabel, title, save_path=None):
+    plt.figure()
+    plt.plot(t, error)
+    plt.xlabel(xlabel)
+    plt.ylabel(ylabel)
+    plt.title(title)
+    plt.grid(True)
+    if save_path:
+        plt.savefig(save_path, dpi=600)
+        plt.close()
+
+def main(log_folder):
+    if log_folder is None:
+        return
+    df = pd.read_csv(log_folder + '/PurePursuitTrajectoryTracker_debug.csv')
+    save_figures =True
+
+    plots_folder = os.path.join(log_folder, 'plots')
+    os.makedirs(plots_folder, exist_ok=True)
+
+    t = df['curr pt[0] vehicle time'].tolist()
+    x = df['curr pt[0]'].tolist()
+    y = df['curr pt[1]'].tolist()
+    v = df['current speed (m/s)'].tolist()
+    yaw = df['current yaw (rad)'].tolist()
+
+    xd = df['desired pt[0]'].tolist()
+    yd = df['desired pt[1]'].tolist()
+    vd = df['desired speed (m/s)'].tolist()
+    yawd = df['desired yaw (rad)'].tolist()
+
+    cte = df['crosstrack error'].tolist()
+    front_wheel_angle = df['front wheel angle (rad)'].tolist()
+    accel = df['output accel (m/s^2)'].tolist()
+
+
+
+    rmse_cte = np.sqrt(np.mean(np.array(cte)**2))
+    print(f'RMSE (cte): {rmse_cte}')
+
+    max_accel_error = np.max((np.array(accel) - 1.0)**2)
+    print(f'Maximum (acceleration - 1.0)^2: {max_accel_error}')
+
+    rms_forward_acceleration = np.sqrt(np.mean(np.array(accel)**2))
+    print(f'RMS of Acceleration: {rms_forward_acceleration}')
+
+    dt = np.mean(np.diff(t))
+    jerk = np.gradient(np.array(accel), dt)
+    rms_jerk = np.sqrt(np.mean(jerk**2))
+    print(f'RMS of Jerk: {rms_jerk}')
+
+    speed_error = np.array(v) - np.array(vd)
+    rms_speed_error = np.sqrt(np.mean(speed_error**2))
+    print(f'RMS of Speed Error: {rms_speed_error}')
+
+    angular_velocity = np.gradient(front_wheel_angle, dt)
+    angular_acceleration = np.gradient(angular_velocity, dt)
+    angular_jerk = np.gradient(angular_acceleration, dt)
+    rms_angular_acceleration = np.sqrt(np.mean(angular_acceleration**2))
+    rms_angular_jerk = np.sqrt(np.mean(angular_jerk**2))
+    print(f'RMS of Angular Acceleration: {rms_angular_acceleration}')
+    print(f'RMS of Angular Jerk: {rms_angular_jerk}')
+
+    yaw_error = np.array(yaw) - np.array(yawd)
+    rms_yaw_error = np.sqrt(np.mean(yaw_error**2))
+    print(f'RMS of Yaw Error: {rms_yaw_error}')
+
+    w1, w2, w3 = 0.5, 0.3, 0.2
+    comfort_index = w1 * rms_forward_acceleration + w2 * rms_jerk + w3 * rms_speed_error
+    print(f'===== Comfort Index: {comfort_index:.2f} =====')
+
+    w1, w2, w3, w4, w5, w6, w7 = 0.2, 0.1, 0.15, 0.2, 0.15, 0.2, 0.1
+    safety_index = (w1 * rmse_cte + w2 * rms_angular_jerk + w3 * rms_angular_acceleration + 
+                    w4 * rms_forward_acceleration + w5 * rms_jerk + w6 * rms_speed_error + 
+                    w7 * rms_yaw_error)
+    print(f'=====  Safety Index: {safety_index:.2f} =====')
+
+    create_plot(t, y, yd, '$t$ (s)', '$y(t)$, $y_{d}(t)$ (m)', 'Actual and Desired y', ['Actual $y(t)$', 'Desired $y_{d}(t)$'], os.path.join(plots_folder, 'y_vs_yd.png') if save_figures else None)
+    create_error_plot(t, np.array(y) - np.array(yd), '$t$ (s)', 'Error in $y(t)$ (m)', 'Error between Actual and Desired $y(t)$', os.path.join(plots_folder, 'error_y.png') if save_figures else None)
+
+    create_plot(t, x, xd, '$t$ (s)', '$x(t)$, $x_{d}(t)$ (m)', 'Actual and Desired x', ['Actual $x(t)$', 'Desired $x_{d}(t)$'], os.path.join(plots_folder, 'x_vs_xd.png') if save_figures else None)
+    create_error_plot(t, np.array(x) - np.array(xd), '$t$ (s)', 'Error in $x(t)$ (m)', 'Error between Actual and Desired $x(t)$', os.path.join(plots_folder, 'error_x.png') if save_figures else None)
+
+    create_plot(t, v, vd, '$t$ (s)', '$v(t)$, $v_{d}(t)$ (m/s)', 'Actual and Desired v', ['Actual $v(t)$', 'Desired $v_{d}(t)$'], os.path.join(plots_folder, 'v_vs_vd.png') if save_figures else None)
+    create_error_plot(t, np.array(v) - np.array(vd), '$t$ (s)', 'Error in $v(t)$ (m/s)', 'Error between Actual and Desired $v(t)$', os.path.join(plots_folder, 'error_v.png') if save_figures else None)
+
+    plt.figure()
+    plt.plot(t, cte)
+    rmse_cte = np.sqrt(np.mean(np.array(cte)**2))
+    print(f'RMSE (cte): {rmse_cte}')
+    plt.xlabel('$t$ (s)')
+    plt.ylabel('Crosstrack Error (m)')
+    plt.title(f'Crosstrack Error over Time (RMSE: {rmse_cte:.2f})')
+    plt.grid(True)
+    if save_figures:
+        plt.savefig(os.path.join(plots_folder, 'cte.png'), dpi=600)
+        plt.close()
+
+    plt.figure()
+    plt.plot(t, front_wheel_angle)
+    plt.xlabel('$t$ (s)')
+    plt.ylabel('Front Wheel Angle (rad)')
+    plt.title('Front Wheel Angle over Time')
+    plt.grid(True)
+    if save_figures:
+        plt.savefig(os.path.join(plots_folder, 'front_wheel_angle.png'), dpi=600)
+        plt.close()
+
+    plt.figure()
+    plt.plot(t, accel)
+    plt.xlabel('$t$ (s)')
+    plt.ylabel('Acceleration (m/s^2)')
+    plt.title('Acceleration over Time')
+    plt.grid(True)
+    if save_figures:
+        plt.savefig(os.path.join(plots_folder, 'accel.png'), dpi=600)
+        plt.close()
+
+
+
+
+if __name__ == '__main__':
+    # Change this to the log folder you want to plot if running this script manually
+    log_folder = '2025-02-13_21-10-39'
+    main(log_folder)

launch/blink_launch.yaml

@@ -38,6 +38,8 @@ log:
     #state: ['all']
     # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate
     #state_rate: 10
+    # If True, then make plots of the recorded data specifically from PurePursuitTrajectoryTracker_debug.csv and report metrics. Default False
+    #auto_plot : True
 replay:  # Add items here to set certain topics / inputs to be replayed from logs
     # Specify which log folder to replay from
     log: 

launch/fixed_route.yaml

@@ -43,6 +43,8 @@ log:
     #state: ['all']
     # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate
     #state_rate: 10
+    # If True, then make plots of the recorded data specifically from PurePursuitTrajectoryTracker_debug.csv and report metrics. Default False
+    #auto_plot : True
 replay:  # Add items here to set certain topics / inputs to be replayed from logs
     # Specify which log folder to replay from
     log: 

launch/gather_data.yaml

@@ -37,6 +37,8 @@ log:
     #state: ['all']
     # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate
     #state_rate: 10
+    # If True, then make plots of the recorded data specifically from PurePursuitTrajectoryTracker_debug.csv and report metrics. Default False
+    #auto_plot : True
 replay:  # Add items here to set certain topics / inputs to be replayed from logs
     # Specify which log folder to replay from
     log: 

launch/pedestrian_detection.yaml

@@ -52,6 +52,8 @@ log:
     #state: ['all']
     # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate
     #state_rate: 10
+    # If True, then make plots of the recorded data specifically from PurePursuitTrajectoryTracker_debug.csv and report metrics. Default False
+    #auto_plot : True
 replay:  # Add items here to set certain topics / inputs to be replayed from logs
     # Specify which log folder to replay from
     log: