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_;

src/grid_map.cpp

@@ -7,7 +7,10 @@
 
 namespace car_nav_lite {
 
-GridMap::GridMap() : cells_(GRID_SIZE * GRID_SIZE, 0) {}
+GridMap::GridMap() : cells_(GRID_SIZE * GRID_SIZE, 0),
+                     static_cells_(GRID_SIZE * GRID_SIZE, 0),
+                     cone_cells_(GRID_SIZE * GRID_SIZE, 0),
+                     dynamic_cells_(GRID_SIZE * GRID_SIZE, 0) {}
 
 bool GridMap::loadPGM(const std::string& path) {
     std::ifstream file(path, std::ios::binary);
@@ -55,6 +58,24 @@ bool GridMap::loadPGM(const std::string& path) {
 
 void GridMap::resetDynamic() { cells_ = static_cells_; }
 
+void GridMap::composeLayers() {
+    for (size_t i = 0; i < cells_.size(); ++i) {
+        uint8_t v = static_cells_[i];
+        if (cone_cells_[i] > v) v = cone_cells_[i];
+        if (dynamic_cells_[i] > v) v = dynamic_cells_[i];
+        cells_[i] = v;
+    }
+}
+
+void GridMap::freezeSessionMap() {
+    frozen_ = true;
+    // Merge cone layer into static for persistence
+    for (size_t i = 0; i < cells_.size(); ++i)
+        if (cone_cells_[i] > static_cells_[i])
+            static_cells_[i] = cone_cells_[i];
+    composeLayers();
+}
+
 bool GridMap::worldToGrid(double wx, double wy, int& gx, int& gy) const {
     gx = (int)((wx - GRID_ORIGIN_X) / GRID_RES);
     gy = (int)((wy - GRID_ORIGIN_Y) / GRID_RES);
@@ -142,6 +163,7 @@ void GridMap::updateScan(const LaserScan& scan, const Pose2D& pose) {
     int ox, oy;
     if (!worldToGrid(pose.x, pose.y, ox, oy)) return;
 
+    auto& target = frozen_ ? dynamic_cells_ : cells_;
     for (int i = 0; i < scan.size(); ++i) {
         double r = scan.ranges[i];
         if (r < scan.range_min || r > scan.range_max) continue;
@@ -151,7 +173,8 @@ void GridMap::updateScan(const LaserScan& scan, const Pose2D& pose) {
         double hwx = pose.x + r * std::cos(angle);
         double hwy = pose.y + r * std::sin(angle);
         if (worldToGrid(hwx, hwy, hx, hy)) {
-            set(hx, hy, 255);  // hit → fully occupied immediately
+            if (frozen_) dynamic_cells_[hy * GRID_SIZE + hx] = 255;
+            else set(hx, hy, 255);
         }
 
         // Raytrace clear
@@ -160,9 +183,14 @@ void GridMap::updateScan(const LaserScan& scan, const Pose2D& pose) {
         double cwx = pose.x + clear_r * std::cos(angle);
         double cwy = pose.y + clear_r * std::sin(angle);
         if (worldToGrid(cwx, cwy, cx, cy)) {
-            bresenhamLine(ox, oy, cx, cy, 5);  // clear → -occupied
+            if (frozen_) {
+                bresenhamDynamic(ox, oy, cx, cy, 5);
+            } else {
+                bresenhamLine(ox, oy, cx, cy, 5);
+            }
         }
     }
+    if (frozen_) composeLayers();
 }
 
 void GridMap::bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val) {
@@ -182,6 +210,22 @@ void GridMap::bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_va
     }
 }
 
+void GridMap::bresenhamDynamic(int gx0, int gy0, int gx1, int gy1, uint8_t v) {
+    int dx = std::abs(gx1 - gx0), dy = -std::abs(gy1 - gy0);
+    int sx = gx0 < gx1 ? 1 : -1, sy = gy0 < gy1 ? 1 : -1;
+    int err = dx + dy, x = gx0, y = gy0;
+    while (true) {
+        if (inBounds(x, y)) {
+            uint8_t& c = dynamic_cells_[y * GRID_SIZE + x];
+            if (c >= v) c -= v;
+        }
+        if (x == gx1 && y == gy1) break;
+        int e2 = 2 * err;
+        if (e2 >= dy) { err += dy; x += sx; }
+        if (e2 <= dx) { err += dx; y += sy; }
+    }
+}
+
 void GridMap::markCircle(double wx, double wy, double radius, uint8_t value) {
     int cx, cy, cr;
     if (!worldToGrid(wx, wy, cx, cy)) return;
@@ -195,6 +239,16 @@ void GridMap::markCircle(double wx, double wy, double radius, uint8_t value) {
     }
 }
 
+void GridMap::markCone(double wx, double wy, double radius, uint8_t value) {
+    int cx, cy, cr;
+    if (!worldToGrid(wx, wy, cx, cy)) return;
+    cr = (int)(radius / GRID_RES);
+    for (int dy = -cr; dy <= cr; ++dy)
+        for (int dx = -cr; dx <= cr; ++dx)
+            if (dx*dx + dy*dy <= cr*cr && inBounds(cx+dx, cy+dy))
+                cone_cells_[(cy+dy) * GRID_SIZE + (cx+dx)] = value;
+}
+
 double GridMap::raycast(double wx, double wy, double angle, double max_range) const {
     // DDA (Digital Differential Analyzer) raycast on static occupancy grid.
     // Steps along grid cells from (wx,wy) in direction angle until hitting

src/localizer.cpp

@@ -131,8 +131,10 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
     }
 
     // ── 5. Update offset ──
-    // Store metrics
+    // 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) {
         double dx = sum_dx / line_count, dy = sum_dy / line_count;
         loc_dx_ = dx; loc_dy_ = dy; loc_dyaw_ = -dyaw;

src/main.cpp

@@ -40,6 +40,12 @@ public:
         declare_parameter("min_turn_radius", 0.20);
         declare_parameter("steer_limit", 0.6);
         declare_parameter("speed_curv_factor", 6.0);
+        declare_parameter("calib_scan_count", 30);
+        declare_parameter("fence_expected_size", 5.0);
+        declare_parameter("fence_size_tolerance", 0.35);
+        declare_parameter("wall_ransac_thresh", 0.04);
+        declare_parameter("cone_mark_radius", 0.18);
+        declare_parameter("dynamic_obstacle_enable", false);
         _read_params();
 
         param_cb_ = add_on_set_parameters_callback(
@@ -101,6 +107,10 @@ private:
     std::string mf_;
     bool test_=false, tps_=false;
     double la_=0.5, ms_=1.0, mr_=0.20, sl_=0.6, cf_=6.0;
+    int calib_scan_count_=30;
+    double fence_expected_size_=5.0, fence_size_tolerance_=0.35;
+    double wall_ransac_thresh_=0.04, cone_mark_radius_=0.18;
+    bool dynamic_obstacle_enable_=false;
     rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_;
     void _read_params(){
         mf_=get_parameter("map_file").as_string();
@@ -110,6 +120,12 @@ private:
         mr_=get_parameter("min_turn_radius").as_double();
         sl_=get_parameter("steer_limit").as_double();
         cf_=get_parameter("speed_curv_factor").as_double();
+        calib_scan_count_=get_parameter("calib_scan_count").as_int();
+        fence_expected_size_=get_parameter("fence_expected_size").as_double();
+        fence_size_tolerance_=get_parameter("fence_size_tolerance").as_double();
+        wall_ransac_thresh_=get_parameter("wall_ransac_thresh").as_double();
+        cone_mark_radius_=get_parameter("cone_mark_radius").as_double();
+        dynamic_obstacle_enable_=get_parameter("dynamic_obstacle_enable").as_bool();
     }
 
     // ── State ──
@@ -124,8 +140,7 @@ private:
     FenceModel fence_model_;
     enum SessionState { CALIBRATING, READY };
     SessionState session_state_ = CALIBRATING;
-    int calib_scan_count_ = 0;
+    int calib_cnt_ = 0;
-    static constexpr int CALIB_SCANS_NEEDED = 30;
     rclcpp::Subscription<LaserScanMsg>::SharedPtr sub_scan_;
     rclcpp::Subscription<OdometryMsg>::SharedPtr  sub_odom_;
     rclcpp::Subscription<ImuMsg>::SharedPtr       sub_imu_;
@@ -145,19 +160,20 @@ private:
         latest_scan_.ranges=m->ranges;has_scan_=true;
         scan_pose_snapshot_ = localizer_.pose();
 
-        if (session_state_ == CALIBRATING && calib_scan_count_ < CALIB_SCANS_NEEDED) {
+        if (session_state_ == CALIBRATING && calib_cnt_ < calib_scan_count_) {
             session_mapper_.addScan(latest_scan_, localizer_.pose());
-            if (++calib_scan_count_ >= CALIB_SCANS_NEEDED) {
+            if (++calib_cnt_ >= calib_scan_count_) {
                 std::vector<Eigen::Vector2d> cones;
-                if (session_mapper_.build(fence_model_, cones, 5.0, 0.35, 0.04)) {
+                if (session_mapper_.build(fence_model_, cones, fence_expected_size_, fence_size_tolerance_, wall_ransac_thresh_)) {
                     localizer_.setFenceModel(fence_model_);
-                    for (auto& c : cones)
+                    for (auto& c : cones) map_.markCone(c.x(), c.y(), cone_mark_radius_, 255);
-                        map_.markCircle(c.x(), c.y(), 0.18, 255);
+                    map_.morphologyClose(3);
+                    map_.freezeSessionMap();
                     session_state_ = READY;
-                    fprintf(stderr, "[Session] READY — fence %.2fx%.2f, %zu cones\n",
+                    fprintf(stderr, "[Session] READY — fence %.2fx%.2f yaw=%.1f°, %zu cones\n",
-                            fence_model_.width, fence_model_.height, cones.size());
+                            fence_model_.width, fence_model_.height, fence_model_.yaw_field*57.3, cones.size());
                 } else {
-                    calib_scan_count_ = 0;  // retry
+                    calib_cnt_ = 0;
                     fprintf(stderr, "[Session] calibration failed, retrying...\n");
                 }
             }
@@ -226,7 +242,9 @@ private:
     void tick(){
         std::lock_guard lk(mtx_);
         auto pose=localizer_.pose();publish_tf(pose);
-        if(has_scan_)map_.updateScan(latest_scan_,pose);
+        // READY: only update scan if dynamic obstacles enabled
+        if(has_scan_ && (session_state_==CALIBRATING || dynamic_obstacle_enable_))
+            map_.updateScan(latest_scan_,pose);
 
         // Test mode: auto-activate figure-8 after 3s
         if(test_&&!tps_){
@@ -242,6 +260,9 @@ private:
         bt_.setParams(la_,ms_,mr_,sl_,cf_);
         Twist cmd=bt_.tick(pose,map_);
 
+        // Slow down if localization confidence is low
+        if (localizer_.loc_conf_ < 0.3) { cmd.vx *= 0.4; cmd.wz *= 0.4; }
+
         auto tw=geometry_msgs::msg::Twist();tw.linear.x=cmd.vx;tw.angular.z=cmd.wz;
         pub_cmd_->publish(tw);
         if(!bt_.currentPath().empty()){
@@ -289,7 +310,10 @@ private:
         pub_metrics_->publish(msg);
     }
 
-    void morph_close(){std::lock_guard lk(mtx_); map_.morphologyClose(3);}
+    void morph_close(){std::lock_guard lk(mtx_);
+        if (session_state_ == READY) return;  // frozen map, no morph needed
+        map_.morphologyClose(3);
+    }
 
     void publish_tf(const Pose2D& pose){
         geometry_msgs::msg::TransformStamped tf;