tune controller

Author: harishkumarbalaji

GEMstack/knowledge/defaults/e4_gazebo.yaml

@@ -12,14 +12,14 @@ control:
         brake_amount : 0.5
         brake_speed : 2.0
     pure_pursuit:
-        lookahead: 4.0
-        lookahead_scale: 1.0
-        crosstrack_gain: 1.0
+        lookahead: 2.5
+        lookahead_scale: 3.0
+        crosstrack_gain: 0.5
         desired_speed: trajectory
     longitudinal_control:
-        pid_p: 0.1
-        pid_i: 0.5
-        pid_d: 0.5
+        pid_p: 1.5
+        pid_i: 0.2
+        pid_d: 0.0
 
 #configure the simulator, if using
 simulator:

GEMstack/onboard/interface/gem_gazebo.py

@@ -1,6 +1,7 @@
 from .gem import *
 from ...utils import settings
 import math
+import time 
 
 # ROS Headers
 import rospy
@@ -74,6 +75,7 @@ def __init__(self):
         self.ackermann_pub = rospy.Publisher(
             '/ackermann_cmd', AckermannDrive, queue_size=1)
         self.ackermann_cmd = AckermannDrive()
+        self.last_command = None  # Store the last command
 
         # Subscribe to IMU topic by default
         self.imu_sub = rospy.Subscriber("/imu", Imu, self.imu_callback)
@@ -214,21 +216,64 @@ def hardware_faults(self) -> List[str]:
         return self.faults
 
     def send_command(self, command : GEMVehicleCommand):
+        # Throttle rate at which we send commands
+        t = self.time()
+        if t < self.last_command_time + 1.0/self.max_send_rate:
+            # Skip command, similar to hardware interface
+            return
+        self.last_command_time = t
 
-        v  =  self.last_reading.speed
-        # convert pedal to acceleration
-        accelerator_pedal_position = np.clip(command.accelerator_pedal_position,0.0,1.0)
-        brake_pedal_position = np.clip(command.brake_pedal_position,0.0,1.0)
-        acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,1)
-        acceleration = np.clip(acceleration,-2, 1)
-        # convert wheel angle to steering angle
+        # Get current speed
+        v = self.last_reading.speed
+        
+        # Convert pedal to acceleration
+        accelerator_pedal_position = np.clip(command.accelerator_pedal_position, 0.0, 1.0)
+        brake_pedal_position = np.clip(command.brake_pedal_position, 0.0, 1.0)
+        
+        # Zero out accelerator if brake is active (just like hardware interface)
+        if brake_pedal_position > 0.0:
+            accelerator_pedal_position = 0.0
+            
+        # Calculate acceleration from pedal positions
+        acceleration = pedal_positions_to_acceleration(accelerator_pedal_position, brake_pedal_position, v, 0, 1)
+        
+        # Apply reasonable limits to acceleration
+        max_accel = settings.get('vehicle.limits.max_acceleration', 1.0)
+        max_decel = settings.get('vehicle.limits.max_deceleration', -2.0)
+        acceleration = np.clip(acceleration, max_decel, max_accel)
+        
+        # Convert wheel angle to steering angle (front wheel angle)
         phides = steer2front(command.steering_wheel_angle)
 
-        #create and publish drive message
+        # Apply steering angle limits
+        min_wheel_angle = settings.get('vehicle.geometry.min_wheel_angle', -0.6)
+        max_wheel_angle = settings.get('vehicle.geometry.max_wheel_angle', 0.6)
+        phides = np.clip(phides, min_wheel_angle, max_wheel_angle)
+        
+        # Calculate target speed based on acceleration
+        # Don't use infinite speed, instead calculate a reasonable target speed
+        current_speed = v
+        target_speed = current_speed
+        
+        if acceleration > 0:
+            # Accelerating - set target speed to current speed plus some increment
+            # This is more realistic than infinite speed
+            max_speed = settings.get('vehicle.limits.max_speed', 10.0)
+            target_speed = min(current_speed + acceleration * 0.5, max_speed)
+        elif acceleration < 0:
+            # Braking - set target speed to zero if deceleration is significant
+            if brake_pedal_position > 0.1:
+                target_speed = 0.0
+        
+        # Create and publish drive message
         msg = AckermannDrive()
         msg.acceleration = acceleration
-        msg.speed = float('inf') if acceleration >0 else 0  #acceleration * self.dt 
+        msg.speed = target_speed
         msg.steering_angle = phides
-
+        msg.steering_angle_velocity = command.steering_wheel_speed  # Respect steering velocity limit
+        
+        # Debug output
+        print(f"[ACKERMANN] Speed: {msg.speed:.2f}, Accel: {msg.acceleration:.2f}, Steer: {msg.steering_angle:.2f}")
+        
         self.ackermann_pub.publish(msg)
         self.last_command = command

testing/test_comfort_metrics.py

@@ -8,6 +8,7 @@
 import numpy as np
 from matplotlib.collections import LineCollection
 from matplotlib.colors import Normalize
+from matplotlib import cm
 import os
 
 CMAP = "RdYlGn"
@@ -104,13 +105,18 @@ def compute_derivative(times, values):
     """
     dt = np.diff(times)
     dv = np.diff(values)
-    derivative = dv / dt
+    
+    # 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 = plt.cm.ScalarMappable(cmap=CMAP)
+    sm = cm.ScalarMappable(cmap=CMAP)
     cbar = figure.colorbar(sm, cax=cbar_ax)
     cbar.set_label("Comfort/Safety Level")
 
@@ -149,9 +155,18 @@ def plot_jerk(axis, time, jerk, safe_thresh=1.0, unsafe_thresh=2.5):
     lc.set_array(safety[:-1])
     axis.add_collection(lc)
 
-    # set limits & labels
+    # 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_ylim(jerk.min(), jerk.max())
     axis.set_xlabel("Time (s)")
     axis.set_ylabel("Jerk (m/s³)")
     axis.set_title("Vehicle Jerk Over Time")
@@ -171,8 +186,17 @@ def plot_acceleration(axis, time, acceleration, safe_thresh=0.5, unsafe_thresh=1
     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_ylim(acceleration.min(), acceleration.max())
     axis.set_xlabel("Time (s)")
     axis.set_ylabel("Acceleration (m/s²)")
     axis.set_title("Vehicle Acceleration Over Time")
@@ -192,8 +216,17 @@ def plot_heading_acceleration(axis, time, heading_acc, safe_thresh=0.0075, unsaf
     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_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")
@@ -214,10 +247,17 @@ def plot_crosstrack_error(axis, time, cte, safe_thresh=0.1, unsafe_thresh=0.4):
     lc.set_linewidth(2.0)
     axis.add_collection(lc)
 
-    # set axis limits
+    # 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_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")
@@ -286,17 +326,31 @@ def plot_pedestrian_dist(axis, pedestrian_times, pedestrian_distances, safe_thre
 
     # 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)
+    
+    # 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)
 
-        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')
+        # 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.")
@@ -310,7 +364,14 @@ def plot_pedestrian_dist(axis, pedestrian_times, pedestrian_distances, safe_thre
         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²")
+    # 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")