agv/RCS-3000
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

上传文件至 src/dispatch

Browse Source
This commit is contained in:
TY
2026-01-09 15:01:58 +08:00
parent 38f4fdd93d
commit de2450e775
5 changed files with 1534 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

354
src/dispatch/graph_map.cpp Normal file
View File

@@ -0,0 +1,354 @@
#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;
}