MPPI planner + fence-line localization + scan obstacles

- Lightweight MPPI (200 trajectories, 15 steps, endpoint path_align)
- adaptive exploration (more random when stuck)
- fence-line Hough detection + geometric correction
- scan hit points marked at 255 immediately
- forward half-step pre-check for obstacle avoidance
- Ackermann constraint |wz| <= |vx|/min_turn_r
- XORshift RNG (stdlib broken on ARM)

include/car_nav_lite/bt_executor.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <optional>
+#include "car_nav_lite/types.hpp"
+#include "car_nav_lite/grid_map.hpp"
+#include "car_nav_lite/localizer.hpp"
+#include "car_nav_lite/global_planner.hpp"
+#include "car_nav_lite/local_planner.hpp"
+
+namespace car_nav_lite {
+
+class BTExecutor {
+public:
+    BTExecutor();
+
+    void setGoal(const Pose2D& goal);
+    void setTestPath(const std::vector<Pose2D>& path);
+    void setParams(double la, double ms, double mr, double sl, double cf);
+    bool hasGoal() const { return goal_.has_value(); }
+
+    Twist tick(const Pose2D& pose, const GridMap& map);
+
+    NavState state() const { return state_; }
+    const std::vector<Pose2D>& currentPath() const { return current_path_; }
+
+private:
+    NavState state_ = NavState::IDLE;
+    std::optional<Pose2D> goal_;
+    std::vector<Pose2D> current_path_;
+    GlobalPlanner planner_;
+    LocalPlanner  local_planner_;
+
+    bool atGoal(const Pose2D& pose) const;
+};
+
+}  // namespace car_nav_lite

include/car_nav_lite/global_planner.hpp

@@ -0,0 +1,37 @@
+#pragma once
+
+#include "car_nav_lite/types.hpp"
+#include "car_nav_lite/grid_map.hpp"
+#include <queue>
+#include <vector>
+#include <unordered_set>
+
+namespace car_nav_lite {
+
+class GlobalPlanner {
+public:
+    GlobalPlanner();
+
+    // ── A* search, returns world-coordinate path ───────
+    // inflation_override < 0 → use default INFLATION_CELLS
+    std::vector<Pose2D> plan(const Pose2D& start, const Pose2D& goal,
+                             const GridMap& map, int inflation_override = -1);
+
+    // ── Smooth path (funnel + approximate) ─────────────
+    std::vector<Pose2D> smooth(const std::vector<Pose2D>& raw_path,
+                               const GridMap& map);
+
+private:
+    int manhattan(int x1, int y1, int x2, int y2) const {
+        return 10 * (std::abs(x1-x2) + std::abs(y1-y2));
+    }
+    int euclidean(int x1, int y1, int x2, int y2) const {
+        double dx = x1 - x2, dy = y1 - y2;
+        return (int)(10.0 * std::sqrt(dx*dx + dy*dy));
+    }
+    int moveCost(int x1, int y1, int x2, int y2) const {
+        return (x1 != x2 && y1 != y2) ? 14 : 10;  // diagonal costs more
+    }
+};
+
+}  // namespace car_nav_lite

include/car_nav_lite/grid_map.cpp

