Fix #9: point-to-line residual optimization for fence localization

- SessionMapper: fix wall line construction (0/w/h → x_min/x_max/y_min/y_max)
- session_mapper.reset() on calibration failure
- CALIBRATING state forces cmd_vel=0
- GridMap::markFenceModel() draws calibrated fence into static_cells_
- /nav_metrics now publishes loc_conf
- add_on_set_parameters_callback handles all new params
- CMake maps install guarded with EXISTS check

CMakeLists.txt

@@ -42,6 +42,9 @@ add_executable(calib_check src/calib_check.cpp)
 target_include_directories(calib_check PRIVATE ${INCLUDE_DIR} ${EIGEN3_INCLUDE_DIRS})
 ament_target_dependencies(calib_check rclcpp geometry_msgs nav_msgs tf2 tf2_ros)
 install(TARGETS nav_lite_node calib_check DESTINATION lib/${PROJECT_NAME})
-install(DIRECTORY launch config maps DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
+if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/maps")
+  install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
+endif()
 
 ament_package()

include/car_nav_lite/grid_map.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "car_nav_lite/types.hpp"
+#include "car_nav_lite/fence_model.hpp"
 #include <string>
 #include <array>
 
@@ -54,6 +55,7 @@ public:
     // ── 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 markFenceModel(const FenceModel& fence, double thickness = 0.04);
     void markRect(double x1, double y1, double x2, double y2, uint8_t value = 255);
 
     // ── Getters ────────────────────────────────────────

include/car_nav_lite/session_mapper.hpp

@@ -20,6 +20,7 @@ public:
                double expected_size = 5.0, double size_tol = 0.35,
                double ransac_thresh = 0.04);
 
+    void reset() { points_world_.clear(); scan_count_ = 0; }
     int scanCount() const { return scan_count_; }
     const std::vector<Eigen::Vector2d>& points() const { return points_world_; }
 

src/grid_map.cpp

@@ -239,6 +239,30 @@ void GridMap::markCircle(double wx, double wy, double radius, uint8_t value) {
     }
 }
 
+void GridMap::markFenceModel(const FenceModel& fence, double thickness) {
+    if (!fence.valid) return;
+    // Build four corners from boundary positions (already world-aligned)
+    double lx = fence.left_x, rx = fence.right_x;
+    double by = fence.bottom_y, ty = fence.top_y;
+    // Draw four edges
+    struct Seg { double x1,y1,x2,y2; };
+    Seg segs[] = {{lx,by,lx,ty}, {rx,by,rx,ty}, {lx,by,rx,by}, {lx,ty,rx,ty}};
+    int t = (int)(thickness / GRID_RES) + 1;
+    for (auto& s : segs) {
+        int gx1, gy1, gx2, gy2;
+        if (!worldToGrid(s.x1,s.y1,gx1,gy1) || !worldToGrid(s.x2,s.y2,gx2,gy2)) continue;
+        int dx=std::abs(gx2-gx1), dy=-std::abs(gy2-gy1);
+        int sx=gx1<gx2?1:-1, sy=gy1<gy2?1:-1;
+        int err=dx+dy, x=gx1, y=gy1;
+        while(true){for(int ty2=-t;ty2<=t;++ty2)for(int tx2=-t;tx2<=t;++tx2)
+            if(tx2*tx2+ty2*ty2<=t*t&&inBounds(x+tx2,y+ty2))
+                static_cells_[(y+ty2)*GRID_SIZE+(x+tx2)]=255;
+            if(x==gx2&&y==gy2)break;int e2=2*err;
+            if(e2>=dy){err+=dy;x+=sx;}if(e2<=dx){err+=dx;y+=sy;}
+        }
+    }
+}
+
 void GridMap::markCone(double wx, double wy, double radius, uint8_t value) {
     int cx, cy, cr;
     if (!worldToGrid(wx, wy, cx, cy)) return;

src/localizer.cpp

@@ -131,11 +131,55 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
     }
 
     // ── 5. Update offset ──
-    // Store metrics + confidence
     loc_lines_ = line_count;
     loc_conf_ = (line_count >= 3) ? 0.9 : (line_count >= 2 ? 0.6 : 0.3);
     if (std::abs(dyaw) > 0.15) loc_conf_ *= 0.5;
