上传文件至 include/dispatch
This commit is contained in:
185
include/dispatch/graph_map.h
Normal file
185
include/dispatch/graph_map.h
Normal file
@@ -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
|
||||
254
include/dispatch/resource_manager.h
Normal file
254
include/dispatch/resource_manager.h
Normal file
@@ -0,0 +1,254 @@
|
||||
#ifndef RESOURCE_MANAGER_H
|
||||
#define RESOURCE_MANAGER_H
|
||||
|
||||
#include "graph_map.h"
|
||||
#include <unordered_map>
|
||||
#include <unordered_set>
|
||||
#include <queue>
|
||||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
|
||||
/**
|
||||
* @brief 资源请求状态
|
||||
*/
|
||||
enum class ResourceRequestStatus {
|
||||
GRANTED, // 已授予
|
||||
PENDING, // 等待中
|
||||
TIMEOUT, // 超时
|
||||
CANCELLED // 已取消
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 资源请求
|
||||
*/
|
||||
struct ResourceRequest {
|
||||
int id;
|
||||
AGV* agv;
|
||||
Path* path;
|
||||
uint64_t timestamp;
|
||||
ResourceRequestStatus status;
|
||||
std::condition_variable* condition; // 用于通知
|
||||
|
||||
ResourceRequest(int id_, AGV* agv_, Path* path_)
|
||||
: id(id_), agv(agv_), path(path_), timestamp(getCurrentTimestamp()),
|
||||
status(ResourceRequestStatus::PENDING), condition(nullptr) {}
|
||||
|
||||
bool operator<(const ResourceRequest& other) const {
|
||||
// 可以根据优先级或其他规则排序
|
||||
return timestamp < other.timestamp; // FIFO
|
||||
}
|
||||
|
||||
private:
|
||||
static uint64_t getCurrentTimestamp() {
|
||||
return std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::steady_clock::now().time_since_epoch()).count();
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 资源管理器
|
||||
* 负责管理路径和节点的占用
|
||||
*/
|
||||
class ResourceManager {
|
||||
private:
|
||||
// 资源占用状态
|
||||
std::unordered_map<Path*, AGV*> path_occupancy_;
|
||||
std::unordered_map<Point*, AGV*> point_occupancy_;
|
||||
|
||||
// 等待队列（路径和节点）
|
||||
std::unordered_map<Path*, std::queue<ResourceRequest*>> path_wait_queue_;
|
||||
std::unordered_map<Point*, std::queue<ResourceRequest*>> point_wait_queue_;
|
||||
|
||||
// 互斥锁
|
||||
mutable std::mutex resource_mutex_;
|
||||
|
||||
// 请求ID生成器
|
||||
std::atomic<int> next_request_id_{1};
|
||||
|
||||
// 统计信息
|
||||
std::atomic<uint64_t> total_requests_{0};
|
||||
std::atomic<uint64_t> granted_requests_{0};
|
||||
std::atomic<uint64_t> wait_count_{0};
|
||||
|
||||
public:
|
||||
ResourceManager() = default;
|
||||
~ResourceManager() = default;
|
||||
|
||||
/**
|
||||
* @brief 请求占用路径
|
||||
* @param agv 请求的AGV
|
||||
* @param path 要占用的路径
|
||||
* @param timeout_ms 超时时间（毫秒），0表示无限等待
|
||||
* @return 请求状态
|
||||
*/
|
||||
ResourceRequestStatus requestPath(AGV* agv, Path* path, int timeout_ms = 0);
|
||||
|
||||
/**
|
||||
* @brief 释放路径资源
|
||||
* @param agv 释放的AGV
|
||||
* @param path 释放的路径
|
||||
*/
|
||||
void releasePath(AGV* agv, Path* path);
|
||||
|
||||
/**
|
||||
* @brief 检查路径是否可用
|
||||
* @param path 要检查的路径
|
||||
* @param requester 请求的AGV（如果是自己占用也算可用）
|
||||
* @return 是否可用
|
||||
*/
|
||||
bool isPathAvailable(Path* path, AGV* requester = nullptr);
|
||||
|
||||
/**
|
||||
* @brief 请求占用节点
|
||||
* @param agv 请求的AGV
|
||||
* @param point 要占用的节点
|
||||
* @param timeout_ms 超时时间
|
||||
* @return 请求状态
|
||||
*/
|
||||
ResourceRequestStatus requestPoint(AGV* agv, Point* point, int timeout_ms = 0);
|
||||
|
||||
/**
|
||||
* @brief 释放节点资源
|
||||
* @param agv 释放的AGV
|
||||
* @param point 释放的节点
|
||||
*/
|
||||
void releasePoint(AGV* agv, Point* point);
|
||||
|
||||
/**
|
||||
* @brief 检查节点是否可用
|
||||
* @param point 要检查的节点
|
||||
* @param requester 请求的AGV
|
||||
* @return 是否可用
|
||||
*/
|
||||
bool isPointAvailable(Point* point, AGV* requester = nullptr);
|
||||
|
||||
/**
|
||||
* @brief 获取占用路径的AGV
|
||||
* @param path 路径
|
||||
* @return 占用的AGV，如果没有则返回nullptr
|
||||
*/
|
||||
AGV* getPathOccupant(Path* path) const;
|
||||
|
||||
/**
|
||||
* @brief 获取占用节点的AGV
|
||||
* @param point 节点
|
||||
* @return 占用的AGV，如果没有则返回nullptr
|
||||
*/
|
||||
AGV* getPointOccupant(Point* point) const;
|
||||
|
||||
/**
|
||||
* @brief 强制释放AGV占用的所有资源（用于错误处理）
|
||||
* @param agv 要释放资源的AGV
|
||||
*/
|
||||
void releaseAllResources(AGV* agv);
|
||||
|
||||
/**
|
||||
* @brief 获取统计信息
|
||||
*/
|
||||
void getStatistics(uint64_t& total_requests, uint64_t& granted_requests,
|
||||
uint64_t& wait_count) const;
|
||||
|
||||
/**
|
||||
* @brief 打印资源占用状态
|
||||
*/
|
||||
void printResourceStatus() const;
|
||||
|
||||
/**
|
||||
* @brief 检测死锁
|
||||
* @return 发现的死锁数量
|
||||
*/
|
||||
int detectDeadlocks();
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 处理等待队列
|
||||
* 当资源释放时，检查等待队列并分配给下一个请求
|
||||
*/
|
||||
void processWaitQueue(Path* path);
|
||||
void processWaitQueue(Point* point);
|
||||
|
||||
/**
|
||||
* @brief 取消请求
|
||||
*/
|
||||
void cancelRequest(ResourceRequest* request);
|
||||
|
||||
/**
|
||||
* @brief 检查循环等待（死锁检测）
|
||||
*/
|
||||
bool hasCircularWait(AGV* agv, std::unordered_set<AGV*>& visited,
|
||||
std::unordered_set<Path*>& path_set);
|
||||
|
||||
/**
|
||||
* @brief 记录统计信息
|
||||
*/
|
||||
void recordRequest(bool granted);
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 资源自动管理器
|
||||
* 使用RAII模式自动管理资源
|
||||
*/
|
||||
class ScopedPathResource {
|
||||
private:
|
||||
ResourceManager* manager_;
|
||||
AGV* agv_;
|
||||
Path* path_;
|
||||
bool acquired_;
|
||||
|
||||
public:
|
||||
ScopedPathResource(ResourceManager* manager, AGV* agv, Path* path)
|
||||
: manager_(manager), agv_(agv), path_(path), acquired_(false) {
|
||||
if (manager_ && agv_ && path_) {
|
||||
auto status = manager_->requestPath(agv_, path_);
|
||||
acquired_ = (status == ResourceRequestStatus::GRANTED);
|
||||
}
|
||||
}
|
||||
|
||||
~ScopedPathResource() {
|
||||
if (acquired_ && manager_ && agv_ && path_) {
|
||||
manager_->releasePath(agv_, path_);
|
||||
}
|
||||
}
|
||||
|
||||
bool isAcquired() const { return acquired_; }
|
||||
|
||||
// 禁止拷贝
|
||||
ScopedPathResource(const ScopedPathResource&) = delete;
|
||||
ScopedPathResource& operator=(const ScopedPathResource&) = delete;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 节点资源自动管理器
|
||||
*/
|
||||
class ScopedPointResource {
|
||||
private:
|
||||
ResourceManager* manager_;
|
||||
AGV* agv_;
|
||||
Point* point_;
|
||||
bool acquired_;
|
||||
|
||||
public:
|
||||
ScopedPointResource(ResourceManager* manager, AGV* agv, Point* point)
|
||||
: manager_(manager), agv_(agv), point_(point), acquired_(false) {
|
||||
if (manager_ && agv_ && point_) {
|
||||
auto status = manager_->requestPoint(agv_, point_);
|
||||
acquired_ = (status == ResourceRequestStatus::GRANTED);
|
||||
}
|
||||
}
|
||||
|
||||
~ScopedPointResource() {
|
||||
if (acquired_ && manager_ && agv_ && point_) {
|
||||
manager_->releasePoint(agv_, point_);
|
||||
}
|
||||
}
|
||||
|
||||
bool isAcquired() const { return acquired_; }
|
||||
|
||||
// 禁止拷贝
|
||||
ScopedPointResource(const ScopedPointResource&) = delete;
|
||||
ScopedPointResource& operator=(const ScopedPointResource&) = delete;
|
||||
};
|
||||
|
||||
#endif // RESOURCE_MANAGER_H
|
||||
Reference in New Issue
Block a user