@@ -0,0 +1,158 @@
+#include "car_nav_lite/grid_map.hpp"
+#include <fstream>
+#include <sstream>
+#include <cmath>
+#include <algorithm>
+
+namespace car_nav_lite {
+
+GridMap::GridMap() : cells_(GRID_SIZE * GRID_SIZE, 0) {}
+
+bool GridMap::loadPGM(const std::string& path) {
+    std::ifstream file(path, std::ios::binary);
+    if (!file) return false;
+
+    std::string magic;
+    file >> magic;  // P5
+    if (magic != "P5") return false;
+
+    int w, h, maxval;
+    // Skip comments
+    char c;
+    while (file.peek() == '\n' || file.peek() == '#') {
+        if (file.peek() == '#') {
+            std::string line; std::getline(file, line);
+        } else {
+            file.get();
+        }
+    }
+    file >> w >> h >> maxval;
+
+    if (w != GRID_SIZE || h != GRID_SIZE) {
+        fprintf(stderr, "[GridMap] PGM size %dx%d != %dx%d, will crop/clamp\n",
+                w, h, GRID_SIZE, GRID_SIZE);
+    }
+    file.get(); // consume whitespace before binary
+
+    std::vector<uint8_t> raw(w * h);
+    file.read(reinterpret_cast<char*>(raw.data()), w * h);
+
+    // Copy into our grid, inverting: PGM 0=black(obstacle), 255=white(free)
+    // We want 255=obstacle, 0=free
+    for (int gy = 0; gy < std::min(h, GRID_SIZE); ++gy) {
+        int src_row = h - 1 - gy;  // PGM origin is top-left, we want bottom-left
+        for (int gx = 0; gx < std::min(w, GRID_SIZE); ++gx) {
+            uint8_t pgm = raw[src_row * w + gx];
+            // Invert: black (0) -> obstacle (255), white (255) -> free (0)
+            // Threshold at 128: <128 = occupied
+            cells_[gy * GRID_SIZE + gx] = (pgm < 128) ? 255 : 0;
+        }
+    }
+    return true;
+}
+
+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);
+    return inBounds(gx, gy);
+}
+
+void GridMap::gridToWorld(int gx, int gy, double& wx, double& wy) const {
+    wx = GRID_ORIGIN_X + gx * GRID_RES + GRID_RES / 2.0;
+    wy = GRID_ORIGIN_Y + gy * GRID_RES + GRID_RES / 2.0;
+}
+
+uint8_t GridMap::at(int gx, int gy) const {
+    if (!inBounds(gx, gy)) return 255; // out of bounds = obstacle
+    return cells_[gy * GRID_SIZE + gx];
+}
+
+void GridMap::set(int gx, int gy, uint8_t v) {
+    if (!inBounds(gx, gy)) return;
+    cells_[gy * GRID_SIZE + gx] = v;
+}
+
+bool GridMap::inBounds(int gx, int gy) const {
+    return gx >= 0 && gx < GRID_SIZE && gy >= 0 && gy < GRID_SIZE;
+}
+
+bool GridMap::isFree(int gx, int gy) const {
+    if (!inBounds(gx, gy)) return false;
+    for (int dy = -INFLATION_CELLS; dy <= INFLATION_CELLS; ++dy) {
+        for (int dx = -INFLATION_CELLS; dx <= INFLATION_CELLS; ++dx) {
+            int nx = gx + dx, ny = gy + dy;
+            if (!inBounds(nx, ny)) continue;  // skip OOB; don't treat as obstacle
+            if (at(nx, ny) > 128) return false;
+        }
+    }
+    return true;
+}
+
+void GridMap::updateScan(const LaserScan& scan, const Pose2D& pose) {
+    int ox, oy;
+    if (!worldToGrid(pose.x, pose.y, ox, oy)) return;
+
+    for (int i = 0; i < scan.size(); ++i) {
+        double r = scan.ranges[i];
+        if (r < scan.range_min || r > scan.range_max) continue;
+        double angle = scan.angle_min + i * scan.angle_increment + pose.yaw;
+
+        int hx, hy;
+        double hwx = pose.x + r * std::cos(angle);
+        double hwy = pose.y + r * std::sin(angle);
+        if (worldToGrid(hwx, hwy, hx, hy)) {
+            uint8_t cur = at(hx, hy);
+            set(hx, hy, std::min(255, (int)cur + 20));  // hit → +occupied
+        }
+
+        // Raytrace clear
+        double clear_r = std::max(0.1, r - 0.1);
+        int cx, cy;
+        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
+        }
+    }
+}
+
+void GridMap::bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val) {
+    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;
+    int x = gx0, y = gy0;
+    while (true) {
+        if (inBounds(x, y)) {
+            uint8_t& v = cells_[y * GRID_SIZE + x];
+            if (v >= clear_val) v -= clear_val;
+        }
+        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;
+    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) {
+                set(cx+dx, cy+dy, value);
+            }
+        }
+    }
+}
+
+void GridMap::markRect(double x1, double y1, double x2, double y2, uint8_t value) {
+    int gx1, gy1, gx2, gy2;
+    worldToGrid(x1, y1, gx1, gy1);
+    worldToGrid(x2, y2, gx2, gy2);
+    for (int gy = std::min(gy1, gy2); gy <= std::max(gy1, gy2); ++gy)
+        for (int gx = std::min(gx1, gx2); gx <= std::max(gx1, gx2); ++gx)
+            set(gx, gy, value);
+}
+
+}  // namespace car_nav_lite

include/car_nav_lite/grid_map.hpp

@@ -0,0 +1,58 @@
+#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);
+
+    // ── 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

include/car_nav_lite/local_planner.hpp