-    if (line_count >= 2) {
+
+    // Point-to-Line2D residual optimization (if fence model available)
+    if (use_fence_ && fence_.valid && yaw_count >= 1) {
+        constexpr double GATE = 0.20;
+        double H[9] = {0}, b2[3] = {0};
+        int n_pts = 0;
+        for (int i = 0; i < N; i += 2) {
+            double r = scan.ranges[i];
+            if (r < 0.1 || r > 4.5) continue;
+            double a = scan.angle_min + i * scan.angle_increment;
+            double wx = base.x + r * std::cos(a + base.yaw);
+            double wy = base.y + r * std::sin(a + base.yaw);
+            const Line2D* best = nullptr; double best_d = 1e9;
+            for (auto* L : {&fence_.left, &fence_.right, &fence_.bottom, &fence_.top}) {
+                double d = std::abs(L->nx * wx + L->ny * wy - L->d);
+                if (d < best_d) { best_d = d; best = L; }
+            }
+            if (best_d > GATE || !best) continue;
+            double res = best->nx * wx + best->ny * wy - best->d;
+            double qx = r * std::cos(a), qy = r * std::sin(a);
+            double dRx = -std::sin(base.yaw)*qx - std::cos(base.yaw)*qy;
+            double dRy =  std::cos(base.yaw)*qx - std::sin(base.yaw)*qy;
+            double Jyaw = best->nx * dRx + best->ny * dRy;
+            double w = (std::abs(res) < 0.05) ? 1.0 : 0.05 / std::abs(res);
+            double J[3] = {best->nx, best->ny, Jyaw};
+            for (int ri = 0; ri < 3; ++ri) {
+                b2[ri] -= w * J[ri] * res;
+                for (int ci = 0; ci < 3; ++ci) H[ri*3+ci] += w * J[ri] * J[ci];
+            }
+            ++n_pts;
+        }
+        if (n_pts > 10) {
+            double det = H[0]*(H[4]*H[8]-H[5]*H[7])-H[1]*(H[3]*H[8]-H[5]*H[6])+H[2]*(H[3]*H[7]-H[4]*H[6]);
+            if (std::abs(det) > 1e-12) {
+                double inv=1.0/det;
+                double dx=(H[4]*H[8]-H[5]*H[7])*b2[0]+(H[2]*H[7]-H[1]*H[8])*b2[1]+(H[1]*H[5]-H[2]*H[4])*b2[2];
+                double dy=(H[5]*H[6]-H[3]*H[8])*b2[0]+(H[0]*H[8]-H[2]*H[6])*b2[1]+(H[2]*H[3]-H[0]*H[5])*b2[2];
+                double da=(H[3]*H[7]-H[4]*H[6])*b2[0]+(H[1]*H[6]-H[0]*H[7])*b2[1]+(H[0]*H[4]-H[1]*H[3])*b2[2];
+                dx=limit(dx*inv,-0.10,0.10); dy=limit(dy*inv,-0.10,0.10); da=limit(da*inv,-0.10,0.10);
+                correction_.x=0.8*correction_.x+0.2*dx; correction_.y=0.8*correction_.y+0.2*dy;
+                correction_.yaw=0.8*correction_.yaw+0.2*da;
+                loc_dx_=correction_.x; loc_dy_=correction_.y; loc_dyaw_=correction_.yaw;
+                loc_conf_ = std::min(1.0, n_pts/80.0) * 0.9;
+            }
+        }
+    } else if (line_count >= 2) {
         double dx = sum_dx / line_count, dy = sum_dy / line_count;
         loc_dx_ = dx; loc_dy_ = dy; loc_dyaw_ = -dyaw;
         double corr_dist = std::sqrt(dx*dx + dy*dy);

src/main.cpp

@@ -58,6 +58,12 @@ public:
                     else if (n=="steer_limit") sl_=p.as_double();
                     else if (n=="speed_curv_factor") cf_=p.as_double();
                     else if (n=="test_mode") { test_=p.as_bool(); tps_=false; }
+                    else if (n=="calib_scan_count") calib_scan_count_=p.as_int();
+                    else if (n=="fence_expected_size") fence_expected_size_=p.as_double();
+                    else if (n=="fence_size_tolerance") fence_size_tolerance_=p.as_double();
+                    else if (n=="wall_ransac_thresh") wall_ransac_thresh_=p.as_double();
+                    else if (n=="cone_mark_radius") cone_mark_radius_=p.as_double();
+                    else if (n=="dynamic_obstacle_enable") dynamic_obstacle_enable_=p.as_bool();
                 }
                 RCLCPP_INFO(get_logger(),"params: la=%.2f ms=%.1f mr=%.2f sl=%.2f cf=%.1f test=%d",
                             la_,ms_,mr_,sl_,cf_,test_);
@@ -167,13 +173,14 @@ private:
                 if (session_mapper_.build(fence_model_, cones, fence_expected_size_, fence_size_tolerance_, wall_ransac_thresh_)) {
                     localizer_.setFenceModel(fence_model_);
                     for (auto& c : cones) map_.markCone(c.x(), c.y(), cone_mark_radius_, 255);
+                    map_.markFenceModel(fence_model_);
                     map_.morphologyClose(3);
                     map_.freezeSessionMap();
                     session_state_ = READY;
                     fprintf(stderr, "[Session] READY — fence %.2fx%.2f yaw=%.1f°, %zu cones\n",
                             fence_model_.width, fence_model_.height, fence_model_.yaw_field*57.3, cones.size());
                 } else {
-                    calib_cnt_ = 0;
+                    calib_cnt_ = 0; session_mapper_.reset();
                     fprintf(stderr, "[Session] calibration failed, retrying...\n");
                 }
             }
