Critical fixes #1-8: wall coords, calib reset, cmd=0 guard, fence marking, loc_conf, params callback, CMake maps

- 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/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)
+    // Build fence lines from field-aligned extents (NOT from 0/w/h!)
-    Eigen::Vector2d lb = rotBack(x_min, 0), lt = rotBack(x_min, 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());
-
+    Eigen::Vector2d rb = rotBack(x_max, y_min), rt = rotBack(x_max, y_max);
-    // Right wall
-    Eigen::Vector2d rb = rotBack(x_max, 0), rt = rotBack(x_max, h);
     fence.right.setFromTwoPoints(rb.x(), rb.y(), rt.x(), rt.y());
-
+    Eigen::Vector2d bb = rotBack(x_min, y_min), br = rotBack(x_max, y_min);
-    // Bottom wall: field-y = y_min
-    Eigen::Vector2d bb = rotBack(0, y_min), br = rotBack(w, y_min);
     fence.bottom.setFromTwoPoints(bb.x(), bb.y(), br.x(), br.y());
-
+    Eigen::Vector2d tb = rotBack(x_min, y_max), tr_ = rotBack(x_max, y_max);
-    // Top wall
-    Eigen::Vector2d tb = rotBack(0, y_max), tr_ = rotBack(w, y_max);
     fence.top.setFromTwoPoints(tb.x(), tb.y(), tr_.x(), tr_.y());
 
     fence.valid = true;