Step 2: FenceModel calibration replaces hardcoded ±2.5m

- fence_model.hpp: Line2D + FenceModel with wall positions
- session_mapper: RANSAC yaw fit + percentile boundary + cone clustering
- localizer: setFenceModel(), use calibrated walls in correctWithScanCV
- main.cpp: CALIBRATING→READY state machine (30 scan frames)
- /nav_metrics publisher (5Hz JSON metrics)
- mppi_alive/min_cost exposed via LocalPlanner public members
- endpoint-only path_align (EXACT-MPPI PathFollowCritic style)
- adaptive exploration ratio based on survival rate

include/car_nav_lite/bt_executor.hpp

@@ -18,6 +18,11 @@ public:
     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); }
+    void fillMetrics(NavMetrics& m) const {
+        m.mppi_alive = local_planner_.mppi_alive_;
+        m.mppi_total = local_planner_.mppi_total_;
+        m.mppi_min_cost = local_planner_.mppi_min_cost_;
+    }
     bool hasGoal() const { return goal_.has_value(); }
 
     Twist tick(const Pose2D& pose, const GridMap& map);

include/car_nav_lite/fence_model.hpp

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <vector>
+#include <cmath>
+#include <Eigen/Dense>
+
+namespace car_nav_lite {
+
+// ── Line in general form: n.x * x + n.y * y = d ──────────
+struct Line2D {
+    double nx = 0.0, ny = 0.0, d = 0.0;
+
+    void setFromTwoPoints(double x1, double y1, double x2, double y2) {
+        double dx = x2 - x1, dy = y2 - y1;
+        double len = std::sqrt(dx*dx + dy*dy);
+        if (len < 1e-9) { nx = 1; ny = 0; d = x1; return; }
+        nx = -dy / len;
+        ny =  dx / len;
+        d  = nx * x1 + ny * y1;
+    }
+
+    double distance(double x, double y) const {
+        return std::abs(nx * x + ny * y - d);
+    }
+
+    double signedDistance(double x, double y) const {
+        return nx * x + ny * y - d;
+    }
+};
+
+// ── Fence model: four walls of the arena ─────────────────
+struct FenceModel {
+    bool valid = false;
+    Line2D left, right, bottom, top;
+    // Pre-computed wall positions for localization
+    double left_x = -2.5, right_x = 2.5;
+    double bottom_y = -2.5, top_y = 2.5;
+    double yaw_field = 0.0;
+    double width = 0.0, height = 0.0;
+    double cx = 0.0, cy = 0.0;
+};
+
+}  // namespace car_nav_lite

include/car_nav_lite/local_planner.hpp

@@ -18,6 +18,10 @@ public:
                   const std::vector<Pose2D>& path,
                   const GridMap& map);
 
+public:  // metrics accessible from BTExecutor
+    int mppi_alive_ = 0, mppi_total_ = 0;
+    double mppi_min_cost_ = 0;
+
 private:
     // ── Params ──
     double ms_ = 1.0, mr_ = 0.25, sl_ = 0.6, cf_ = 6.0;

include/car_nav_lite/localizer.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "car_nav_lite/types.hpp"
+#include "car_nav_lite/fence_model.hpp"
 
 namespace car_nav_lite {
 
@@ -13,15 +14,22 @@ public:
     void updateOdom(double vx, double wz, double dt);
     void updateIMU(double gyro_z, double dt);
     void setOdomPose(const Pose2D& p);
+    void setFenceModel(const FenceModel& f) { fence_ = f; use_fence_ = f.valid; }
 
     void correctWithScanCV(const LaserScan& scan, const GridMap& map,
                             const Pose2D* base_override = nullptr);
 
     Pose2D pose() const;
 
+    // Metrics
+    int loc_lines_ = 0;
+    double loc_dx_ = 0, loc_dy_ = 0, loc_dyaw_ = 0;
+
 private:
     Pose2D odom_pose_;
     Pose2D correction_;
+    FenceModel fence_;
+    bool use_fence_ = false;
     double yaw_imu_ = 0.0, yaw_odom_ = 0.0;
 };
 

include/car_nav_lite/session_mapper.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <vector>
+#include <Eigen/Dense>
+#include "car_nav_lite/types.hpp"
+#include "car_nav_lite/fence_model.hpp"
+
+namespace car_nav_lite {
+
+class SessionMapper {
+public:
+    SessionMapper();
+
+    // ── Accumulate scan during calibration ───────────────
+    void addScan(const LaserScan& scan, const Pose2D& start_pose,
+                 int step = 4);
+
+    // ── Build fence model + extract cones ────────────────
+    bool build(FenceModel& fence, std::vector<Eigen::Vector2d>& cones,
+               double expected_size = 5.0, double size_tol = 0.35,
+               double ransac_thresh = 0.04);
+
+    int scanCount() const { return scan_count_; }
+    const std::vector<Eigen::Vector2d>& points() const { return points_world_; }
+
+private:
+    std::vector<Eigen::Vector2d> points_world_;
+    int scan_count_ = 0;
+};
+
+}  // namespace car_nav_lite

include/car_nav_lite/types.hpp

@@ -80,4 +80,24 @@ inline double limit(double v, double lo, double hi) {
     return v < lo ? lo : (v > hi ? hi : v);
 }
 
+// ── Navigation metrics (for /nav_metrics JSON) ────────────
+struct NavMetrics {
+    // localization
+    double loc_conf = 0;
+    double loc_rms = 0;
+    int loc_lines = 0;
+    double loc_dx = 0, loc_dy = 0, loc_dyaw = 0;
+
+    // planning
+    int mppi_alive = 0;
+    int mppi_total = 0;
+    double mppi_min_cost = 0;
+    double mppi_alive_ratio = 0;
+
+    // state
+    double cmd_vx = 0, cmd_wz = 0;
+    double pose_x = 0, pose_y = 0, pose_yaw = 0;
+    const char* state = "IDLE";
+};
+
 }  // namespace car_nav_lite