MPPI + fence-line loc + waypoint nav with fail skip

include/car_nav_lite/bt_executor.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <functional>
 #include <optional>
 #include "car_nav_lite/types.hpp"
 #include "car_nav_lite/grid_map.hpp"
@@ -16,6 +17,7 @@ public:
     void setGoal(const Pose2D& goal);
     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); }
     bool hasGoal() const { return goal_.has_value(); }
 
     Twist tick(const Pose2D& pose, const GridMap& map);
@@ -29,6 +31,7 @@ private:
     std::vector<Pose2D> current_path_;
     GlobalPlanner planner_;
     LocalPlanner  local_planner_;
+    std::function<void()> fail_callback_;
 
     bool atGoal(const Pose2D& pose) const;
 };

include/car_nav_lite/grid_map.hpp

@@ -41,6 +41,9 @@ public:
     // ── Obstacle update from scan ──────────────────────
     void updateScan(const LaserScan& scan, const Pose2D& pose);
 
+    // ── Morphological close (remove noise, merge nearby blobs) ──
+    void morphologyClose(int kernel_size = 3);
+
     // ── Pre-built obstacle marks ───────────────────────
     void markCircle(double wx, double wy, double radius, uint8_t value = 255);
     void markRect(double x1, double y1, double x2, double y2, uint8_t value = 255);

include/car_nav_lite/localizer.hpp

