Ackermann MPPI: v+kappa sampling, no reverse, progress critic, safety stop-only

CMakeLists.txt

@@ -26,6 +26,8 @@ add_executable(nav_lite_node
   src/local_planner.cpp
   src/bt_executor.cpp
   src/session_mapper.cpp
+  src/cone_updater.cpp
+  src/laser_safety.cpp
 )
 
 target_include_directories(nav_lite_node PRIVATE

include/car_nav_lite/cone_updater.hpp

@@ -0,0 +1,38 @@
+#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 ConeUpdater {
+public:
+    ConeUpdater();
+
+    // Process one scan frame: extract cone candidates, track across frames.
+    // confirmed_cones is the current known cone list (for dedup).
+    void update(const LaserScan& scan, const Pose2D& pose,
+                const FenceModel& fence,
+                const std::vector<Eigen::Vector2d>& known_cones);
+
+    // Returns newly confirmed cones and clears them from pending.
+    std::vector<Eigen::Vector2d> popConfirmed();
+
+private:
+    struct Candidate {
+        Eigen::Vector2d center;
+        int hits = 0;
+        int age = 0;
+    };
+    std::vector<Candidate> candidates_;
+
+    static constexpr int CONFIRM_HITS = 3;
+    static constexpr double CLUSTER_DIST = 0.10;
+    static constexpr double MERGE_DIST = 0.25;
+    static constexpr double WALL_DIST = 0.15;
+    static constexpr int MIN_POINTS = 4;
+};
+
+}  // namespace car_nav_lite

include/car_nav_lite/fence_model.hpp