@@ -0,0 +1,60 @@
+#pragma once
+
+#include "car_nav_lite/types.hpp"
+#include "car_nav_lite/grid_map.hpp"
+
+namespace car_nav_lite {
+
+class LocalPlanner {
+public:
+    LocalPlanner();
+
+    void setParams(double la, double ms, double mr, double sl, double cf);
+
+    // ── Lightweight MPPI (EXACT-MPPI-style, Ackermann) ──
+    // Samples K trajectories with Gaussian noise around previous optimal,
+    // scores via path-align + obstacle + goal critics, softmax-weights.
+    Twist compute(const Pose2D& pose,
+                  const std::vector<Pose2D>& path,
+                  const GridMap& map);
+
+private:
+    // ── Params ──
+    double ms_ = 1.0, mr_ = 0.25, sl_ = 0.6, cf_ = 6.0;
+
+    // ── MPPI settings ──
+    static constexpr int K = 200;        // trajectory count
+    static constexpr int T = 15;         // timesteps
+    static constexpr double DT = 0.1;    // step duration (1.5s horizon)
+    static constexpr double TEMPERATURE = 0.5;
+    static constexpr double VX_STD = 0.2;
+    static constexpr double WZ_STD = 0.8;
+
+    // ── Cost weights (EXACT-MPPI critic ratios) ──
+    static constexpr double W_ALIGN = 10.0;    // PathAlignCritic
+    static constexpr double W_FOLLOW = 5.0;    // PathFollowCritic
+    static constexpr double W_GOAL = 5.0;      // GoalCritic
+    static constexpr double W_OBS = 5.0;       // ObstaclesCritic (proportional)
+    static constexpr double W_FORWARD = 3.0;   // PreferForwardCritic (penalize reverse)
+    static constexpr double W_HEADING = 1.5;   // heading-to-goal (keep low — path_align dominates)
+    static constexpr double W_SMOOTH = 0.5;    // control smoothness
+
+    // ── Warm-start ──
+    double prev_vx_ = 0.5, prev_wz_ = 0;
+
+    // ── Simple RNG (XORshift + Box-Muller, no stdlib issues on ARM) ──
+    uint32_t rng_state_ = 123456789;
+    double randNormal();  // returns N(0,1)
+
+    // ── Rollout one trajectory, return total cost ──
+    double evaluateTrajectory(const Pose2D& pose, double vx, double wz,
+                              const std::vector<Pose2D>& path,
+                              const std::vector<double>& path_dists,
+                              double total_path_len,
+                              const GridMap& map);
+
+    static void computePathDistances(const std::vector<Pose2D>& path,
+                                     std::vector<double>& dists);
+};
+
+}  // namespace car_nav_lite

include/car_nav_lite/localizer.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "car_nav_lite/types.hpp"
+
+namespace car_nav_lite {
+
+class GridMap;
+
+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) ──
+    // Correction = matched_pose - odom_pose, stored as offset.
+    // 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:
+    Pose2D odom_pose_;
+    Pose2D correction_;
+    double yaw_imu_ = 0.0, yaw_odom_ = 0.0;
+};
+
+}  // namespace car_nav_lite

include/car_nav_lite/main.cpp

