diff --git a/.gitignore b/.gitignore index 61a2df38b..3da224293 100644 --- a/.gitignore +++ b/.gitignore @@ -171,8 +171,6 @@ cython_debug/ .vscode/ setup/zed_sdk.run cuda/ -homework/yolov8n.pt -homework/yolo11n.pt GEMstack/knowledge/detection/yolov8n.pt GEMstack/knowledge/detection/yolo11n.pt yolov8n.pt diff --git a/GEMstack/knowledge/defaults/__init__.py b/GEMstack/knowledge/defaults/__init__.py index f7126e6a8..ee5354a7c 100644 --- a/GEMstack/knowledge/defaults/__init__.py +++ b/GEMstack/knowledge/defaults/__init__.py @@ -1,36 +1,7 @@ from ...utils.config import load_config_recursive -import os,sys - - -# Identify the vehicle used and load settings else default to vehicle config at current.yaml' -# default currently uses GEM e4 -vehicle = "default" - -for arg in sys.argv: - if arg.startswith('--'): - k,v = arg.split('=',1) - k = k[2:] - v = v.strip('"') - if k == "vehicle": - vehicle = v - sys.argv.remove(arg) - break - -if(vehicle == 'e2'): - vehicle_config = 'e2.yaml' -elif(vehicle == 'e4'): - vehicle_config = 'current.yaml' -elif(vehicle == 'e2_gazebo'): - vehicle_config = 'e2_gazebo.yaml' -elif(vehicle == 'e4_gazebo'): - vehicle_config = 'e4_gazebo.yaml' -else: - print("Unknown vehicle argument passed") - vehicle = "default" - vehicle_config = 'current.yaml' - -SETTINGS_FILE = os.path.join(os.path.split(__file__)[0],vehicle_config) +import os +SETTINGS_FILE = os.path.join(os.path.split(__file__)[0],'current.yaml') print("**************************************************************") -print(f"Loading {vehicle} settings from",SETTINGS_FILE) +print("Loading default settings from",SETTINGS_FILE) print("**************************************************************") SETTINGS = load_config_recursive(SETTINGS_FILE) diff --git a/GEMstack/knowledge/detection/yolov8n.pt b/GEMstack/knowledge/detection/yolov8n.pt deleted file mode 100644 index 0db4ca4b4..000000000 Binary files a/GEMstack/knowledge/detection/yolov8n.pt and /dev/null differ diff --git a/GEMstack/onboard/perception/object_detection.py b/GEMstack/onboard/perception/test_yolo_gazebo_simulation.py similarity index 79% rename from GEMstack/onboard/perception/object_detection.py rename to GEMstack/onboard/perception/test_yolo_gazebo_simulation.py index 3cc2619af..2bd124781 100644 --- a/GEMstack/onboard/perception/object_detection.py +++ b/GEMstack/onboard/perception/test_yolo_gazebo_simulation.py @@ -1,3 +1,16 @@ +""" +Demo script to run YOLO object detection on a Gazebo simulation. + +This code subscribes to the front camera feed from the GEM e2/e4 model and applies YOLO-based object detection to the incoming images. + +Visualization: +- Use RViz or rqt to monitor the topics. + +ROS Topics: +- Raw camera feed: /gem/debug +- YOLO detection output: /gem/image_detection +""" + # Python import os @@ -26,23 +39,17 @@ class ObjectDetection(Component): - # TODO: Pull params into a JSON/yaml - # TODO: Convert some lists into np.arrays, vectorize calculations - # TODO: Implement logging instead of print, cleanup comments - # TODO: Cleanup funcs + split into separate classes - # TODO: Decide if we want to name dets "peds" or "objs"/"agents" - # Maybe peds for now and Agents in agent_detection.py? - def __init__(self, vehicle_interface : GEMInterface) -> None: + def __init__(self, vehicle_interface : GEMInterface) -> None: + """ + Initializes essential functions required to run the YOLO model. + """ # vehicle interface self.vehicle_interface = vehicle_interface - - # cv2 to ros converter self.bridge = CvBridge() - # yolo model self.detector = YOLO(os.getcwd()+'/GEMstack/knowledge/detection/yolov8n.pt') self.confidence = 0.1 @@ -50,17 +57,27 @@ def __init__(self, vehicle_interface : GEMInterface) -> None: def initialize(self): + """ + Defines callback functions for subscribing to the front camera image stream and sets up publishers for debugging purposes. + """ + super().initialize() self.vehicle_interface.subscribe_sensor('front_camera',self.front_camera_callback, type = cv2.Mat) - # self.pub_pedestrians_pc2 = rospy.Publisher("/point_cloud/pedestrians", PointCloud2, queue_size=10) - # self.pub_obj_centers_pc2 = rospy.Publisher("/point_cloud/obj_centers", PointCloud2, queue_size=10) - # self.pub_bboxes_markers = rospy.Publisher("/markers/bboxes", MarkerArray, queue_size=10) self.pub_image = rospy.Publisher("/gem/debug", Image, queue_size=1) self.pub_detimage = rospy.Publisher("/gem/image_detection", Image, queue_size=1) def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: + """ + Displays vehicle statistics in the GLOBAL reference frame. + + This function also allows switching between coordinate frames using the `VehicleState -> pose` method. + + Returning an `AgentState` will automatically log detected objects. + """ + + print(f"VEHICLE State at time: {vehicle.pose.t}") print(f"x: {vehicle.pose.x}") @@ -79,6 +96,10 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: def front_camera_callback(self, image: cv2.Mat): + """ + A simple callback function that processes incoming images, runs them through a YOLO model, and visualizes the detections. + """ + ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8') self.pub_image.publish(ros_img) @@ -132,10 +153,6 @@ def front_camera_callback(self, image: cv2.Mat): ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8') self.pub_detimage.publish(ros_img) - - - - def rate(self): diff --git a/docs/Gazebo Simulation Documentation.md b/docs/Gazebo Simulation Documentation.md index d2f51bb03..aea20d841 100644 --- a/docs/Gazebo Simulation Documentation.md +++ b/docs/Gazebo Simulation Documentation.md @@ -68,15 +68,16 @@ In one terminal, run the Gazebo simulator (using the instructions provided in th Open a second terminal and launch GEMStack with your configured launch file. ```bash -python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/{your_file}.yaml +python3 main.py --variant=gazebo --settings={your_file}.yaml launch/{your_file}.yaml + ``` - Make sure to set the variant to `gazebo`. -- You can specify the vehicle type to be `e2_gazebo` or `e4_gazebo`. +- You can specify the settings file to GEMstack/knowledge/defaults/e4_gazebo.yaml or e2_gazebo.yaml till we work on closing the sim to real gap. Example command launching the fixed route with e4 vehicle: ```bash -python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/fixed_route.yaml +python3 main.py --variant=gazebo --settings=GEMstack/knowledge/defaults/e4_gazebo.yaml launch/fixed_route.yaml ``` **Variants:** - sim @@ -87,4 +88,16 @@ python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/fixed_route.yaml - e4 - e2_gazebo - e4_gazebo + +**Setting:** + +By default it takes GEMstack/knowledge/defaults/current.yaml which is GEM E4 Vehicle configuration settings. + +Other available configuration files: + +GEMstack/knowledge/defaults/ +- e2.yaml +- e2_gazebo.yaml +- e4_gazebo.yaml + --- diff --git a/docs/Safety and Comfort Metrics Documentation.md b/docs/Safety and Comfort Metrics Documentation.md deleted file mode 100644 index ea0e2f22f..000000000 --- a/docs/Safety and Comfort Metrics Documentation.md +++ /dev/null @@ -1,39 +0,0 @@ -# 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 diff --git a/launch/gazebo_simulation.yaml b/launch/gazebo_simulation.yaml index eb334fb2e..be77df511 100644 --- a/launch/gazebo_simulation.yaml +++ b/launch/gazebo_simulation.yaml @@ -72,7 +72,7 @@ variants: drive: perception: state_estimation : GNSSStateEstimator - agent_detection : object_detection.ObjectDetection + agent_detection : test_yolo_gazebo_simulation.ObjectDetection perception_normalization : StandardPerceptionNormalizer planning: diff --git a/launch/perception.yaml b/launch/perception.yaml deleted file mode 100644 index 3cbeb0ab2..000000000 --- a/launch/perception.yaml +++ /dev/null @@ -1,101 +0,0 @@ -description: "Run the yielding trajectory planner on the real vehicle with real perception" -mode: hardware -vehicle_interface: gem_hardware.GEMHardwareInterface -mission_execution: StandardExecutor - -# Recovery behavior after a component failure -recovery: - planning: - trajectory_tracking : recovery.StopTrajectoryTracker - -# Driving behavior for the GEM vehicle. Runs real pedestrian perception, yield planner, but does not send commands to real vehicle. -drive: - perception: - state_estimation : GNSSStateEstimator - agent_detection : pedestrian_detection.PedestrianDetector2D - perception_normalization : StandardPerceptionNormalizer - planning: - relations_estimation: pedestrian_yield_logic.PedestrianYielder - route_planning: - type: StaticRoutePlanner - args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv','start'] - motion_planning: longitudinal_planning.YieldTrajectoryPlanner - trajectory_tracking: - type: pure_pursuit.PurePursuitTrajectoryTracker - print: False - - -log: - # Specify the top-level folder to save the log files. Default is 'logs' - #folder : 'logs' - # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix - #prefix : 'fixed_route_' - # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix - #suffix : 'test3' - # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics. - ros_topics : - # Specify options to pass to rosbag record. Default is no options. - #rosbag_options : '--split --size=1024' - # If True, then record all readings / commands of the vehicle interface. Default False - vehicle_interface : True - # Specify which components to record to behavior.json. Default records nothing - components : ['state_estimation','agent_detection','motion_planning'] - # Specify which components of state to record to state.json. Default records nothing - #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 -replay: # Add items here to set certain topics / inputs to be replayed from logs - # Specify which log folder to replay from - log: - ros_topics : [] - components : [] - -#usually can keep this constant -computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" - -variants: - detector_only: - run: - description: "Run the pedestrian detection code" - drive: - planning: - trajectory_tracking: - visualization: !include "mpl_visualization.yaml" - - real_sim: - run: - description: "Run the pedestrian detection code with real detection and fake simulation" - mode: hardware - vehicle_interface: - type: gem_mixed.GEMRealSensorsWithSimMotionInterface - args: - scene: !relative_path '../scenes/xyhead_demo.yaml' - mission_execution: StandardExecutor - require_engaged: False - visualization: !include "klampt_visualization.yaml" - drive: - perception: - state_estimation : OmniscientStateEstimator - - - fake_real: - run: - description: "Run the yielding trajectory planner on the real vehicle with faked perception" - drive: - perception: - agent_detection : pedestrian_detection.FakePedestrianDetector2D - - fake_sim: - run: - description: "Run the yielding trajectory planner in simulation with faked perception" - mode: simulation - vehicle_interface: - type: gem_simulator.GEMDoubleIntegratorSimulationInterface - args: - scene: !relative_path '../scenes/xyhead_demo.yaml' - visualization: !include "klampt_visualization.yaml" - drive: - perception: - agent_detection : pedestrian_detection.FakePedestrianDetector2D - #agent_detection : OmniscientAgentDetector #this option reads agents from the simulator - state_estimation : OmniscientStateEstimator \ No newline at end of file diff --git a/testing/test_comfort_metrics.py b/testing/test_comfort_metrics.py deleted file mode 100644 index 78370b88f..000000000 --- a/testing/test_comfort_metrics.py +++ /dev/null @@ -1,377 +0,0 @@ -import sys -import os -sys.path.append(os.getcwd()) - -from GEMstack.state import AgentEnum -import json -import matplotlib.pyplot as plt -import numpy as np -from matplotlib.collections import LineCollection -from matplotlib.colors import Normalize -from matplotlib import cm -import os - -CMAP = "RdYlGn" - -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: - factor = 1.0 - elif abs_val >= unsafe_thresh: - factor = 0.0 - else: - 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: - entry = json.loads(line) - 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 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), - 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) - - 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) - vehicle_time = data[:, 18] - cte = data[:, 20] - x_actual, y_actual = data[:, 2], data[:, 5] - x_desired, y_desired = data[:, 11], data[:, 14] - return vehicle_time, cte, x_actual, y_actual, x_desired, y_desired - -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) - - # Avoid division by zero or very small values - mask = np.abs(dt) > 1e-10 - derivative = np.zeros_like(dt) - derivative[mask] = dv[mask] / dt[mask] - - 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 = 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, 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') - - 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[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 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 axis limits safely, avoiding NaN/Inf values - if len(jerk) > 0: - valid_jerk = jerk[~np.isnan(jerk) & ~np.isinf(jerk)] - if len(valid_jerk) > 0: - ymin, ymax = valid_jerk.min(), valid_jerk.max() - # Add small margin if min equals max - if ymin == ymax: - ymin -= 0.1 - ymax += 0.1 - axis.set_ylim(ymin, ymax) - - axis.set_xlim(time.min(), time.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) - - # Set axis limits safely - if len(acceleration) > 0: - valid_accel = acceleration[~np.isnan(acceleration) & ~np.isinf(acceleration)] - if len(valid_accel) > 0: - ymin, ymax = valid_accel.min(), valid_accel.max() - if ymin == ymax: - ymin -= 0.1 - ymax += 0.1 - axis.set_ylim(ymin, ymax) - - axis.set_xlim(time.min(), time.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 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) - - # Set axis limits safely - if len(heading_acc) > 0: - valid_heading = heading_acc[~np.isnan(heading_acc) & ~np.isinf(heading_acc)] - if len(valid_heading) > 0: - ymin, ymax = valid_heading.min(), valid_heading.max() - if ymin == ymax: - ymin -= 0.1 - ymax += 0.1 - axis.set_ylim(ymin, ymax) - - axis.set_xlim(time.min(), time.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 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 safely - if len(cte) > 0: - valid_cte = cte[~np.isnan(cte) & ~np.isinf(cte)] - if len(valid_cte) > 0: - ymin, ymax = valid_cte.min(), valid_cte.max() - if ymin == ymax: - ymin -= 0.1 - ymax += 0.1 - axis.set_ylim(ymin, ymax) - - axis.set_xlim(time.min(), time.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.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") - axis.legend() - axis.grid(True) - -def plot_pedestrian_dist(axis, pedestrian_times, pedestrian_distances, safe_thresh=5.0, unsafe_thresh=2.0): - """Plots pedestrian distance to vehicle vs. time""" - 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)") - axis.set_title("Pedestrian Distance Over Time") - axis.grid(True) - -if __name__=='__main__': - if len(sys.argv) == 2: - log_dir = "logs/" + sys.argv[1] - else: - - logs_root = "logs/" - # Get all subdirectories inside "logs/" - subdirs = [os.path.join(logs_root, d) for d in os.listdir(logs_root) if os.path.isdir(os.path.join(logs_root, d))] - - # Find the latest directory based on modification time - log_dir = max(subdirs, key=os.path.getmtime) - - print(f"Using latest log directory: {log_dir}") - 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) - - # Ensure we have valid data before computing derivatives - if len(times) > 1 and len(accelerations) > 1 and len(heading_rates) > 1: - time_jerk, jerk = compute_derivative(times, accelerations) - time_heading_acc, heading_acc = compute_derivative(times, heading_rates) - else: - print("Warning: Not enough data points to compute derivatives.") - time_jerk, jerk = np.array([]), np.array([]) - time_heading_acc, heading_acc = np.array([]), np.array([]) - - # 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, times, accelerations, vehicle_time, cte, - x_actual, y_actual, x_desired, y_desired, ped_times, ped_distances) - - # Calculate RMS values only for valid data points - valid_cte = cte[~np.isnan(cte) & ~np.isinf(cte)] - if len(valid_cte) > 0: - print("RMS Cross Track Error:", np.sqrt(np.mean(valid_cte**2)), "m") - - valid_pos_error = np.sqrt((x_actual - x_desired)**2 + (y_actual - y_desired)**2) - valid_pos_error = valid_pos_error[~np.isnan(valid_pos_error) & ~np.isinf(valid_pos_error)] - if len(valid_pos_error) > 0: - print("RMS Position Error:", np.sqrt(np.mean(valid_pos_error**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(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() - - # Calculate RMS values only for valid data points - valid_jerk = jerk[~np.isnan(jerk) & ~np.isinf(jerk)] - if len(valid_jerk) > 0: - print("RMS Jerk:", np.sqrt(np.mean(valid_jerk**2)), "m/s³") - - valid_heading_acc = heading_acc[~np.isnan(heading_acc) & ~np.isinf(heading_acc)] - if len(valid_heading_acc) > 0: - print("RMS Heading Acceleration:", np.sqrt(np.mean(valid_heading_acc**2)), "rad/s²") - - if len(ped_distances) > 0: - print("Minimum Pedestrian Distance:", np.min(ped_distances), "m")