@@ -35,6 +35,9 @@ struct FenceModel {
     // Pre-computed wall positions for localization
     double left_x = -2.5, right_x = 2.5;
     double bottom_y = -2.5, top_y = 2.5;
+    // Pre-computed corner points for drawing rotated fence
+    double p_lb_x=0, p_lb_y=0, p_lt_x=0, p_lt_y=0;
+    double p_rb_x=0, p_rb_y=0, p_rt_x=0, p_rt_y=0;
     double yaw_field = 0.0;
     double width = 0.0, height = 0.0;
     double cx = 0.0, cy = 0.0;

include/car_nav_lite/grid_map.hpp

@@ -51,6 +51,7 @@ public:
     // ── Layered map (fence + cone + dynamic) ──
     void freezeSessionMap();
     void composeLayers();
+    void decayDynamic(uint8_t amount = 10);
 
     // ── Pre-built obstacle marks ───────────────────────
     void markCircle(double wx, double wy, double radius, uint8_t value = 255);

include/car_nav_lite/laser_safety.hpp

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "car_nav_lite/types.hpp"
+
+namespace car_nav_lite {
+
+// Fast scan-based safety filter. Runs every frame before publishing cmd_vel.
+// Checks if the planned command would hit anything in the latest scan.
+struct LaserSafetyFilter {
+    bool enable = true;
+    double front_width = 0.30;   // half-width of front safety zone (m)
+    double stop_dist  = 0.35;   // brake distance (m)
+    double slow_dist  = 0.70;   // slow-down distance (m)
+    double turn_wz    = 0.8;    // evasive turn rate (rad/s)
+    double max_range  = 2.0;    // max lookahead (m)
+
+    Twist filter(const Twist& cmd, const LaserScan& scan) const;
+};
+
+}  // namespace car_nav_lite

include/car_nav_lite/localizer.hpp

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

src/cone_updater.cpp

@@ -0,0 +1,114 @@
+#include "car_nav_lite/cone_updater.hpp"
+#include <cmath>
+#include <cstdio>
+#include <algorithm>
+
+namespace car_nav_lite {
+
+ConeUpdater::ConeUpdater() {}
+
+void ConeUpdater::update(const LaserScan& scan, const Pose2D& pose,
+                          const FenceModel& fence,
+                          const std::vector<Eigen::Vector2d>& known_cones) {
+    if (!fence.valid) return;
+
+    // ── 1. Extract non-wall, non-known-cone world points ──
+    std::vector<Eigen::Vector2d> pts;
+    double cy = std::cos(pose.yaw), sy = std::sin(pose.yaw);
+    for (int i = 0; i < scan.size(); i += 3) {
+        double r = scan.ranges[i];
+        if (r < 0.15 || r > 4.5) continue;
+        double a = scan.angle_min + i * scan.angle_increment;
+        double wx = pose.x + r * std::cos(a + pose.yaw);
+        double wy = pose.y + r * std::sin(a + pose.yaw);
+
+        // Skip wall points
+        double dw = std::min({
+            fence.left.distance(wx, wy), fence.right.distance(wx, wy),
+            fence.bottom.distance(wx, wy), fence.top.distance(wx, wy)
+        });
+        if (dw < WALL_DIST) continue;
+
+        // Skip near known cones
+        bool near_cone = false;
+        for (auto& c : known_cones) {
+            double dx = wx - c.x(), dy = wy - c.y();
+            if (dx*dx + dy*dy < MERGE_DIST * MERGE_DIST) { near_cone = true; break; }
+        }
+        if (near_cone) continue;
+
+        pts.emplace_back(wx, wy);
+    }
+    if (pts.size() < 10) return;
+
+    // ── 2. Simple grid clustering ──
+    std::vector<std::vector<Eigen::Vector2d>> clusters;
+    std::vector<bool> used(pts.size(), false);
+    for (size_t i = 0; i < pts.size(); ++i) {
+        if (used[i]) continue;
+        std::vector<Eigen::Vector2d> cl;
+        cl.push_back(pts[i]); used[i] = true;
+        for (size_t j = 0; j < cl.size(); ++j) {
+            for (size_t k = i+1; k < pts.size(); ++k) {
+                if (used[k]) continue;
+                double dx = pts[k].x() - cl[j].x();
+                double dy = pts[k].y() - cl[j].y();
+                if (dx*dx + dy*dy < CLUSTER_DIST * CLUSTER_DIST) {
+                    cl.push_back(pts[k]); used[k] = true;
+                }
+            }
+        }
+        if (cl.size() >= MIN_POINTS) clusters.push_back(cl);
+    }
+
+    // ── 3. Match clusters to candidates ──
+    for (auto& cl : clusters) {
+        Eigen::Vector2d center(0, 0);
+        for (auto& p : cl) { center.x() += p.x(); center.y() += p.y(); }
+        center /= (double)cl.size();
+
+        // Find nearest existing candidate
+        Candidate* best = nullptr;
+        double best_d2 = 1e9;
+        for (auto& c : candidates_) {
+            double dx = center.x() - c.center.x();
+            double dy = center.y() - c.center.y();
+            double d2 = dx*dx + dy*dy;
+            if (d2 < MERGE_DIST * MERGE_DIST && d2 < best_d2) {
+                best_d2 = d2; best = &c;
+            }
+        }
+
+        if (best) {
+            // Update existing candidate
+            best->center = (best->center + center) / 2.0;
+            best->hits++;
+            best->age = 0;
+        } else {
+            // New candidate
+            candidates_.push_back({center, 1, 0});
+        }
+    }
+
+    // ── 4. Age all candidates, remove stale ──
+    for (auto& c : candidates_) c.age++;
+    candidates_.erase(
+        std::remove_if(candidates_.begin(), candidates_.end(),
+                       [](const Candidate& c) { return c.age > 10 && c.hits < CONFIRM_HITS; }),
+        candidates_.end());
+}
+
+std::vector<Eigen::Vector2d> ConeUpdater::popConfirmed() {
+    std::vector<Eigen::Vector2d> confirmed;
+    for (auto it = candidates_.begin(); it != candidates_.end(); ) {
+        if (it->hits >= CONFIRM_HITS) {
+            confirmed.push_back(it->center);
+            it = candidates_.erase(it);
+        } else {
+            ++it;
+        }
+    }
+    return confirmed;
+}
+
+}  // namespace car_nav_lite

src/grid_map.cpp

@@ -67,6 +67,10 @@ void GridMap::composeLayers() {
     }
 }
 