@@ -0,0 +1,179 @@
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
+#include <memory>
+#include <mutex>
+#include <cmath>
+
+#include "car_nav_lite/types.hpp"
+#include "car_nav_lite/grid_map.hpp"
+#include "car_nav_lite/localizer.hpp"
+#include "car_nav_lite/bt_executor.hpp"
+
+using namespace std::chrono_literals;
+using namespace car_nav_lite;
+using LaserScanMsg = sensor_msgs::msg::LaserScan;
+using OdometryMsg  = nav_msgs::msg::Odometry;
+using ImuMsg       = sensor_msgs::msg::Imu;
+using PoseStMsg    = geometry_msgs::msg::PoseStamped;
+
+class NavLiteNode : public rclcpp::Node {
+public:
+    NavLiteNode() : Node("nav_lite_node") {
+        declare_parameter("map_file", std::string(""));
+        declare_parameter("lookahead", 0.5);
+        map_file_  = get_parameter("map_file").as_string();
+        lookahead_ = get_parameter("lookahead").as_double();
+
+        sub_scan_ = create_subscription<LaserScanMsg>("/scan", 10,
+            std::bind(&NavLiteNode::scan_cb, this, std::placeholders::_1));
+        sub_odom_ = create_subscription<OdometryMsg>("/odom", 10,
+            std::bind(&NavLiteNode::odom_cb, this, std::placeholders::_1));
+        sub_imu_  = create_subscription<ImuMsg>("/imu/data_raw", 10,
+            std::bind(&NavLiteNode::imu_cb, this, std::placeholders::_1));
+        sub_goal_ = create_subscription<PoseStMsg>("/goal_pose", 10,
+            std::bind(&NavLiteNode::goal_cb, this, std::placeholders::_1));
+
+        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());
+
+        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
+
+        if (!map_file_.empty()) {
+            RCLCPP_INFO(get_logger(), "Loading map: %s", map_file_.c_str());
+            if (!map_.loadPGM(map_file_))
+                RCLCPP_WARN(get_logger(), "Failed to load map");
+            else
+                RCLCPP_INFO(get_logger(), "Map OK (%dx%d)", map_.width(), map_.height());
+        }
+
+        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(1s,   std::bind(&NavLiteNode::correct_localization, this));
+
+        RCLCPP_INFO(get_logger(), "car_nav_lite ready. /goal_pose -> /cmd_vel");
+    }
+
+private:
+    std::mutex mtx_;
+    LaserScan latest_scan_;
+    double latest_vx_ = 0, latest_wz_ = 0, latest_gyro_z_ = 0;
+    bool has_scan_ = false;
+    rclcpp::Time last_odom_t_{0, 0, RCL_ROS_TIME};
+    rclcpp::Time last_imu_t_ {0, 0, RCL_ROS_TIME};
+
+    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;
+    }
+
+    void odom_cb(OdometryMsg::SharedPtr m) {
+        std::lock_guard lk(mtx_);
+        latest_vx_ = m->twist.twist.linear.x;
+        latest_wz_ = m->twist.twist.angular.z;
+        // Directly use absolute pose from odom (no twist integration drift)
+        double qx=m->pose.pose.orientation.x, qy=m->pose.pose.orientation.y;
+        double qz=m->pose.pose.orientation.z, qw=m->pose.pose.orientation.w;
+        double yaw = std::atan2(2.0*(qw*qz+qx*qy), 1.0-2.0*(qy*qy+qz*qz));
+        localizer_.setPose(Pose2D(m->pose.pose.position.x,
+                                   m->pose.pose.position.y, yaw));
+    }
+
+    void imu_cb(ImuMsg::SharedPtr m) {
+        std::lock_guard lk(mtx_);
+        auto now = rclcpp::Time(m->header.stamp);
+        double dt = (last_imu_t_.nanoseconds() > 0) ? (now - last_imu_t_).seconds() : 0.005;
+        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_id='%s', expecting 'map'. Ignored.",
+                        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);
+        bt_.setGoal(g);
+        RCLCPP_INFO(get_logger(), "New goal: map(%.2f, %.2f)", g.x, g.y);
+    }
+
+    void tick() {
+        std::lock_guard lk(mtx_);
+        auto pose = localizer_.pose(); publish_tf(pose);
+        if (has_scan_) map_.updateScan(latest_scan_, pose);
+        Twist cmd = bt_.tick(pose, map_);
+        auto tw = geometry_msgs::msg::Twist(); tw.linear.x=cmd.vx; tw.angular.z=cmd.wz;
+        pub_cmd_->publish(tw);
+        if (!bt_.currentPath().empty()) {
+            auto pm=nav_msgs::msg::Path(); pm.header.stamp=now(); pm.header.frame_id="map";
+            for (auto& p: bt_.currentPath()) {
+                geometry_msgs::msg::PoseStamped ps; ps.pose.position.x=p.x; ps.pose.position.y=p.y;
+                pm.poses.push_back(ps);
+            } pub_path_->publish(pm);
+        }
+        static int c=0;
+        if (++c%60==0) RCLCPP_INFO(get_logger(),"pose=(%.2f,%.2f,%.1f) cmd=(%.2f,%.2f)",pose.x,pose.y,pose.yaw*57.3,cmd.vx,cmd.wz);
+    }
+
+    void publish_map() {
+        std::lock_guard lk(mtx_);
+        auto m=nav_msgs::msg::OccupancyGrid(); m.header.stamp=now(); m.header.frame_id="map";
+        m.info.resolution=GRID_RES; m.info.width=m.info.height=GRID_SIZE;
+        m.info.origin.position.x=GRID_ORIGIN_X; m.info.origin.position.y=GRID_ORIGIN_Y;
+        m.data.resize(GRID_SIZE*GRID_SIZE);
+        for (size_t i=0;i<map_.data().size();++i){uint8_t v=map_.data()[i];m.data[i]=(v==0)?0:((v>128)?100:-1);}
+        pub_map_->publish(m);
+    }
+
+    void correct_localization() {
+        // Disabled until PGM static map is loaded — scan-to-map needs populated obstacles
+        // std::lock_guard lk(mtx_);
+        // if (has_scan_) localizer_.correctWithScan(latest_scan_, map_);
+    }
+
+    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";
+        tf.transform.translation.x=pose.x; tf.transform.translation.y=pose.y;
+        double hy=pose.yaw/2.0; tf.transform.rotation.z=std::sin(hy); tf.transform.rotation.w=std::cos(hy);
+        tf_broadcaster_->sendTransform(tf);
+    }
+
+    GridMap map_; Localizer localizer_; BTExecutor bt_;
+    rclcpp::Subscription<LaserScanMsg>::SharedPtr sub_scan_;
+    rclcpp::Subscription<OdometryMsg>::SharedPtr  sub_odom_;
+    rclcpp::Subscription<ImuMsg>::SharedPtr       sub_imu_;
+    rclcpp::Subscription<PoseStMsg>::SharedPtr    sub_goal_;
+    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_;
+    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+    rclcpp::TimerBase::SharedPtr timer_, map_timer_, correct_timer_;
+    std::string map_file_;
+    double lookahead_ = 0.5;
+    bool initialized_ = false;
+};
+
+int main(int argc, char* argv[]) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<NavLiteNode>());
+    rclcpp::shutdown(); return 0;
+}

