66 lines
2.8 KiB
C++
66 lines
2.8 KiB
C++
#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);
|
|
|
|
// ── Reset dynamic obstacles to static PGM layer ──
|
|
void resetDynamic();
|
|
|
|
// ── 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_;
|
|
OccupancyGrid static_cells_; // snapshot after PGM load
|
|
void bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val);
|
|
};
|
|
|
|
} // namespace car_nav_lite
|