+void GridMap::decayDynamic(uint8_t amount) {
+    for (auto& v : dynamic_cells_) v = (v > amount) ? (v - amount) : 0;
+}
+
 void GridMap::freezeSessionMap() {
     frozen_ = true;
     // Merge cone layer into static for persistence
@@ -163,6 +167,7 @@ void GridMap::updateScan(const LaserScan& scan, const Pose2D& pose) {
     int ox, oy;
     if (!worldToGrid(pose.x, pose.y, ox, oy)) return;
 
+    if (frozen_) decayDynamic(10);
     auto& target = frozen_ ? dynamic_cells_ : cells_;
     for (int i = 0; i < scan.size(); ++i) {
         double r = scan.ranges[i];
@@ -241,12 +246,14 @@ 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
+    // Draw four edges using pre-computed rotated corner points
     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}};
+    Seg segs[] = {
+        {fence.p_lb_x,fence.p_lb_y, fence.p_lt_x,fence.p_lt_y},  // left
+        {fence.p_rb_x,fence.p_rb_y, fence.p_rt_x,fence.p_rt_y},  // right
+        {fence.p_lb_x,fence.p_lb_y, fence.p_rb_x,fence.p_rb_y},  // bottom
+        {fence.p_lt_x,fence.p_lt_y, fence.p_rt_x,fence.p_rt_y},  // top
+    };
     int t = (int)(thickness / GRID_RES) + 1;
     for (auto& s : segs) {
         int gx1, gy1, gx2, gy2;

src/laser_safety.cpp

@@ -0,0 +1,50 @@
+#include "car_nav_lite/laser_safety.hpp"
+#include <cmath>
+#include <algorithm>
+
+namespace car_nav_lite {
+
+Twist LaserSafetyFilter::filter(const Twist& cmd, const LaserScan& scan) const {
+    Twist out = cmd;
+    if (!enable || scan.size() < 10) return out;
+
+    double front_min = 999.0;
+    double left_sum = 0, right_sum = 0;
+    int left_n = 0, right_n = 0;
+    double half_w = front_width;
+    double stop = stop_dist + std::max(0.0, cmd.vx) * 0.5;
+    double slow = slow_dist + std::max(0.0, cmd.vx) * 0.5;
+
+    for (int i = 0; i < scan.size(); ++i) {
+        double r = scan.ranges[i];
+        if (r < 0.10 || r > max_range) continue;
+        double a = scan.angle_min + i * scan.angle_increment;
+        double x = r * std::cos(a);  // body-frame
+        double y = r * std::sin(a);
+        if (x <= 0) continue;  // behind car
+
+        // Front safety zone
+        if (std::abs(y) < half_w && x < front_min) front_min = x;
+
+        // Side clearance for evasive turn
+        if (x > 0 && x < 1.0) {
+            if (y > 0) { left_sum += r; left_n++; }
+            else       { right_sum += r; right_n++; }
+        }
+    }
+
+    double left_avg = left_n > 0 ? left_sum / left_n : 0;
+    double right_avg = right_n > 0 ? right_sum / right_n : 0;
+
+    if (front_min < stop) {
+        // Ackermann: cannot turn in place. Stop means stop.
+        out.vx = 0.0;
+        out.wz = 0.0;
+    } else if (front_min < slow) {
+        out.vx *= 0.3;
+    }
+
+    return out;
+}
+
+}  // namespace car_nav_lite

src/local_planner.cpp

@@ -77,8 +77,20 @@ double LocalPlanner::evaluateTrajectory(
     double end_to_goal = std::hypot(goal.x - sx, goal.y - sy);
     cost += W_FOLLOW * end_to_goal;
 
-    // ── Reverse penalty (mild) ──
-    if (vx < 0) cost += std::abs(vx) * 3.0;
+    // ── Progress critic: reward forward progress along path ──
+    if (total_path_len > 0.5) {
+        // Find start and end path indices
+        size_t s0 = 0, s1 = 0;
+        double d0 = 1e9, d1 = 1e9;
+        for (size_t i = 0; i < path.size(); ++i) {
+            double ds = std::hypot(pose.x - path[i].x, pose.y - path[i].y);
+            if (ds < d0) { d0 = ds; s0 = i; }
+            double de = std::hypot(sx - path[i].x, sy - path[i].y);
+            if (de < d1) { d1 = de; s1 = i; }
+        }
+        double progress = std::max(0.0, path_dists[s1] - path_dists[s0]);
+        cost += 20.0 * std::max(0.0, 0.25 - progress);  // penalty for too little progress
+    }
 
     return cost;
 }
@@ -107,20 +119,22 @@ Twist LocalPlanner::compute(const Pose2D& pose,
     for (int k = 0; k < K; ++k) {
         double vx, wz;
         if (k < exploit_n) {
-            // Exploit: sample around previous optimal
+            // Exploit: sample v + kappa (Ackermann-compatible)
             vx = prev_vx_ + VX_STD * randNormal();
-            wz = prev_wz_ + WZ_STD * randNormal();
+            double kappa = (prev_vx_ > 0.01) ? (prev_wz_ / prev_vx_ + WZ_STD * randNormal()) : (WZ_STD * randNormal());
+            wz = vx * kappa;
         } else {
-            // Explore: uniform random across full range (including reverse)
+            // Explore: uniform vx + kappa (Ackermann-compatible)
             uint32_t x = rng_state_;
             x ^= x << 13; x ^= x >> 17; x ^= x << 5; rng_state_ = x;
-            vx = (double)(x & 0xFFFF) / 65535.0 * (ms_ + 0.5) - 0.5;
+            vx = (double)(x & 0xFFFF) / 65535.0 * ms_;
             x ^= x << 13; x ^= x >> 17; x ^= x << 5; rng_state_ = x;
-            wz = ((double)(x & 0xFFFF) / 65535.0 - 0.5) * 3.0;
+            double kappa = ((double)(x & 0xFFFF) / 65535.0 - 0.5) * (2.0 / mr_);
+            wz = vx * kappa;
         }
 
-        // Clamp + Ackermann (no hard wz limit — Ackermann constraint is enough)
-        vx = limit(vx, -0.5, ms_);
+        // Clamp + Ackermann: no reverse, curvature-based wz
+        vx = limit(vx, 0.0, ms_);
         double max_wz = std::abs(vx) / mr_;
         wz = limit(wz, -max_wz, max_wz);
 
@@ -152,9 +166,6 @@ Twist LocalPlanner::compute(const Pose2D& pose,
     cmd.vx = weighted_vx / sum_weights;
     cmd.wz = weighted_wz / sum_weights;
 
-    // Enforce minimum reverse speed
-    if (cmd.vx < 0 && cmd.vx > -0.3) cmd.vx = -0.3;
-
     cmd.wz = limit(cmd.wz, -std::abs(cmd.vx) / mr_, std::abs(cmd.vx) / mr_);
 
     prev_vx_ = cmd.vx;

src/localizer.cpp

@@ -40,7 +40,51 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
     const int N = scan.size();
     Pose2D base = base_override ? *base_override : odom_pose_;
 
-    // ── 1. Render scan to local image ──
+    // ── 1. Point-to-Line2D residual (primary, no Hough needed) ──
+    if (use_fence_ && fence_.valid) {
+        constexpr double GATE = 0.20;
+        double H[9]={0}, b2[3]={0}; double sum_r2=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*cos(a+base.yaw),wy=base.y+r*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;sum_r2+=res*res;
+            double qx=r*cos(a),qy=r*sin(a);
+            double dRx=-sin(base.yaw)*qx-cos(base.yaw)*qy,dRy=cos(base.yaw)*qx-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;
+        }
+        double loc_rms = (n_pts>0) ? std::sqrt(sum_r2/n_pts) : 999.0;
+        double conf_pts = std::min(1.0, n_pts/80.0);
+        double conf_rms = std::min(1.0, 0.08/std::max(loc_rms,1e-3));
+        double quality = conf_pts * conf_rms;
+        loc_rms_=loc_rms; loc_conf_=quality*0.9; loc_lines_=n_pts/10;
+
+        // Only apply correction if quality is adequate
+        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 (n_pts>20 && quality>0.25 && 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.2*dx;correction_.y+=0.2*dy;correction_.yaw+=0.2*da;
+            correction_.x=limit(correction_.x,-0.30,0.30);correction_.y=limit(correction_.y,-0.30,0.30);
+            correction_.yaw=limit(correction_.yaw,-0.25,0.25);
+            loc_dx_=correction_.x;loc_dy_=correction_.y;loc_dyaw_=correction_.yaw;
+            return;
+        }
+        // Quality insufficient — don't update correction, fall through to Hough
+        if (use_fence_) return;  // fence exists: trust quality gate, skip Hough entirely
+    }
+
+    // ── 2. Hough fallback: render scan to image + line detection ──
     constexpr int IMG_SZ = 600, HALF = IMG_SZ / 2;
     int cx_g, cy_g;
     map.worldToGrid(base.x, base.y, cx_g, cy_g);
@@ -130,56 +174,11 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
         }
     }
 
-    // ── 5. Update offset ──
+    // ── 5. Hough update ──
     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;
-
-    // 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) {
+    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

@@ -21,6 +21,8 @@
 #include "car_nav_lite/bt_executor.hpp"
 #include "car_nav_lite/fence_model.hpp"
 #include "car_nav_lite/session_mapper.hpp"
+#include "car_nav_lite/cone_updater.hpp"
+#include "car_nav_lite/laser_safety.hpp"
 
 using namespace std::chrono_literals;
 using namespace car_nav_lite;
@@ -46,6 +48,7 @@ public:
         declare_parameter("wall_ransac_thresh", 0.04);
         declare_parameter("cone_mark_radius", 0.18);
         declare_parameter("dynamic_obstacle_enable", false);
+        declare_parameter("online_cone_enable", true);
         _read_params();
 
         param_cb_ = add_on_set_parameters_callback(
@@ -64,6 +67,7 @@ public:
                     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();
+                    else if (n=="online_cone_enable") online_cone_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_);
@@ -116,7 +120,7 @@ private:
     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;
+    bool dynamic_obstacle_enable_=false, online_cone_enable_=true;
     rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_;
     void _read_params(){
         mf_=get_parameter("map_file").as_string();
@@ -132,6 +136,7 @@ private:
         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();
+        online_cone_enable_=get_parameter("online_cone_enable").as_bool();
     }
 
     // ── State ──
@@ -144,6 +149,10 @@ private:
     GridMap map_; Localizer localizer_; BTExecutor bt_;
     SessionMapper session_mapper_;
     FenceModel fence_model_;
+    ConeUpdater cone_updater_;
+    LaserSafetyFilter safety_filter_;
+    std::vector<Eigen::Vector2d> known_cones_;
+    rclcpp::Time last_cone_tick_{0,0,RCL_ROS_TIME};
     enum SessionState { CALIBRATING, READY };
     SessionState session_state_ = CALIBRATING;
     int calib_cnt_ = 0;
@@ -172,6 +181,7 @@ private:
                 std::vector<Eigen::Vector2d> cones;
                 if (session_mapper_.build(fence_model_, cones, fence_expected_size_, fence_size_tolerance_, wall_ransac_thresh_)) {
                     localizer_.setFenceModel(fence_model_);
+                    known_cones_ = cones;
                     for (auto& c : cones) map_.markCone(c.x(), c.y(), cone_mark_radius_, 255);
                     map_.markFenceModel(fence_model_);
                     map_.morphologyClose(3);
@@ -203,7 +213,7 @@ private:
         last_imu_t_=now;latest_gyro_z_=m->angular_velocity.z;
         if(dt>0&&dt<1.0)localizer_.updateIMU(latest_gyro_z_,dt);}
     void goal_cb(PoseStMsg::SharedPtr m){std::lock_guard lk(mtx_);
-        if(m->header.frame_id!="map"){RCLCPP_WARN(get_logger(),"goal frame not map");return;}
+        if(m->header.frame_id!="map"){RCLCPP_WARN(get_logger(),"goal frame not map: '%s'",m->header.frame_id.c_str());return;}
         Pose2D g(m->pose.position.x,m->pose.position.y,0.0);
         double qw=m->pose.orientation.w,qz=m->pose.orientation.z;
         if(qw!=0||qz!=0)g.yaw=std::atan2(2.0*qw*qz,qw*qw-qz*qz);
@@ -248,16 +258,30 @@ private:
 
     void tick(){
         std::lock_guard lk(mtx_);
+        auto pose=localizer_.pose();publish_tf(pose);
         // CALIBRATING: force zero cmd, wait for fence model
         if (session_state_ == CALIBRATING) {
-            auto zero = geometry_msgs::msg::Twist();
-            pub_cmd_->publish(zero);
+            pub_cmd_->publish(geometry_msgs::msg::Twist());
             return;
         }
-        auto pose=localizer_.pose();publish_tf(pose);
-        // READY: only update scan if dynamic obstacles enabled
-        if(has_scan_ && dynamic_obstacle_enable_)
+        // READY: online cone discovery (3Hz) + optional dynamic obstacles
+        if (has_scan_) {
+            auto cone_now = this->now();
+            if (online_cone_enable_ && (cone_now - last_cone_tick_).seconds() > 0.3) {
+                last_cone_tick_ = cone_now;
+                cone_updater_.update(latest_scan_, pose, fence_model_, known_cones_);
+                auto new_cones = cone_updater_.popConfirmed();
+                for (auto& c : new_cones) {
+                    known_cones_.push_back(c);
+                    map_.markCone(c.x(), c.y(), cone_mark_radius_, 255);
+                    fprintf(stderr, "[Session] new cone: (%.2f,%.2f) total=%zu\n",
+                            c.x(), c.y(), known_cones_.size());
+                }
+                if (!new_cones.empty()) map_.composeLayers();
+            }
+            if (dynamic_obstacle_enable_)
                 map_.updateScan(latest_scan_, pose);
+        }
 
         // Test mode: auto-activate figure-8 after 3s
         if(test_&&!tps_){
@@ -276,6 +300,11 @@ private:
         // Slow down if localization confidence is low
         if (localizer_.loc_conf_ < 0.3) { cmd.vx *= 0.4; cmd.wz *= 0.4; }
 
+        // Laser safety filter: last-moment scan check
+        if (has_scan_ && session_state_ == READY) {
+            Twist safe = safety_filter_.filter({(float)cmd.vx, (float)cmd.wz}, latest_scan_);
+            cmd.vx = safe.vx; cmd.wz = safe.wz;
+        }
         auto tw=geometry_msgs::msg::Twist();tw.linear.x=cmd.vx;tw.angular.z=cmd.wz;
         pub_cmd_->publish(tw);
         if(!bt_.currentPath().empty()){
@@ -314,10 +343,10 @@ private:
         char buf[512];
         snprintf(buf, sizeof(buf),
             "{\"state\":\"%s\",\"x\":%.2f,\"y\":%.2f,\"yaw\":%.1f,"
-            "\"loc_conf\":%.2f,\"loc_lines\":%d,\"loc_dx\":%.3f,\"loc_dy\":%.3f,\"loc_dyaw\":%.2f,"
+            "\"loc_conf\":%.2f,\"loc_rms\":%.3f,\"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,
-            localizer_.loc_conf_, m.loc_lines, m.loc_dx, m.loc_dy, m.loc_dyaw*57.3,
+            localizer_.loc_conf_, localizer_.loc_rms_, 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

@@ -89,12 +89,16 @@ bool SessionMapper::build(FenceModel& fence, std::vector<Eigen::Vector2d>& cones
     double y_min = ys[lo_idx], y_max = ys[hi_idx];
     double w = x_max - x_min, h = y_max - y_min;
 
-    // Validate size
+    // Validate size — allow known-size fallback if partial fence
     if (std::abs(w - expected_size) > size_tol ||
         std::abs(h - expected_size) > size_tol) {
-        fprintf(stderr, "[SessionMapper] size mismatch: %.2fx%.2f (expected %.1f±%.1f)\n",
-                w, h, expected_size, size_tol);
-        return false;
+        fprintf(stderr, "[SessionMapper] partial fence %.2fx%.2f, fallback to known %.1fx%.1f\n",
+                w, h, expected_size, expected_size);
+        // Use known field size centered at origin with estimated yaw
+        double half = expected_size / 2.0;
+        x_min = -half; x_max = half;
+        y_min = -half; y_max = half;
+        w = h = expected_size;
     }
 
     fence.width = w; fence.height = h;
@@ -115,6 +119,7 @@ bool SessionMapper::build(FenceModel& fence, std::vector<Eigen::Vector2d>& cones
     Eigen::Vector2d tw = rotBack(fence.cx, y_max);
     fence.left_x = lw.x(); fence.right_x = rw.x();
     fence.bottom_y = bw.y(); fence.top_y = tw.y();
+    // Corner points for drawing rotated fence (lb/lt/rb/rt are already computed above)
 
     // 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);
@@ -126,6 +131,11 @@ bool SessionMapper::build(FenceModel& fence, std::vector<Eigen::Vector2d>& cones
     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.p_lb_x=lb.x();fence.p_lb_y=lb.y();
+    fence.p_lt_x=lt.x();fence.p_lt_y=lt.y();
+    fence.p_rb_x=rb.x();fence.p_rb_y=rb.y();
+    fence.p_rt_x=rt.x();fence.p_rt_y=rt.y();
+
     fence.valid = true;
 
     // ── 5. Extract cones: non-wall points → cluster ──