上传文件至 include/dispatch

include/dispatch/graph_map.h

@@ -0,0 +1,185 @@
+#ifndef GRAPH_MAP_H
+#define GRAPH_MAP_H
+
+#include <string>
+#include <vector>
+#include <unordered_map>
+#include <unordered_set>
+#include <memory>
+#include <mutex>
+#include <cmath>
+
+// 前向声明
+class AGV;
+
+/**
+ * @brief 地图节点（Point）
+ */
+class Point {
+public:
+    int id;                     // 节点唯一ID
+    double x, y;               // 坐标位置（米）
+    std::string name;          // 节点名称
+
+    // 资源占用管理
+    AGV* occupied_by;          // 当前占用该节点的AGV
+
+    Point(int id_, double x_, double y_, const std::string& name_ = "")
+        : id(id_), x(x_), y(y_), name(name_), occupied_by(nullptr) {}
+
+    bool isOccupied() const { return occupied_by != nullptr; }
+
+    double distanceTo(const Point& other) const {
+        double dx = x - other.x;
+        double dy = y - other.y;
+        return sqrt(dx * dx + dy * dy);
+    }
+};
+
+/**
+ * @brief 路径边（Path）
+ */
+class Path {
+public:
+    int id;                     // 路径唯一ID
+    Point* start;              // 起始节点
+    Point* end;                // 终点节点
+    double length;             // 路径长度（米）
+    double max_speed;          // 最大允许速度（米/秒）
+    bool bidirectional;        // 是否双向通行
+
+    // 资源占用管理
+    AGV* occupied_by;          // 当前占用该路径的AGV
+
+    Path(int id_, Point* start_, Point* end_, bool bidirectional_ = true)
+        : id(id_), start(start_), end(end_), bidirectional(bidirectional_), occupied_by(nullptr) {
+        length = start->distanceTo(*end);
+        max_speed = 2.0;  // 默认最大速度2m/s
+    }
+
+    bool isOccupied() const { return occupied_by != nullptr; }
+
+    Point* getOtherEnd(const Point* p) {
+        if (p == start) return end;
+        if (p == end) return start;
+        return nullptr;
+    }
+};
+
+/**
+ * @brief 地图图结构
+ */
+class GraphMap {
+private:
+    std::unordered_map<int, std::unique_ptr<Point>> points_;
+    std::unordered_map<int, std::unique_ptr<Path>> paths_;
+    std::unordered_map<Point*, std::vector<Path*>> adj_paths_;
+    mutable std::mutex map_mutex_;
+
+public:
+    GraphMap() = default;
+
+    Point* addPoint(int id, double x, double y, const std::string& name = "") {
+        std::lock_guard<std::mutex> lock(map_mutex_);
+        auto point = std::make_unique<Point>(id, x, y, name);
+        Point* ptr = point.get();
+        points_[id] = std::move(point);
+        return ptr;
+    }
+
+    Path* addPath(int id, int start_id, int end_id, bool bidirectional = true) {
+        std::lock_guard<std::mutex> lock(map_mutex_);
+
+        auto start_it = points_.find(start_id);
+        auto end_it = points_.find(end_id);
+
+        if (start_it == points_.end() || end_it == points_.end()) {
+            return nullptr;
+        }
+
+        auto path = std::make_unique<Path>(id, start_it->second.get(), end_it->second.get(), bidirectional);
+        Path* ptr = path.get();
+
+        paths_[id] = std::move(path);
+
+        adj_paths_[start_it->second.get()].push_back(ptr);
+        adj_paths_[end_it->second.get()].push_back(ptr);
+
+        return ptr;
+    }
+
+    Point* getPoint(int id) {
+        auto it = points_.find(id);
+        return it != points_.end() ? it->second.get() : nullptr;
+    }
+
+    Point* getPointById(int id) {
+        return getPoint(id);  // 别名，保持接口一致性
+    }
+
+    Path* getPath(int id) {
+        auto it = paths_.find(id);
+        return it != paths_.end() ? it->second.get() : nullptr;
+    }
+
+    const std::vector<Path*>& getAdjacentPaths(Point* point) {
+        static std::vector<Path*> empty;
+        auto it = adj_paths_.find(point);
+        return it != adj_paths_.end() ? it->second : empty;
+    }
+
+    // 获取所有路径（用于清理AGV占用）
+    std::vector<Path*> getAllPaths() {
+        std::lock_guard<std::mutex> lock(map_mutex_);
+        std::vector<Path*> all_paths;
+        for (const auto& pair : paths_) {
+            all_paths.push_back(pair.second.get());
+        }
+        return all_paths;
+    }
+
+    // A*路径规划
+    std::vector<Path*> findPath(Point* start, Point* end, const std::vector<AGV*>& avoid_agvs);
+
+    // K最短路径规划 (Yen's算法或简化变体)
+    // 用于DQN强化学习: 生成K条候选路径供智能体选择
+    std::vector<std::vector<Path*>> findKShortestPaths(
+        Point* start,
+        Point* end,
+        int K,
+        const std::vector<AGV*>& avoid_agvs = {}
+    );
+
+    // 计算路径特征 (用于DQN状态编码)
+    struct PathFeatures {
+        double length;
+        int conflicts;
+        double avg_speed;
+        double smoothness;
+    };
+    PathFeatures calculatePathFeatures(const std::vector<Path*>& path);
+
+private:
+    // 内部辅助函数
+    std::vector<Path*> _findPathWithPenalties(
+        Point* start,
+        Point* end,
+        const std::unordered_map<Path*, double>& penalties,
+        const std::vector<AGV*>& avoid_agvs
+    );
+
+    bool _arePathsSame(const std::vector<Path*>& path1, const std::vector<Path*>& path2);
+
+public:
+    bool isPathAvailable(Path* path, const AGV* requester);
+    void reservePath(Path* path, AGV* agv);
+    void releasePath(Path* path, AGV* agv);
+
+    void getStatistics(size_t& num_points, size_t& num_paths) const {
+        std::lock_guard<std::mutex> lock(map_mutex_);
+        num_points = points_.size();
+        num_paths = paths_.size();
+    }
+};
+
+#endif // GRAPH_MAP_H