@@ -241,9 +248,15 @@ private:
 
     void tick(){
         std::lock_guard lk(mtx_);
+        // CALIBRATING: force zero cmd, wait for fence model
+        if (session_state_ == CALIBRATING) {
+            auto zero = geometry_msgs::msg::Twist();
+            pub_cmd_->publish(zero);
+            return;
+        }
         auto pose=localizer_.pose();publish_tf(pose);
         // READY: only update scan if dynamic obstacles enabled
-        if(has_scan_ && (session_state_==CALIBRATING || dynamic_obstacle_enable_))
+        if(has_scan_ && dynamic_obstacle_enable_)
             map_.updateScan(latest_scan_,pose);
 
         // Test mode: auto-activate figure-8 after 3s
@@ -301,10 +314,10 @@ private:
         char buf[512];
         snprintf(buf, sizeof(buf),
             "{\"state\":\"%s\",\"x\":%.2f,\"y\":%.2f,\"yaw\":%.1f,"
-            "\"loc_lines\":%d,\"loc_dx\":%.3f,\"loc_dy\":%.3f,\"loc_dyaw\":%.2f,"
+            "\"loc_conf\":%.2f,\"loc_lines\":%d,\"loc_dx\":%.3f,\"loc_dy\":%.3f,\"loc_dyaw\":%.2f,"
             "\"mppi_alive\":%d,\"mppi_total\":%d,\"mppi_minC\":%.1f}",
             m.state, m.pose_x, m.pose_y, m.pose_yaw*57.3,
-            m.loc_lines, m.loc_dx, m.loc_dy, m.loc_dyaw*57.3,
+            localizer_.loc_conf_, m.loc_lines, m.loc_dx, m.loc_dy, m.loc_dyaw*57.3,
             m.mppi_alive, m.mppi_total, m.mppi_min_cost);
         std_msgs::msg::String msg; msg.data = buf;
         pub_metrics_->publish(msg);

src/session_mapper.cpp

@@ -116,20 +116,14 @@ bool SessionMapper::build(FenceModel& fence, std::vector<Eigen::Vector2d>& cones
     fence.left_x = lw.x(); fence.right_x = rw.x();
     fence.bottom_y = bw.y(); fence.top_y = tw.y();
 
-    // Left wall: field-x = x_min, direction = field-y (0, 1)
-    Eigen::Vector2d lb = rotBack(x_min, 0), lt = rotBack(x_min, h);
+    // Build fence lines from field-aligned extents (NOT from 0/w/h!)
+    Eigen::Vector2d lb = rotBack(x_min, y_min), lt = rotBack(x_min, y_max);
     fence.left.setFromTwoPoints(lb.x(), lb.y(), lt.x(), lt.y());
-
-    // Right wall
-    Eigen::Vector2d rb = rotBack(x_max, 0), rt = rotBack(x_max, h);
+    Eigen::Vector2d rb = rotBack(x_max, y_min), rt = rotBack(x_max, y_max);
     fence.right.setFromTwoPoints(rb.x(), rb.y(), rt.x(), rt.y());
-
-    // Bottom wall: field-y = y_min
-    Eigen::Vector2d bb = rotBack(0, y_min), br = rotBack(w, y_min);
+    Eigen::Vector2d bb = rotBack(x_min, y_min), br = rotBack(x_max, y_min);
     fence.bottom.setFromTwoPoints(bb.x(), bb.y(), br.x(), br.y());
-
-    // Top wall
-    Eigen::Vector2d tb = rotBack(0, y_max), tr_ = rotBack(w, y_max);
+    Eigen::Vector2d tb = rotBack(x_min, y_max), tr_ = rotBack(x_max, y_max);
     fence.top.setFromTwoPoints(tb.x(), tb.y(), tr_.x(), tr_.y());
 
     fence.valid = true;