Steps 3-7: layered GridMap, confidence slowdown, frozen map, ROS2 params

include/car_nav_lite/grid_map.hpp

@@ -47,8 +47,13 @@ public:
     // ── Reset dynamic obstacles to static PGM layer ──
     void resetDynamic();
 
+    // ── Layered map (fence + cone + dynamic) ──
+    void freezeSessionMap();
+    void composeLayers();
+
     // ── Pre-built obstacle marks ───────────────────────
     void markCircle(double wx, double wy, double radius, uint8_t value = 255);
+    void markCone(double wx, double wy, double radius, uint8_t value = 255);
     void markRect(double x1, double y1, double x2, double y2, uint8_t value = 255);
 
     // ── Getters ────────────────────────────────────────
@@ -58,8 +63,12 @@ public:
 
 private:
     OccupancyGrid cells_;
-    OccupancyGrid static_cells_;  // snapshot after PGM load
+    OccupancyGrid static_cells_;
+    OccupancyGrid cone_cells_;     // fixed cones from session
+    OccupancyGrid dynamic_cells_;  // optional real-time obstacles
+    bool frozen_ = false;
     void bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val);
+    void bresenhamDynamic(int gx0, int gy0, int gx1, int gy1, uint8_t v);
 };
 
 }  // namespace car_nav_lite

include/car_nav_lite/localizer.hpp

@@ -24,6 +24,7 @@ public:
     // Metrics
     int loc_lines_ = 0;
     double loc_dx_ = 0, loc_dy_ = 0, loc_dyaw_ = 0;
+    double loc_conf_ = 1.0;
 
 private:
     Pose2D odom_pose_;