slalom test logs

Author: KenC1014

GEMstack/onboard/planning/racing_planning.py

@@ -55,7 +55,7 @@ def waypoint_generate(vehicle_state, cones, cone_idx):
     target_heading = car_heading
 
     # ===== Parameters =====
-    u_turn_radius = 11.5      # Radius for U-turn
+    u_turn_radius = 7      # Radius for U-turn
     offset = 2.0                # Offset for left/right pass
     lookahead_distance = 10.0   # Distance ahead for fixed point
     # ======================

GEMstack/onboard/planning/stanley.py

@@ -360,11 +360,10 @@ def update(self, vehicle: VehicleState, trajectory: Trajectory):
           4) Send command to the vehicle
         """
         # path to trajectory if racing enabled
-        # if self.desired_speed_source in ['racing']: ## conditional needed for no racing
-        #     self.stanley.set_racing_path(trajectory)
-        # else:
-        #     self.stanley.set_path(trajectory)
-        self.stanley.set_path(trajectory)
+        if self.desired_speed_source in ['racing']: ## conditional needed for no racing
+            self.stanley.set_racing_path(trajectory)
+        else:
+            self.stanley.set_path(trajectory)
         accel, f_delta = self.stanley.compute(vehicle, self)
 
         # If your low-level interface expects steering wheel angle:

GEMstack/onboard/planning/velocity_profile.py

@@ -205,7 +205,7 @@ def limit_velocity_by_steering_rate(ds, velocity, wheelbase, max_steering_rate,
     # raise ValueError(max_steering_rate)
     max_steering_rate = steer2front(max_steering_rate)
     max_steering_rate = 1
-    
+
 
     # Limit velocity where steering rate exceeds max
     limited_velocity = np.copy(velocity)
@@ -387,11 +387,5 @@ def compute_velocity_profile(points, plot=True):
 
     plot_speed_profile_gradient(fig, axs[1, 0], xs, ys, v_profile)
     plot_speed_profile_gradient(fig, axs[1, 1], xs, ys, v_profile2)
-
-
     plt.tight_layout()
-    plt.show()
-
-
-
-
+    plt.show()

GEMstack/state/trajectory.py

@@ -411,6 +411,24 @@ def compute_headings(path : Path, smoothed = False) -> Path:
 
         path.points = np.vstack((x_smooth, y_smooth)).T
 
+        # raise NotImplementedError("Smoothing not done yet")
+        points = np.array(path.points)
+        x = points[:, 0]
+        y = points[:, 1]
+       
+        dx = np.diff(x)
+        dy = np.diff(y)
+        ds = np.hypot(dx, dy)
+        s = np.insert(np.cumsum(ds), 0, 0)
+
+        tck_x = splrep(s, x, s=0.5)
+        tck_y = splrep(s, y, s=0.5)
+
+        s_fine = np.linspace(0, s[-1], 200)
+        x_smooth = splev(s_fine, tck_x)
+        y_smooth = splev(s_fine, tck_y)
+
+        path.points = np.vstack((x_smooth, y_smooth)).T
     if len(path.points) < 2:
         raise ValueError("Path must have at least 2 points")
     derivs = []

testing/racing_logs/slalom_test/StanleyTrajectoryTracker_debug.csv

@@ -0,0 +1,22 @@
+events:
+- description: Hardware fault override_active
+  time: 1746658328.1433983
+  vehicle_time: 1746658328.1433842
+- description: Ctrl+C pressed, switching to recovery mode
+  time: 1746658330.244504
+  vehicle_time: 1746658330.2434165
+- description: Mission execution ended
+  time: 1746658330.2494235
+  vehicle_time: 1746658330.2434165
+exit_reason: normal exit
+git_branch: racing_temp
+git_commit_id: 6e84854b3ec79015eff2824dec10d4ae45d62600
+pipelines:
+- name: drive
+  time: 1746658299.0166996
+  vehicle_time: 1746658298.9165509
+- name: recovery
+  time: 1746658330.2455354
+  vehicle_time: 1746658330.2434165
+start_time: 1746658298.9004052
+start_time_human_readable: '2025-05-07 17:51:38'