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

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