Min curvature complex maps

sim_ws/src/f1tenth_gym_ros/launch/gym_bridge.rviz

@@ -11,6 +11,7 @@ Panels:
         - /Path1
         - /PointStamped1
         - /Map2
+        - /Path2
       Splitter Ratio: 0.5
     Tree Height: 773
   - Class: rviz_common/Selection
@@ -255,6 +256,34 @@ Visualization Manager:
         Value: /local_costmap_updates
       Use Timestamp: false
       Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 170; 0; 255
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /pure_pursuit/risk_path
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

sim_ws/src/scripts/map_preparation/generate_raceline.py

@@ -31,10 +31,9 @@
     plot_widths_simple,
     resample_path,
     solve_min_curvature_raceline,
+    solve_min_curvature_from_raceline,
 )
-from utils.ui_centerline import (
-    tune_centerline,
-)
+from utils.ui_centerline import tune_centerline
 from utils.planning import brushfire_algo, plan_full_path
 from utils.ui_raceline_editor import edit_raceline_with_drag
 from raceline_config import DEFAULT_CONFIG
@@ -109,6 +108,10 @@ def run():
                 world_to_grid((x, y), origin, res, height) for x, y in imported_world
             ]
 
+            if not imported_grid:
+                print("Imported raceline is empty. Exiting.")
+                return
+
             smooth_raceline = imported_grid
             raw_raceline = imported_grid
             waypoints = []
@@ -199,6 +202,36 @@ def run():
                 else:
                     print("Drag edit cancelled. Keeping pre-edit smoothed raceline.")
 
+            min_curv_raceline, alpha, normals, w_left, w_right = (
+                solve_min_curvature_from_raceline(
+                    smooth_raceline,
+                    occupancy_grid,
+                    res,
+                    vehicle_width=cfg.vehicle_width,
+                    smoothness_weight=1e-4,
+                    debug=cfg.debug_min_curvature,
+                )
+            )
+
+            if normals is not None:
+                plot_raceline_result(
+                    occupancy_grid,
+                    smooth_raceline,
+                    min_curv_raceline,
+                    normals,
+                    w_left,
+                    w_right,
+                    res,
+                    title="Min Curvature (from A*)",
+                )
+
+            smooth_raceline = smooth_path(
+                min_curv_raceline,
+                smoothing_factor=cfg.min_curv_smoothing_factor,
+                num_points=len(min_curv_raceline),
+                closed=True,
+            )
+
     elif cfg.raceline_mode == "min_curvature":
         center = tune_centerline(occupancy_grid)
 
@@ -306,6 +339,7 @@ def run():
             else:
                 print("Drag edit cancelled. Keeping pre-edit smoothed raceline.")
 
+
     elif cfg.raceline_mode == "manual_spline":
         print("\nOpening manual spline window.")
         print("  Left-click  : add control points around the track")
@@ -331,7 +365,8 @@ def run():
         waypoints = control_points[1:]
     else:
         raise ValueError(
-            f"Unknown RACELINE_MODE '{cfg.raceline_mode}'. Use 'astar' or 'manual_spline'."
+            f"Unknown RACELINE_MODE '{cfg.raceline_mode}'. Use 'astar', "
+            "'min_curvature' or 'manual_spline'."
         )
 
     path_csv = os.path.join(cfg.csv_folder, cfg.csv_name)