include/car_nav_lite/types.hpp

@@ -0,0 +1,83 @@
+#pragma once
+
+#include <cmath>
+#include <vector>
+#include <cstdint>
+
+namespace car_nav_lite {
+
+// ── 2D Pose ─────────────────────────────────────────────
+struct Pose2D {
+    double x = 0.0, y = 0.0, yaw = 0.0;
+    Pose2D() = default;
+    Pose2D(double x_, double y_, double yaw_) : x(x_), y(y_), yaw(yaw_) {}
+    double distTo(const Pose2D& o) const {
+        double dx = x - o.x, dy = y - o.y;
+        return std::sqrt(dx*dx + dy*dy);
+    }
+    double angleTo(const Pose2D& o) const {
+        return std::atan2(o.y - y, o.x - x);
+    }
+};
+
+// ── Twist command ───────────────────────────────────────
+struct Twist {
+    double vx = 0.0, wz = 0.0;
+};
+
+// ── LaserScan (deserialized from /scan HTTP JSON) ───────
+struct LaserScan {
+    double angle_min = 0.0, angle_max = 0.0, angle_increment = 0.0;
+    double range_min = 0.0, range_max = 0.0;
+    std::vector<float> ranges;
+    int size() const { return (int)ranges.size(); }
+};
+
+// ── Grid cell index ─────────────────────────────────────
+struct GridIndex {
+    int x = 0, y = 0;
+    GridIndex() = default;
+    GridIndex(int x_, int y_) : x(x_), y(y_) {}
+    bool operator==(const GridIndex& o) const { return x == o.x && y == o.y; }
+};
+
+// ── A* Node ─────────────────────────────────────────────
+struct AStarNode {
+    int x, y;
+    int g = 0, h = 0, f = 0;
+    AStarNode* parent = nullptr;
+};
+
+// ── State enum ──────────────────────────────────────────
+enum class NavState { IDLE, PLANNING, TRACKING, ARRIVED, FAILED };
+
+// ── Binary occupancy grid (0-255) ───────────────────────
+using OccupancyGrid = std::vector<uint8_t>;
+
+// ── Grid map parameters ─────────────────────────────────
+constexpr int   GRID_SIZE   = 1008;     // cells (5.04m @ 5mm)
+constexpr double GRID_RES   = 0.005;   // m/cell (5mm)
+constexpr double GRID_ORIGIN_X = -2.52;  // world x at grid[0] (buffer for 2.5 edge)
+constexpr double GRID_ORIGIN_Y = -2.52;  // world y at grid[0]
+constexpr int   INFLATION_CELLS = 30;  // 15cm expansion
+
+// ── Vehicle parameters ──────────────────────────────────
+constexpr double WHEELBASE    = 0.1682;  // m (front-rear axle)
+constexpr double MIN_TURN_R   = 0.25;    // m minimum turning radius
+constexpr double DEFAULT_LOOKAHEAD = 1.2; // m Pure Pursuit lookahead
+
+// ── Car footprint (origincar: 30cm×19cm) ──────────────
+constexpr double CAR_HALF_L = 0.15;   // half-length
+constexpr double CAR_HALF_W = 0.095;  // half-width
+constexpr double CAR_HALF_DIAG = 0.178; // sqrt(L²+W²)
+
+// ── Planner limits ──────────────────────────────────────
+constexpr double MAX_SPEED    = 4.0;    // m/s
+constexpr double MIN_SPEED    = 0.5;    // m/s (minimum for Ackermann turning)
+
+// ── Helper ──────────────────────────────────────────────
+inline double limit(double v, double lo, double hi) {
+    return v < lo ? lo : (v > hi ? hi : v);
+}
+
+}  // namespace car_nav_lite