Rosbag latency

zebROS_ws/src/rosbag_scripts/scripts/analyze.py

@@ -0,0 +1,100 @@
+# Not a rosbag script, just used to generate fancy graphs based on action success and failure rates 
+
+import pandas as pd
+import matplotlib.pyplot as plt
+
+# Plot for success rate 
+def plot_success_rate(file_path):
+    # Load the dataset
+    df = pd.read_csv(file_path)
+
+    # Calculate success and fail counts per action
+    action_counts = df.groupby(['Action', 'Result']).size().unstack(fill_value=0)
+    action_counts['Total'] = action_counts['SUCCESS'] + action_counts['FAIL']
+
+    # Adjust Intake action
+    intake_total = action_counts.loc[['L2', 'L3', 'L4']].sum()
+    action_counts.loc['Intake'] = intake_total
+    action_counts.loc['Intake', 'FAIL'] = 40  # Set fail count to 40
+    action_counts.loc['Intake', 'SUCCESS'] = action_counts.loc['Intake', 'Total'] - 40
+
+    # Recalculate success rate
+    success_rate = action_counts['SUCCESS'] / action_counts['Total']
+
+    # Sort values
+    sorted_success_rate = success_rate.sort_values()
+    sorted_action_counts = action_counts.loc[sorted_success_rate.index]
+
+    # Plot
+    plt.figure(figsize=(10, 6))
+    ax = sorted_success_rate.plot(kind='bar', color='skyblue', edgecolor='black')
+    plt.xlabel("Action")
+    plt.ylabel("Success Rate")
+    plt.title("Success Rate by Action")
+    plt.xticks(rotation=45, ha='right', fontsize=10)
+    plt.ylim(0, 1)
+    plt.grid(axis='y', linestyle='--', alpha=0.7)
+
+    # Add percentage labels on top of bars and success/total under action labels
+    for i, action in enumerate(sorted_success_rate.index):
+        total_attempts = sorted_action_counts.loc[action, 'Total']
+        total_success = sorted_action_counts.loc[action, 'SUCCESS']
+        percent_label = f"{sorted_success_rate[action]*100:.1f}%"
+        ax.text(i, sorted_success_rate[action] + 0.02, percent_label, ha='center', fontsize=10, fontweight='bold')
+
+    # Adjust x-axis labels to include success/total on a new line
+    new_labels = [f"{action}\n{sorted_action_counts.loc[action, 'SUCCESS']}/{sorted_action_counts.loc[action, 'Total']}" for action in sorted_success_rate.index]
+    ax.set_xticklabels(new_labels, rotation=45, ha='right', fontsize=10)
+
+    # Show plot
+    plt.show()
+
+# Example usage
+file_path = "combined_output.csv"  # Replace with actual path
+plot_success_rate(file_path)
+
+# Plot for failure distribution 
+'''import pandas as pd
+import matplotlib.pyplot as plt
+
+def plot_failure_distribution(file_path):
+    # Load the dataset
+    df = pd.read_csv(file_path)
+
+    # Filter only failures and count the occurrences of each failure type (use 'Notes' for failure types)
+    fail_counts = df[df['Result'] == 'FAIL'].groupby('Notes').size()
+    total_failures = fail_counts.sum()
+
+    # Calculate the failure ratios (failure count / total failures)
+    fail_ratios = fail_counts / total_failures
+
+    # Sort the failure types by their ratio
+    sorted_fail_ratios = fail_ratios.sort_values()
+
+    # Plot failure distribution
+    plt.figure(figsize=(10, 6))
+    ax = sorted_fail_ratios.plot(kind='bar', color='salmon', edgecolor='black')
+    plt.xlabel("Failure Type")
+    plt.ylabel("Failure Count / Total Failures")
+    plt.title("Failure Distribution by Type")
+    plt.xticks(rotation=45, ha='right', fontsize=10)
+    plt.ylim(0, 1)
+    plt.grid(axis='y', linestyle='--', alpha=0.7)
+
+    # Add percentage labels on top of bars and count/total under failure type labels
+    for i, failure in enumerate(sorted_fail_ratios.index):
+        count = fail_counts[failure]
+        percent_label = f"{sorted_fail_ratios[failure]*100:.1f}%"
+        ax.text(i, sorted_fail_ratios[failure] + 0.02, percent_label, ha='center', fontsize=10, fontweight='bold')
+
+    # Adjust the x-axis labels to show count/total under each failure type
+    new_labels = [f"{failure}\n{fail_counts[failure]}/{total_failures}" for failure in sorted_fail_ratios.index]
+    ax.set_xticklabels(new_labels, rotation=45, ha='right', fontsize=10)
+
+    # Show plot
+    plt.show()
+
+# Example usage
+file_path = "combined_output.csv"  # Replace with actual path
+plot_failure_distribution(file_path)'''
+

zebROS_ws/src/rosbag_scripts/scripts/latency_check.py

@@ -0,0 +1,49 @@
+#!/usr/bin/env python
+
+import rosbag
+import math
+
+ENABLE_TOPIC = '/frcrobot_rio/match_data'
+CMD_VEL_TOPIC = '/frcrobot_jetson/swerve_drive_controller/cmd_vel_out'
+VEL_THRESHOLD = 0.01  # Threshold to consider velocity as non-zero
+
+def get_enable_time(bag):
+    for topic, msg, t in bag.read_messages(topics=[ENABLE_TOPIC]):
+        try:
+            if msg.Enabled:  # Adjust this if the actual field name is different
+                return t.to_sec()
+        except AttributeError:
+            print("Message from /frcrobot_rio/match_data does not have 'enabled' field.")
+            continue
+    return None
+
+def get_first_nonzero_cmd_vel_time(bag):
+    for topic, msg, t in bag.read_messages(topics=[CMD_VEL_TOPIC]):
+        try:
+            vel = math.hypot(msg.twist.linear.x, msg.twist.linear.y)
+            if vel > VEL_THRESHOLD:
+                return t.to_sec()
+        except AttributeError:
+            print("cmd_vel message missing linear.x or linear.y.")
+            continue
+    return None
+
+def main(bagfile):
+    bag = rosbag.Bag(bagfile)
+
+    enable_time = get_enable_time(bag)
+    if enable_time is None:
+        print("Enable signal not found in bag.")
+        return
+
+    cmd_vel_time = get_first_nonzero_cmd_vel_time(bag)
+    if cmd_vel_time is None:
+        print("Non-zero cmd_vel signal not found in bag.")
+        return
+
+    latency = cmd_vel_time - enable_time
+    print(f"Latency between enable and first non-zero cmd_vel: {latency:.3f} seconds")
+
+    bag.close()
+
+main("rosbags/19700101_000642_NCCMP_Q12.bag")