Working on navigation system

src/navigation/PathFinder.cpp

@@ -3,6 +3,7 @@
 #include "utils/Utils.h"
 #include <algorithm>
 #include <cmath>
+#include <fstream>
 #include <iostream>
 #include <limits>
 #include <queue>
@@ -409,6 +410,58 @@ bool PathFinder::isWalkable(const Eigen::Vector3f& point) const
     return worldToCell(point, cell) && isCellWalkable(cell, walkable);
 }
 
+void PathFinder::addObstaclePolygon(const ObstaclePolygon& polygon)
+{
+    obstaclePolygons.push_back(polygon);
+    if (ready) {
+        rebuildWalkableGrid();
+    }
+}
+
+bool PathFinder::saveConfig(const std::string& path) const
+{
+    json root;
+    root["cellSize"]        = cellSize;
+    root["agentRadius"]     = agentRadius;
+    root["floorY"]          = floorY;
+    root["objectPadding"]   = objectPadding;
+    root["boundaryPadding"] = boundaryPadding;
+
+    json areasArr = json::array();
+    for (const auto& area : areas) {
+        json areaObj;
+        areaObj["name"]      = area.name;
+        areaObj["available"] = area.available;
+        json poly = json::array();
+        for (const auto& pt : area.polygon) {
+            poly.push_back(json::array({ pt.x(), pt.y() }));
+        }
+        areaObj["polygon"] = poly;
+        areasArr.push_back(areaObj);
+    }
+    root["areas"] = areasArr;
+
+    json obstArr = json::array();
+    for (const auto& obs : obstaclePolygons) {
+        json obsObj;
+        obsObj["name"] = obs.name;
+        json poly = json::array();
+        for (const auto& pt : obs.polygon) {
+            poly.push_back(json::array({ pt.x(), pt.y() }));
+        }
+        obsObj["polygon"] = poly;
+        obstArr.push_back(obsObj);
+    }
+    root["obstacles"] = obstArr;
+
+    std::ofstream f(path);
+    if (!f.is_open()) {
+        return false;
+    }
+    f << root.dump(2);
+    return f.good();
+}
+
 void PathFinder::loadConfig(const std::string& configPath, const std::string& zipPath)
 {
     cellSize = 0.4f;

src/navigation/PathFinder.h

@@ -50,8 +50,12 @@ public:
     bool isReady() const { return ready; }
     bool isWalkable(const Eigen::Vector3f& point) const;
     const std::vector<NavigationArea>& getAreas() const { return areas; }
+    const std::vector<ObstaclePolygon>& getObstaclePolygons() const { return obstaclePolygons; }
     float getFloorY() const { return floorY; }
 
+    void addObstaclePolygon(const ObstaclePolygon& polygon);
+    bool saveConfig(const std::string& path) const;
+
 private:
 
     float cellSize = 0.4f;