@@ -10,19 +10,13 @@ class Localizer {
 public:
     Localizer();
 
-    // ── Odometry update (called at odom rate) ──────────
     void updateOdom(double vx, double wz, double dt);
     void updateIMU(double gyro_z, double dt);
-
-    // Set raw odom pose. pose() returns odom + correction offset.
     void setOdomPose(const Pose2D& p);
 
-    // ── Scan-to-map correction (updates persistent offset) ──
+    void correctWithScanCV(const LaserScan& scan, const GridMap& map,
-    // Correction = matched_pose - odom_pose, stored as offset.
+                            const Pose2D* base_override = nullptr);
-    // Only updates if confidence is high; otherwise offset decays to zero.
-    void correctWithScanCV(const LaserScan& scan, const GridMap& map);
 
-    // ── Final pose = odom + correction offset ──────────
     Pose2D pose() const;
 
 private:

scripts/waypoint_nav.py

@@ -0,0 +1,148 @@
+#!/usr/bin/env python3
+"""Sequential waypoint navigation for car_nav_lite.
+
+Publishes /goal_pose waypoints one by one. Waits for the car to reach
+each waypoint before publishing the next.
+
+Usage:
+  python3 waypoint_nav.py                           # default waypoints
+  python3 waypoint_nav.py --waypoints waypoints.json  # from JSON file
+  python3 waypoint_nav.py --loop                      # loop back to start
+
+JSON format:
+  [{"x": 1.0, "y": -1.5, "yaw": 0.0}, ...]
+"""
+
+import argparse
+import json
+import math
+import time
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Odometry
+from std_msgs.msg import String
+
+
+# Default waypoints for 5m×5m arena (car spawns near -2.0, -2.3)
+DEFAULT_WAYPOINTS = [
+    {"x": -0.5, "y": -1.5, "yaw": 0.0, "desc": "mid-left"},
+    {"x":  1.0, "y": -1.5, "yaw": 0.0, "desc": "right"},
+    {"x":  1.5, "y":  0.0, "yaw": 0.0, "desc": "top-right"},
+    {"x":  0.0, "y":  1.5, "yaw": 0.0, "desc": "top-center"},
+    {"x": -1.5, "y":  1.0, "yaw": 0.0, "desc": "top-left"},
+    {"x": -1.5, "y": -0.5, "yaw": 0.0, "desc": "left"},
+]
+
+
+class WaypointNav(Node):
+    def __init__(self, waypoints, loop=False, threshold=0.25):
+        super().__init__("waypoint_nav")
+        self.waypoints = waypoints
+        self.loop = loop
+        self.threshold = threshold
+        self.idx = 0
+        self.reached = False
+        self.fail_gen = 0
+        self.skip_gen = -1
+        self.skip_streak = 0     # consecutive skips
+        self.pose_x = self.pose_y = self.pose_yaw = 0.0
+
+        from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
+        qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE,
+                         durability=DurabilityPolicy.TRANSIENT_LOCAL)
+        self.pub_goal = self.create_publisher(PoseStamped, "/goal_pose", qos)
+        self.sub_odom = self.create_subscription(
+            Odometry, "/odom", self.odom_cb, 10)
+        self.sub_status = self.create_subscription(
+            String, "/nav_status", self.status_cb, 10)
+        self.timer = self.create_timer(0.2, self.check)  # 5Hz
+
+    def odom_cb(self, msg):
+        self.pose_x = msg.pose.pose.position.x
+        self.pose_y = msg.pose.pose.position.y
+        q = msg.pose.pose.orientation
+        self.pose_yaw = math.atan2(2*(q.w*q.z + q.x*q.y), 1 - 2*(q.y*q.y + q.z*q.z))
+
+    def status_cb(self, msg):
+        if msg.data == "fail" and self.reached and self.fail_gen != self.skip_gen:
+            self.skip_streak += 1
+            if self.skip_streak > 3:
+                self.get_logger().error("⏭ 3+ consecutive skips — stopping")
+                return
+            self.get_logger().warn(f"⏭ WP {self.idx+1} plan failed — skip #{self.skip_streak}")
+            self.skip_gen = self.fail_gen
+            self.idx += 1
+            self.reached = False
+
+    def send_goal(self, wp):
+        msg = PoseStamped()
+        msg.header.frame_id = "map"
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.pose.position.x = float(wp["x"])
+        msg.pose.position.y = float(wp["y"])
+        yaw_deg = float(wp.get("yaw", 0.0))
+        yaw = math.radians(yaw_deg)  # JSON stores degrees
+        msg.pose.orientation.z = math.sin(yaw / 2.0)
+        msg.pose.orientation.w = math.cos(yaw / 2.0)
+        self.pub_goal.publish(msg)
+        desc = wp.get("desc", "")
+        self.get_logger().info(
+            f"→ WP {self.idx+1}/{len(self.waypoints)}: "
+            f"({wp['x']:.2f},{wp['y']:.2f}) {desc}"
+        )
+
+    def check(self):
+        if self.idx >= len(self.waypoints):
+            if self.loop:
+                self.idx = 0
+                self.reached = False
+            else:
+                return
+
+        wp = self.waypoints[self.idx]
+        if not self.reached:
+            self.fail_gen += 1  # new generation for this wp
+            self.send_goal(wp)
+            self.reached = True
+
+        dist = math.hypot(self.pose_x - wp["x"], self.pose_y - wp["y"])
+        if dist < self.threshold:
+            self.get_logger().info(
+                f"✓ WP {self.idx+1} reached (dist={dist:.2f}m)")
+            self.idx += 1
+            self.reached = False
+            self.skip_streak = 0
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--waypoints", type=str, default=None,
+                        help="JSON file with waypoints")
+    parser.add_argument("--loop", action="store_true",
+                        help="Loop back to first waypoint after last")
+    parser.add_argument("--threshold", type=float, default=0.3,
+                        help="Arrival distance threshold (m)")
+    args = parser.parse_args()
+
+    if args.waypoints:
+        with open(args.waypoints) as f:
+            waypoints = json.load(f)
+        print(f"Loaded {len(waypoints)} waypoints from {args.waypoints}")
+    else:
+        waypoints = DEFAULT_WAYPOINTS
+        print(f"Using {len(waypoints)} default waypoints")
+
+    rclpy.init()
+    node = WaypointNav(waypoints, args.loop, args.threshold)
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

scripts/waypoints.json

@@ -0,0 +1,110 @@
+[
+  {
+    "x": 2.044,
+    "y": -1.656,
+    "yaw": 71,
+    "desc": "wp0"
+  },
+  {
+    "x": 1.256,
+    "y": -0.756,
+    "yaw": 166,
+    "desc": "wp1"
+  },
+  {
+    "x": 0.056,
+    "y": -0.45,
+    "yaw": 90,
+    "desc": "wp2"
+  },
+  {
+    "x": 0.031,
+    "y": 0.25,
+    "yaw": 64,
+    "desc": "wp3"
+  },
+  {
+    "x": 0.312,
+    "y": 0.694,
+    "yaw": 22,
+    "desc": "wp4"
+  },
+  {
+    "x": 1.475,
+    "y": 0.694,
+    "yaw": 14,
+    "desc": "wp5"
+  },
+  {
+    "x": 1.75,
+    "y": 1.469,
+    "yaw": 109,
+    "desc": "wp6"
+  },
+  {
+    "x": 1.387,
+    "y": 1.8,
+    "yaw": 163,
+    "desc": "wp7"
+  },
+  {
+    "x": 0.369,
+    "y": 1.988,
+    "yaw": 179,
+    "desc": "wp8"
+  },
+  {
+    "x": -0.381,
+    "y": 1.988,
+    "yaw": -180,
+    "desc": "wp9"
+  },
+  {
+    "x": -1.487,
+    "y": 1.919,
+    "yaw": -157,
+    "desc": "wp10"
+  },
+  {
+    "x": -1.825,
+    "y": 1.244,
+    "yaw": -90,
+    "desc": "wp11"
+  },
+  {
+    "x": -1.756,
+    "y": 0.794,
+    "yaw": -49,
+    "desc": "wp12"
+  },
+  {
+    "x": -0.844,
+    "y": 0.544,
+    "yaw": -7,
+    "desc": "wp13"
+  },
+  {
+    "x": -0.388,
+    "y": 0.388,
+    "yaw": -64,
+    "desc": "wp14"
+  },
+  {
+    "x": -0.169,
+    "y": -0.106,
+    "yaw": -96,
+    "desc": "wp15"
+  },
+  {
+    "x": -0.169,
+    "y": -0.506,
+    "yaw": -127,
+    "desc": "wp16"
+  },
+  {
+    "x": -1.956,
+    "y": -2.175,
+    "yaw": -90,
+    "desc": "wp17"
+  }
+]

