MPPI + fence-line loc + waypoint nav with fail skip

include/car_nav_lite/bt_executor.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <functional>
 #include <optional>
 #include "car_nav_lite/types.hpp"
 #include "car_nav_lite/grid_map.hpp"
@@ -16,6 +17,7 @@ public:
     void setGoal(const Pose2D& goal);
     void setTestPath(const std::vector<Pose2D>& path);
     void setParams(double la, double ms, double mr, double sl, double cf);
+    void setFailCallback(std::function<void()> cb) { fail_callback_ = std::move(cb); }
     bool hasGoal() const { return goal_.has_value(); }
 
     Twist tick(const Pose2D& pose, const GridMap& map);
@@ -29,6 +31,7 @@ private:
     std::vector<Pose2D> current_path_;
     GlobalPlanner planner_;
     LocalPlanner  local_planner_;
+    std::function<void()> fail_callback_;
 
     bool atGoal(const Pose2D& pose) const;
 };

include/car_nav_lite/grid_map.hpp

@@ -41,6 +41,9 @@ public:
     // ── Obstacle update from scan ──────────────────────
     void updateScan(const LaserScan& scan, const Pose2D& pose);
 
+    // ── Morphological close (remove noise, merge nearby blobs) ──
+    void morphologyClose(int kernel_size = 3);
+
     // ── Pre-built obstacle marks ───────────────────────
     void markCircle(double wx, double wy, double radius, uint8_t value = 255);
     void markRect(double x1, double y1, double x2, double y2, uint8_t value = 255);

include/car_nav_lite/localizer.hpp

@@ -10,19 +10,13 @@ class Localizer {
 public:
     Localizer();
 
-    // ── Odometry update (called at odom rate) ──────────
     void updateOdom(double vx, double wz, double dt);
     void updateIMU(double gyro_z, double dt);
-
-    // Set raw odom pose. pose() returns odom + correction offset.
     void setOdomPose(const Pose2D& p);
 
-    // ── Scan-to-map correction (updates persistent offset) ──
-    // Correction = matched_pose - odom_pose, stored as offset.
-    // Only updates if confidence is high; otherwise offset decays to zero.
-    void correctWithScanCV(const LaserScan& scan, const GridMap& map);
+    void correctWithScanCV(const LaserScan& scan, const GridMap& map,
+                            const Pose2D* base_override = nullptr);
 
-    // ── Final pose = odom + correction offset ──────────
     Pose2D pose() const;
 
 private: