354 lines
11 KiB
C++
354 lines
11 KiB
C++
#include "graph_map.h"
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
#include <queue>
|
||
|
||
// A*路径规划实现（简化版，不考虑其他AGV）
|
||
std::vector<Path*> GraphMap::findPath(Point* start, Point* end, const std::vector<AGV*>& avoid_agvs) {
|
||
if (start == end) {
|
||
return {};
|
||
}
|
||
|
||
// 注意：avoid_agvs参数保留以保持接口兼容性，但不再使用
|
||
// 路径规划专注于找到理论最优路径，避让由资源管理器处理
|
||
|
||
// A*算法
|
||
struct Node {
|
||
Point* point;
|
||
Path* path_from_parent; // 到达这个点的路径
|
||
double g_score; // 从起点到当前点的实际代价
|
||
double f_score; // g_score + 启发式代价
|
||
|
||
Node(Point* p, Path* path, double g, double f)
|
||
: point(p), path_from_parent(path), g_score(g), f_score(f) {}
|
||
|
||
bool operator<(const Node& other) const {
|
||
return f_score > other.f_score; // 最小堆
|
||
}
|
||
};
|
||
|
||
std::priority_queue<Node> open_set;
|
||
std::unordered_map<Point*, Path*> came_from;
|
||
std::unordered_map<Point*, double> g_score;
|
||
|
||
// 初始化起点
|
||
open_set.emplace(start, nullptr, 0, start->distanceTo(*end));
|
||
g_score[start] = 0;
|
||
|
||
while (!open_set.empty()) {
|
||
Node current = open_set.top();
|
||
open_set.pop();
|
||
|
||
// 到达终点
|
||
if (current.point == end) {
|
||
std::vector<Path*> path_list;
|
||
Point* curr = end;
|
||
|
||
// 回溯构建路径
|
||
while (curr != start && came_from.find(curr) != came_from.end()) {
|
||
Path* path = came_from[curr];
|
||
path_list.insert(path_list.begin(), path);
|
||
curr = path->start == curr ? path->end : path->start;
|
||
}
|
||
|
||
return path_list;
|
||
}
|
||
|
||
// 探索相邻路径
|
||
const std::vector<Path*>& adj_paths = getAdjacentPaths(current.point);
|
||
for (Path* path : adj_paths) {
|
||
// 简化版：不再检查路径占用，由资源管理器处理
|
||
// 获取相邻节点
|
||
Point* neighbor = path->getOtherEnd(current.point);
|
||
if (!neighbor) {
|
||
continue;
|
||
}
|
||
|
||
// 计算代价
|
||
double tentative_g = current.g_score + path->length;
|
||
|
||
// 如果这是更好的路径或者首次访问
|
||
if (g_score.find(neighbor) == g_score.end() || tentative_g < g_score[neighbor]) {
|
||
came_from[neighbor] = path;
|
||
g_score[neighbor] = tentative_g;
|
||
|
||
// 计算启发式代价（到终点的直线距离）
|
||
double f_score = tentative_g + neighbor->distanceTo(*end);
|
||
|
||
open_set.emplace(neighbor, path, tentative_g, f_score);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 无法找到路径
|
||
return {};
|
||
}
|
||
|
||
bool GraphMap::isPathAvailable(Path* path, const AGV* requester) {
|
||
std::lock_guard<std::mutex> lock(map_mutex_);
|
||
|
||
// 如果路径未被占用，则可用
|
||
if (!path->isOccupied()) {
|
||
return true;
|
||
}
|
||
|
||
// 如果被请求者自己占用，也认为可用（已在路上）
|
||
return path->occupied_by == requester;
|
||
}
|
||
|
||
void GraphMap::reservePath(Path* path, AGV* agv) {
|
||
std::lock_guard<std::mutex> lock(map_mutex_);
|
||
path->occupied_by = agv;
|
||
}
|
||
|
||
void GraphMap::releasePath(Path* path, AGV* agv) {
|
||
std::lock_guard<std::mutex> lock(map_mutex_);
|
||
if (path->occupied_by == agv) {
|
||
path->occupied_by = nullptr;
|
||
}
|
||
}
|
||
|
||
// K最短路径实现 (简化版Yen's算法变体)
|
||
std::vector<std::vector<Path*>> GraphMap::findKShortestPaths(
|
||
Point* start,
|
||
Point* end,
|
||
int K,
|
||
const std::vector<AGV*>& avoid_agvs
|
||
) {
|
||
std::vector<std::vector<Path*>> results;
|
||
|
||
if (K <= 0 || start == nullptr || end == nullptr || start == end) {
|
||
return results;
|
||
}
|
||
|
||
// 1. 找到最短路径 (使用标准A*)
|
||
std::vector<Path*> first_path = findPath(start, end, avoid_agvs);
|
||
if (first_path.empty()) {
|
||
return results; // 无法找到任何路径
|
||
}
|
||
results.push_back(first_path);
|
||
|
||
if (K == 1) {
|
||
return results;
|
||
}
|
||
|
||
// 2. 寻找K-1条备选路径 (通过边惩罚方法)
|
||
std::unordered_map<Path*, double> edge_penalties;
|
||
|
||
for (int k = 1; k < K; ++k) {
|
||
std::vector<Path*> best_alternative;
|
||
double best_cost = 1e9;
|
||
|
||
// 尝试惩罚前一条路径中的每条边，寻找替代方案
|
||
const std::vector<Path*>& prev_path = results[k-1];
|
||
|
||
// 随机选择一些偏离点来探索不同路径 (简化版)
|
||
// 完整Yen's算法会比较慢，这里使用贪心偏离策略
|
||
for (size_t deviate_idx = 0; deviate_idx < prev_path.size() && deviate_idx < 3; ++deviate_idx) {
|
||
Path* banned_path = prev_path[deviate_idx];
|
||
|
||
// 临时增加这条边的代价
|
||
double original_penalty = edge_penalties[banned_path];
|
||
edge_penalties[banned_path] += 1000.0; // 大惩罚值
|
||
|
||
// 使用修改后的代价重新运行A*
|
||
std::vector<Path*> alternative = _findPathWithPenalties(start, end, edge_penalties, avoid_agvs);
|
||
|
||
// 恢复代价
|
||
edge_penalties[banned_path] = original_penalty;
|
||
|
||
if (!alternative.empty()) {
|
||
// 计算路径总长度
|
||
double total_length = 0.0;
|
||
for (Path* p : alternative) {
|
||
total_length += p->length;
|
||
}
|
||
|
||
// 检查是否与已有路径重复
|
||
bool is_duplicate = false;
|
||
for (const auto& existing : results) {
|
||
if (_arePathsSame(alternative, existing)) {
|
||
is_duplicate = true;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (!is_duplicate && total_length < best_cost) {
|
||
best_cost = total_length;
|
||
best_alternative = alternative;
|
||
}
|
||
}
|
||
}
|
||
|
||
if (best_alternative.empty()) {
|
||
break; // 无法找到更多路径
|
||
}
|
||
|
||
results.push_back(best_alternative);
|
||
}
|
||
|
||
return results;
|
||
}
|
||
|
||
// 带边惩罚的A*算法 (内部辅助函数)
|
||
std::vector<Path*> GraphMap::_findPathWithPenalties(
|
||
Point* start,
|
||
Point* end,
|
||
const std::unordered_map<Path*, double>& penalties,
|
||
const std::vector<AGV*>& avoid_agvs
|
||
) {
|
||
if (start == end) {
|
||
return {};
|
||
}
|
||
|
||
struct Node {
|
||
Point* point;
|
||
Path* path_from_parent;
|
||
double g_score;
|
||
double f_score;
|
||
|
||
Node(Point* p, Path* path, double g, double f)
|
||
: point(p), path_from_parent(path), g_score(g), f_score(f) {}
|
||
|
||
bool operator<(const Node& other) const {
|
||
return f_score > other.f_score;
|
||
}
|
||
};
|
||
|
||
std::priority_queue<Node> open_set;
|
||
std::unordered_map<Point*, Path*> came_from;
|
||
std::unordered_map<Point*, double> g_score;
|
||
|
||
open_set.emplace(start, nullptr, 0, start->distanceTo(*end));
|
||
g_score[start] = 0;
|
||
|
||
while (!open_set.empty()) {
|
||
Node current = open_set.top();
|
||
open_set.pop();
|
||
|
||
if (current.point == end) {
|
||
std::vector<Path*> path_list;
|
||
Point* curr = end;
|
||
|
||
while (curr != start && came_from.find(curr) != came_from.end()) {
|
||
Path* path = came_from[curr];
|
||
path_list.insert(path_list.begin(), path);
|
||
curr = path->start == curr ? path->end : path->start;
|
||
}
|
||
|
||
return path_list;
|
||
}
|
||
|
||
const std::vector<Path*>& adj_paths = getAdjacentPaths(current.point);
|
||
for (Path* path : adj_paths) {
|
||
Point* neighbor = path->getOtherEnd(current.point);
|
||
if (!neighbor) {
|
||
continue;
|
||
}
|
||
|
||
// 计算带惩罚的代价
|
||
double penalty = 0.0;
|
||
auto it = penalties.find(path);
|
||
if (it != penalties.end()) {
|
||
penalty = it->second;
|
||
}
|
||
|
||
double tentative_g = current.g_score + path->length + penalty;
|
||
|
||
if (g_score.find(neighbor) == g_score.end() || tentative_g < g_score[neighbor]) {
|
||
came_from[neighbor] = path;
|
||
g_score[neighbor] = tentative_g;
|
||
|
||
double f_score = tentative_g + neighbor->distanceTo(*end);
|
||
open_set.emplace(neighbor, path, tentative_g, f_score);
|
||
}
|
||
}
|
||
}
|
||
|
||
return {};
|
||
}
|
||
|
||
// 检查两条路径是否相同 (内部辅助函数)
|
||
bool GraphMap::_arePathsSame(const std::vector<Path*>& path1, const std::vector<Path*>& path2) {
|
||
if (path1.size() != path2.size()) {
|
||
return false;
|
||
}
|
||
|
||
for (size_t i = 0; i < path1.size(); ++i) {
|
||
if (path1[i] != path2[i]) {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
// 计算路径特征
|
||
GraphMap::PathFeatures GraphMap::calculatePathFeatures(const std::vector<Path*>& path) {
|
||
PathFeatures features;
|
||
|
||
if (path.empty()) {
|
||
features.length = 0.0;
|
||
features.conflicts = 0;
|
||
features.avg_speed = 0.0;
|
||
features.smoothness = 0.0;
|
||
return features;
|
||
}
|
||
|
||
// 计算总长度和平均速度
|
||
double total_length = 0.0;
|
||
double total_speed = 0.0;
|
||
int conflicts = 0;
|
||
|
||
for (Path* p : path) {
|
||
total_length += p->length;
|
||
total_speed += p->max_speed;
|
||
if (p->isOccupied()) {
|
||
conflicts++;
|
||
}
|
||
}
|
||
|
||
features.length = total_length;
|
||
features.avg_speed = path.empty() ? 0.0 : total_speed / path.size();
|
||
features.conflicts = conflicts;
|
||
|
||
// 计算平滑度 (基于角度变化)
|
||
double curvature_changes = 0.0;
|
||
for (size_t i = 1; i < path.size() - 1; ++i) {
|
||
// 计算三个连续点形成的角度
|
||
Point* p1 = path[i-1]->start == path[i]->start ? path[i-1]->end : path[i-1]->start;
|
||
Point* p2 = path[i]->start;
|
||
Point* p3 = path[i+1]->end == path[i]->end ? path[i+1]->start : path[i+1]->end;
|
||
|
||
if (p1 && p2 && p3) {
|
||
// 计算向量
|
||
double v1x = p2->x - p1->x;
|
||
double v1y = p2->y - p1->y;
|
||
double v2x = p3->x - p2->x;
|
||
double v2y = p3->y - p2->y;
|
||
|
||
// 归一化
|
||
double norm1 = std::sqrt(v1x * v1x + v1y * v1y);
|
||
double norm2 = std::sqrt(v2x * v2x + v2y * v2y);
|
||
|
||
if (norm1 > 1e-6 && norm2 > 1e-6) {
|
||
v1x /= norm1;
|
||
v1y /= norm1;
|
||
v2x /= norm2;
|
||
v2y /= norm2;
|
||
|
||
// 点积
|
||
double dot = v1x * v2x + v1y * v2y;
|
||
dot = std::max(-1.0, std::min(1.0, dot)); // 裁剪到[-1, 1]
|
||
|
||
// 角度
|
||
double angle = std::acos(dot);
|
||
curvature_changes += angle;
|
||
}
|
||
}
|
||
}
|
||
|
||
features.smoothness = 1.0 / (1.0 + curvature_changes);
|
||
|
||
return features;
|
||
} |