src/bt_executor.cpp

@@ -35,9 +35,11 @@ Twist BTExecutor::tick(const Pose2D& pose, const GridMap& map) {
                 fprintf(stderr, "[BT] Plan OK, %zu waypoints\n", current_path_.size());
                 state_ = NavState::TRACKING;
             } else {
-                fprintf(stderr, "[BT] Plan FAILED to (%.2f,%.2f)\n",
+                static int fail_cnt = 0;
-                        goal_->x, goal_->y);
+                fprintf(stderr, "[BT] Plan FAILED #%d to (%.2f,%.2f)\n",
+                        ++fail_cnt, goal_->x, goal_->y);
                 state_ = NavState::FAILED;
+                fail_callback_();
             }
         }
         break;
@@ -54,7 +56,7 @@ Twist BTExecutor::tick(const Pose2D& pose, const GridMap& map) {
         if (++tick_cnt % 5 == 0 && goal_) {
             static const int inflations[] = {
                 INFLATION_CELLS, INFLATION_CELLS*4/5,
-                (int)(CAR_HALF_W / GRID_RES)
+                (int)(CAR_HALF_W / GRID_RES), 0  // last resort: no inflation
             };
             std::vector<Pose2D> new_path;
             for (int inflate : inflations) {
@@ -79,12 +81,6 @@ Twist BTExecutor::tick(const Pose2D& pose, const GridMap& map) {
 bool BTExecutor::atGoal(const Pose2D& pose) const {
     if (!goal_) return false;
     if (pose.distTo(*goal_) > 0.20) return false;
-    if (std::abs(goal_->yaw) > 0.01) {
-        double h_err = goal_->yaw - pose.yaw;
-        while (h_err > M_PI) h_err -= 2*M_PI;
-        while (h_err < -M_PI) h_err += 2*M_PI;
-        return std::abs(h_err) < 0.15;
-    }
     return true;
 }
 

src/bt_executor.hpp

@@ -13,19 +13,16 @@ class BTExecutor {
 public:
     BTExecutor();
 
-    // ── Set navigation goal ────────────────────────────
     void setGoal(const Pose2D& goal);
     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); }
     bool hasGoal() const { return goal_.has_value(); }
 
-    // ── Tick the state machine (call at 10Hz) ──────────
     Twist tick(const Pose2D& pose, const GridMap& map);
 
-    // ── Status query ───────────────────────────────────
     NavState state() const { return state_; }
     const std::vector<Pose2D>& currentPath() const { return current_path_; }
-    bool hasGoal() const { return goal_.has_value(); }
 
 private:
     NavState state_ = NavState::IDLE;
@@ -33,13 +30,9 @@ private:
     std::vector<Pose2D> current_path_;
     GlobalPlanner planner_;
     LocalPlanner  local_planner_;
-    int replan_count_ = 0;
+    std::function<void()> fail_callback_;
-    int stuck_count_  = 0;
-    int recovery_attempts_ = 0;
-    Pose2D recovery_target_;
 
     bool atGoal(const Pose2D& pose) const;
-    bool tryRecover(const Pose2D& pose, const GridMap& map);
 };
 
 }  // namespace car_nav_lite

src/grid_map.cpp

@@ -3,6 +3,7 @@
 #include <sstream>
 #include <cmath>
 #include <algorithm>
+#include <opencv2/imgproc.hpp>
 
 namespace car_nav_lite {
 
@@ -235,6 +236,13 @@ double GridMap::raycast(double wx, double wy, double angle, double max_range) co
     return max_range;  // no hit
 }
 
+void GridMap::morphologyClose(int kernel_size) {
+    cv::Mat img(GRID_SIZE, GRID_SIZE, CV_8UC1, cells_.data());
+    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE,
+                                                cv::Size(kernel_size, kernel_size));
+    cv::morphologyEx(img, img, cv::MORPH_CLOSE, kernel);
+}
+
 void GridMap::markRect(double x1, double y1, double x2, double y2, uint8_t value) {
     int gx1, gy1, gx2, gy2;
     worldToGrid(x1, y1, gx1, gy1);

src/grid_map.hpp

@@ -0,0 +1,61 @@
+#pragma once
+
+#include "car_nav_lite/types.hpp"
+#include <string>
+#include <array>
+
+namespace car_nav_lite {
+
+// ── 8-connected neighbor deltas ──────────────────────────
+constexpr std::array<std::pair<int,int>, 8> NEIGHBORS = {{
+    {1,0},{-1,0},{0,1},{0,-1},{1,1},{-1,-1},{1,-1},{-1,1}
+}};
+
+class GridMap {
+public:
+    GridMap();
+
+    // ── Load PGM P5 from file ──────────────────────────
+    bool loadPGM(const std::string& path);
+
+    // ── Coordinate transforms ──────────────────────────
+    bool worldToGrid(double wx, double wy, int& gx, int& gy) const;
+    void gridToWorld(int gx, int gy, double& wx, double& wy) const;
+
+    // ── Access ──────────────────────────────────────────
+    uint8_t at(int gx, int gy) const;
+    void set(int gx, int gy, uint8_t v);
+    bool inBounds(int gx, int gy) const;
+    bool isFree(int gx, int gy) const;
+    bool isFree(int gx, int gy, int inflate_cells) const;  // configurable inflation
+
+    // ── Raycast on static obstacles (for scan-to-map matching) ──
+    // Returns range to first obstacle >128 along (wx,wy) → angle.
+    double raycast(double wx, double wy, double angle, double max_range = 5.0) const;
+
+    // ── Oriented footprint collision check (EXACT-MPPI style) ──
+    // Checks raw occupancy against car's oriented rectangle at (wx,wy,yaw).
+    // Handles car width/length correctly regardless of orientation.
+    bool isFreeFootprint(double wx, double wy, double yaw) const;
+
+    // ── Obstacle update from scan ──────────────────────
+    void updateScan(const LaserScan& scan, const Pose2D& pose);
+
+    // ── Morphological close (remove noise, merge nearby blobs) ──
+    void morphologyClose(int kernel_size = 3);
+
+    // ── Pre-built obstacle marks ───────────────────────
+    void markCircle(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 ────────────────────────────────────────
+    int width()  const { return GRID_SIZE; }
+    int height() const { return GRID_SIZE; }
+    const OccupancyGrid& data() const { return cells_; }
+
+private:
+    OccupancyGrid cells_;
+    void bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val);
+};
+
+}  // namespace car_nav_lite

src/local_planner.cpp

@@ -162,6 +162,9 @@ 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;
+
     double max_wz = std::abs(cmd.vx) / mr_;
     cmd.wz = limit(cmd.wz, -max_wz, max_wz);
 

src/localizer.cpp

@@ -32,27 +32,21 @@ Pose2D Localizer::pose() const {
             odom_pose_.yaw + correction_.yaw};
 }
 
