Working on navigation system

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;