-// ── Fence-line-based scan correction ───────────────────────
+// ── Scan-to-map correction (fence-line based, offset mode) ──
-//
+void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
-// The 5m×5m arena has a rectangular fence. We detect Hough lines
+                                   const Pose2D* base_override) {
-// in the scan, keep near-H/V ones (= fence walls), and compare
-// their world positions against the known fence at x=±2.5, y=±2.5.
-// This gives dx,dy correction. Yaw comes from the line angle bias.
-//
-void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map) {
     if (scan.size() < 30) return;
 
     const int N = scan.size();
-    const Pose2D& base = odom_pose_;
+    Pose2D base = base_override ? *base_override : odom_pose_;
 
-    // ── 1. Render scan to local image (world coords, ±1.5m around car) ──
+    // ── 1. Render scan to local image ──
-    constexpr int IMG_SZ = 600;   // 3m × 3m at 5mm
+    constexpr int IMG_SZ = 600, HALF = IMG_SZ / 2;
-    constexpr int HALF = IMG_SZ / 2;
     int cx_g, cy_g;
     map.worldToGrid(base.x, base.y, cx_g, cy_g);
 
     cv::Mat scan_img(IMG_SZ, IMG_SZ, CV_8UC1, cv::Scalar(0));
-    int hit_count = 0;
+    int point_count = 0;
     for (int i = 0; i < N; i += 2) {
         double r = scan.ranges[i];
         if (r < 0.1 || r > 4.5) continue;
@@ -65,47 +59,41 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map) {
         int py = (gy - cy_g) + HALF;
         if (px >= 0 && px < IMG_SZ && py >= 0 && py < IMG_SZ) {
             scan_img.at<uint8_t>(py, px) = 255;
-            ++hit_count;
+            ++point_count;
         }
     }
-    if (hit_count < 30) return;
+    if (point_count < 30) return;
 
-    // ── 2. Hough line detection ────────────────────────────
+    // ── 2. Hough lines ──
     std::vector<cv::Vec4i> lines;
-    cv::HoughLinesP(scan_img, lines,
+    cv::HoughLinesP(scan_img, lines, 1, CV_PI/180.0, 20, 40, 10);
-                    1, CV_PI / 180.0,   // rho=1px, theta=1°
-                    20,                  // vote threshold
-                    40,                  // min line length (40px = 20cm)
-                    10);                 // max gap
 
-    // ── 3. Fence line analysis (no angle filter — car yaw can be anything) ──
+    // ── 3. Yaw from line deviations ──
-    // Use double-angle voting: 0° and 90° fence lines both map to 0° after
+    double sum_dyaw = 0;
-    // doubling. Deviation of the dominant direction from 0 = yaw error.
-    double sum_cos2 = 0, sum_sin2 = 0;
     int yaw_count = 0;
-
     for (auto& l : lines) {
         double ldx = l[2] - l[0], ldy = l[3] - l[1];
         if (ldx == 0 && ldy == 0) continue;
-        double ang = std::atan2(ldy, ldx);  // [-π, π]
+        double ang = std::atan2(ldy, ldx);
-        sum_cos2 += std::cos(2.0 * ang);
+        double a0 = ang;
-        sum_sin2 += std::sin(2.0 * ang);
+        while (a0 > M_PI/2) a0 -= M_PI/2;
+        while (a0 < -M_PI/2) a0 += M_PI/2;
+        sum_dyaw += (std::abs(a0) < std::abs(a0 - M_PI/2)) ? a0 : (a0 - M_PI/2);
         ++yaw_count;
     }
 
-    if (yaw_count < 3) {
+    // ── Debug ──
-        correction_.x *= 0.9; correction_.y *= 0.9;
+    static int yaw_dbg = 0;
-        return;
+    if (yaw_count >= 2 && ++yaw_dbg % 5 == 0) {
+        double dyaw_tmp = yaw_count > 0 ? sum_dyaw / yaw_count : 0;
+        fprintf(stderr, "[YAW] odom=%.1f° scan=%.1f° dyaw=%.2f° lines=%d\n",
+                base.yaw * 57.3, (base.yaw - dyaw_tmp) * 57.3, dyaw_tmp * 57.3, yaw_count);
     }
 
-    // Dominant fence orientation → yaw error
+    if (yaw_count < 2) { correction_.x *= 0.9; correction_.y *= 0.9; return; }
-    double dyaw = std::atan2(sum_sin2, sum_cos2) / 2.0;
+    double dyaw = sum_dyaw / yaw_count;
-    while (dyaw >  M_PI/4) dyaw -= M_PI/2;
-    while (dyaw < -M_PI/4) dyaw += M_PI/2;
 
-    // ── 4. Re-classify lines AFTER yaw correction ──────────
+    // ── 4. Classify lines & compute translation ──
-    // Now we know the fence direction. Lines at ang≈dyaw or ang≈dyaw+π/2
-    // are fence walls.
     double sum_dx = 0, sum_dy = 0;
     int line_count = 0;
     constexpr double TOL = 15.0 * M_PI / 180.0;
@@ -114,31 +102,23 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map) {
         double ldx = l[2] - l[0], ldy = l[3] - l[1];
         if (ldx == 0 && ldy == 0) continue;
         double ang = std::atan2(ldy, ldx);
-        if (ang < 0) ang += M_PI;  // [0, π)
+        if (ang < 0) ang += M_PI;
 
-        // Check if this line aligns with the detected fence direction
         double diff1 = std::abs(ang - dyaw);
         while (diff1 > M_PI) diff1 -= M_PI;
         if (diff1 > M_PI/2) diff1 = M_PI - diff1;
-
         double diff2 = std::abs(ang - (dyaw + M_PI/2));
         while (diff2 > M_PI) diff2 -= M_PI;
         if (diff2 > M_PI/2) diff2 = M_PI - diff2;
-
+        if (std::min(diff1, diff2) > TOL) continue;
-        double diff = std::min(diff1, diff2);
-        if (diff > TOL) continue;  // not aligned with fence
 
         ++line_count;
-
-        // Line midpoint → world coords → compare with known fence (±2.5m)
         double mx = (l[0] + l[2]) / 2.0;
         double my = (l[1] + l[3]) / 2.0;
         double mwx, mwy;
         map.gridToWorld(cx_g - HALF + (int)mx, cy_g - HALF + (int)my, mwx, mwy);
 
-        // Which fence wall? Use the corrected yaw to determine H vs V
+        bool is_horiz = (diff1 < diff2);
-        bool is_horiz = (diff1 < diff2);  // closer to dyaw = horizontal in fence frame
-
         if (is_horiz) {
             double expected = (mwy > 0) ? 2.5 : -2.5;
             sum_dy += (expected - mwy);
@@ -148,35 +128,25 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map) {
         }
     }
 
-    if (line_count < 2) {
+    // ── 5. Update offset ──
-        correction_.yaw = -dyaw;  // at least update yaw
+    if (line_count >= 2) {
-        correction_.x *= 0.9; correction_.y *= 0.9;
+        double dx = sum_dx / line_count, dy = sum_dy / line_count;
-        return;
-    }
-
-    double dx = sum_dx / line_count;
-    double dy = sum_dy / line_count;
         double corr_dist = std::sqrt(dx*dx + dy*dy);
-
+        if (corr_dist < 0.3 && std::abs(dyaw) < 0.2) {
-    // ── 5. Safety checks before updating offset ──
+            correction_.x = dx; correction_.y = dy; correction_.yaw = -dyaw;
-    bool trust_translation = (corr_dist < 0.2 && line_count >= 2);
+            fprintf(stderr, "[Localizer] fence: d(%.3f,%.3f,%.2f°) lines=%d\n",
-    bool trust_yaw = (std::abs(dyaw) < 0.15 && yaw_count >= 2);
+                    dx, dy, -dyaw*57.3, line_count);
-
+        }
-    // Don't let correction push pose outside arena
+    } else if (line_count == 1) {
-    double new_x = odom_pose_.x + dx;
+        bool is_horiz = (std::abs(dyaw) < std::abs(dyaw - M_PI/2));
-    double new_y = odom_pose_.y + dy;
+        if (is_horiz) {
-    if (std::abs(new_x) > 2.3 || std::abs(new_y) > 2.3)
+            correction_.y = sum_dy; correction_.x *= 0.9;
-        trust_translation = false;
+        } else {
-
+            correction_.x = sum_dx; correction_.y *= 0.9;
-    if (trust_translation && trust_yaw) {
+        }
-        correction_.x = dx;
-        correction_.y = dy;
         correction_.yaw = -dyaw;
-        fprintf(stderr, "[Localizer] fence: d(%.3f,%.3f,%.2f°) lines=%d/%d\n",
+        fprintf(stderr, "[Localizer] 1-line: d(%.3f,%.3f,%.2f°)\n",
-                dx, dy, -dyaw*57.3, line_count, yaw_count);
+                sum_dx, sum_dy, -dyaw*57.3);
-    } else if (trust_yaw) {
-        correction_.yaw = -dyaw;
-        correction_.x *= 0.9; correction_.y *= 0.9;
     } else {
         correction_.x *= 0.9; correction_.y *= 0.9;
     }

src/localizer.hpp

@@ -10,19 +10,13 @@ class Localizer {
 public:
     Localizer();
 
-    // ── Odometry update (called at odom rate) ──────────
     void updateOdom(double vx, double wz, double dt);
     void updateIMU(double gyro_z, double dt);
-
-    // Set raw odom pose. pose() returns odom + correction offset.
     void setOdomPose(const Pose2D& p);
 
-    // ── Scan-to-map correction (updates persistent offset) ──
+    void correctWithScanCV(const LaserScan& scan, const GridMap& map,
-    // Correction = matched_pose - odom_pose, stored as offset.
+                            const Pose2D* base_override = nullptr);
-    // Only updates if confidence is high; otherwise offset decays to zero.
-    void correctWithScanCV(const LaserScan& scan, const GridMap& map);
 
-    // ── Final pose = odom + correction offset ──────────
     Pose2D pose() const;
 
 private:

src/main.cpp

@@ -6,6 +6,7 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
+#include <std_msgs/msg/string.hpp>
 #include <tf2_ros/transform_broadcaster.h>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 
@@ -69,6 +70,11 @@ public:
         pub_cmd_  = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
         pub_path_ = create_publisher<nav_msgs::msg::Path>("/global_path", 10);
         pub_map_  = create_publisher<nav_msgs::msg::OccupancyGrid>("/map", rclcpp::QoS(1).transient_local());
+        pub_status_ = create_publisher<std_msgs::msg::String>("/nav_status", 10);
+        bt_.setFailCallback([this](){
+            std_msgs::msg::String msg; msg.data = "fail";
+            pub_status_->publish(msg);
+        });
         tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
 
         if (!mf_.empty()) {
@@ -79,6 +85,7 @@ public:
         timer_ = create_wall_timer(50ms, std::bind(&NavLiteNode::tick, this));
         map_timer_ = create_wall_timer(1s, std::bind(&NavLiteNode::publish_map, this));
         correct_timer_ = create_wall_timer(200ms, std::bind(&NavLiteNode::correct_localization, this));
+        morph_timer_ = create_wall_timer(100ms, std::bind(&NavLiteNode::morph_close, this));
 
         RCLCPP_INFO(get_logger(),"car_nav_lite ready. test_mode:=true for auto path. "
                     "ros2 param set /nav_lite_node <name> <value>");
@@ -105,6 +112,7 @@ private:
     // ── State ──
     std::mutex mtx_;
     LaserScan latest_scan_; bool has_scan_=false;
+    Pose2D scan_pose_snapshot_;
     double latest_vx_=0, latest_wz_=0, latest_gyro_z_=0;
     rclcpp::Time last_odom_t_{0,0,RCL_ROS_TIME};
     rclcpp::Time last_imu_t_{0,0,RCL_ROS_TIME};
@@ -116,14 +124,16 @@ private:
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_cmd_;
     rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_path_;
     rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_map_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_status_;
     std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
-    rclcpp::TimerBase::SharedPtr timer_, map_timer_, correct_timer_;
+    rclcpp::TimerBase::SharedPtr timer_, map_timer_, correct_timer_, morph_timer_;
 
     void scan_cb(LaserScanMsg::SharedPtr m){std::lock_guard lk(mtx_);
         latest_scan_.angle_min=m->angle_min;latest_scan_.angle_max=m->angle_max;
         latest_scan_.angle_increment=m->angle_increment;
         latest_scan_.range_min=m->range_min;latest_scan_.range_max=m->range_max;
-        latest_scan_.ranges=m->ranges;has_scan_=true;}
+        latest_scan_.ranges=m->ranges;has_scan_=true;
+        scan_pose_snapshot_ = localizer_.pose();}
     void odom_cb(OdometryMsg::SharedPtr m){std::lock_guard lk(mtx_);
         // EXACT-MPPI pattern: trust odom pose directly (sim = ground truth,
         // real robot = wheel-encoder / EKF output). No manual integration.
@@ -229,11 +239,13 @@ private:
     void correct_localization(){
         if(!has_scan_)return;
         auto pose_before = localizer_.pose();
-        localizer_.correctWithScanCV(latest_scan_, map_);
+        localizer_.correctWithScanCV(latest_scan_, map_, &scan_pose_snapshot_);
         auto pose_after = localizer_.pose();
         (void)pose_before; (void)pose_after;
     }
 
+    void morph_close(){std::lock_guard lk(mtx_); map_.morphologyClose(3);}
+
     void publish_tf(const Pose2D& pose){
         geometry_msgs::msg::TransformStamped tf;
         tf.header.stamp=now();tf.header.frame_id="map";tf.child_frame_id="base_link";

src/waypoint_nav.py

@@ -0,0 +1,145 @@
+#!/usr/bin/env python3
+"""Sequential waypoint navigation for car_nav_lite.
+
+Publishes /goal_pose waypoints one by one. Waits for the car to reach
+each waypoint before publishing the next.
+
+Usage:
+  python3 waypoint_nav.py                           # default waypoints
+  python3 waypoint_nav.py --waypoints waypoints.json  # from JSON file
+  python3 waypoint_nav.py --loop                      # loop back to start
+
+JSON format:
+  [{"x": 1.0, "y": -1.5, "yaw": 0.0}, ...]
+"""
+
+import argparse
+import json
+import math
+import time
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Odometry
+
+
+# Default waypoints for 5m×5m arena (car spawns near -2.0, -2.3)
+DEFAULT_WAYPOINTS = [
+    {"x": -0.5, "y": -1.5, "yaw": 0.0, "desc": "mid-left"},
+    {"x":  1.0, "y": -1.5, "yaw": 0.0, "desc": "right"},
+    {"x":  1.5, "y":  0.0, "yaw": 0.0, "desc": "top-right"},
+    {"x":  0.0, "y":  1.5, "yaw": 0.0, "desc": "top-center"},
+    {"x": -1.5, "y":  1.0, "yaw": 0.0, "desc": "top-left"},
+    {"x": -1.5, "y": -0.5, "yaw": 0.0, "desc": "left"},
+]
+
+
+class WaypointNav(Node):
+    def __init__(self, waypoints, loop=False, threshold=0.25):
+        super().__init__("waypoint_nav")
+        self.waypoints = waypoints
+        self.loop = loop
+        self.threshold = threshold
+        self.idx = 0
+        self.reached = False
+        self.last_dist = 1e9
+        self.stuck_count = 0
+        self.pose_x = self.pose_y = self.pose_yaw = 0.0
+
+        from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
+        qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE,
+                         durability=DurabilityPolicy.TRANSIENT_LOCAL)
+        self.pub_goal = self.create_publisher(PoseStamped, "/goal_pose", qos)
+        self.sub_odom = self.create_subscription(
+            Odometry, "/odom", self.odom_cb, 10)
+        self.timer = self.create_timer(0.2, self.check)  # 5Hz
+
+    def odom_cb(self, msg):
+        self.pose_x = msg.pose.pose.position.x
+        self.pose_y = msg.pose.pose.position.y
+        q = msg.pose.pose.orientation
+        self.pose_yaw = math.atan2(2*(q.w*q.z + q.x*q.y), 1 - 2*(q.y*q.y + q.z*q.z))
+
+    def send_goal(self, wp):
+        msg = PoseStamped()
+        msg.header.frame_id = "map"
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.pose.position.x = float(wp["x"])
+        msg.pose.position.y = float(wp["y"])
+        yaw_deg = float(wp.get("yaw", 0.0))
+        yaw = math.radians(yaw_deg)  # JSON stores degrees
+        msg.pose.orientation.z = math.sin(yaw / 2.0)
+        msg.pose.orientation.w = math.cos(yaw / 2.0)
+        self.pub_goal.publish(msg)
+        desc = wp.get("desc", "")
+        self.get_logger().info(
+            f"→ WP {self.idx+1}/{len(self.waypoints)}: "
+            f"({wp['x']:.2f},{wp['y']:.2f}) {desc}"
+        )
+
+    def check(self):
+        if self.idx >= len(self.waypoints):
+            if self.loop:
+                self.idx = 0
+                self.reached = False
+            else:
+                return
+
+        wp = self.waypoints[self.idx]
+        if not self.reached:
+            self.send_goal(wp)
+            self.reached = True
+            self.last_dist = 1e9
+            self.stuck_count = 0
+
+        dist = math.hypot(self.pose_x - wp["x"], self.pose_y - wp["y"])
+        if dist < self.last_dist - 0.05:
+            self.stuck_count = 0
+        else:
+            self.stuck_count += 1
+        self.last_dist = min(self.last_dist, dist)
+
+        if self.stuck_count > 15:
+            self.get_logger().warn(f"⏭ WP {self.idx+1} stuck — skip")
+            self.idx += 1
+            self.reached = False
+            return
+
+        if dist < self.threshold:
+            self.get_logger().info(
+                f"✓ WP {self.idx+1} reached (dist={dist:.2f}m)")
+            self.idx += 1
+            self.reached = False
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--waypoints", type=str, default=None,
+                        help="JSON file with waypoints")
+    parser.add_argument("--loop", action="store_true",
+                        help="Loop back to first waypoint after last")
+    parser.add_argument("--threshold", type=float, default=0.3,
+                        help="Arrival distance threshold (m)")
+    args = parser.parse_args()
+
+    if args.waypoints:
+        with open(args.waypoints) as f:
+            waypoints = json.load(f)
+        print(f"Loaded {len(waypoints)} waypoints from {args.waypoints}")
+    else:
+        waypoints = DEFAULT_WAYPOINTS
+        print(f"Using {len(waypoints)} default waypoints")
+
+    rclpy.init()
+    node = WaypointNav(waypoints, args.loop, args.threshold)
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()