上传文件至 src/dispatch
This commit is contained in:
454
src/dispatch/enhanced_agv_manager.cpp
Normal file
454
src/dispatch/enhanced_agv_manager.cpp
Normal file
@@ -0,0 +1,454 @@
|
||||
#include "enhanced_agv_manager.h"
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
|
||||
void EnhancedAGVManager::initializeScoringFunctions() {
|
||||
scoring_functions_[AssignmentStrategy::FIRST_FIT] =
|
||||
[this](AGV* agv, Task* task, double distance) { return scoreFirstFit(agv, task, distance); };
|
||||
|
||||
scoring_functions_[AssignmentStrategy::NEAREST_FIT] =
|
||||
[this](AGV* agv, Task* task, double distance) { return scoreNearestFit(agv, task, distance); };
|
||||
|
||||
scoring_functions_[AssignmentStrategy::BATTERY_AWARE] =
|
||||
[this](AGV* agv, Task* task, double distance) { return scoreBatteryAware(agv, task, distance); };
|
||||
|
||||
scoring_functions_[AssignmentStrategy::LOAD_BALANCED] =
|
||||
[this](AGV* agv, Task* task, double distance) { return scoreLoadBalanced(agv, task, distance); };
|
||||
|
||||
scoring_functions_[AssignmentStrategy::PRIORITY_BASED] =
|
||||
[this](AGV* agv, Task* task, double distance) { return scorePriorityBased(agv, task, distance); };
|
||||
|
||||
scoring_functions_[AssignmentStrategy::COST_OPTIMAL] =
|
||||
[this](AGV* agv, Task* task, double distance) { return scoreCostOptimal(agv, task, distance); };
|
||||
}
|
||||
|
||||
int EnhancedAGVManager::assignTasksSmart() {
|
||||
int assigned_count = 0;
|
||||
|
||||
// 获取所有可用AGV
|
||||
std::vector<AGV*> available_agvs = getIdleAGVs();
|
||||
|
||||
// 获取待分配任务
|
||||
std::vector<Task*> pending_tasks = getPendingTasks();
|
||||
|
||||
if (available_agvs.empty() || pending_tasks.empty()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 按优先级排序任务
|
||||
std::sort(pending_tasks.begin(), pending_tasks.end(),
|
||||
[](const Task* a, const Task* b) { return a->priority > b->priority; });
|
||||
|
||||
// 根据策略进行分配
|
||||
if (strategy_ == AssignmentStrategy::COST_OPTIMAL) {
|
||||
// 批量优化分配
|
||||
auto assignments = optimizeBatchAssignment(pending_tasks, available_agvs);
|
||||
|
||||
// 执行分配
|
||||
for (const auto& option : assignments) {
|
||||
// 使用基类的任务分配逻辑
|
||||
if (assignSingleTask(option.agv, option.task)) {
|
||||
assigned_count++;
|
||||
std::cout << "[智能调度] 任务 " << option.task->id
|
||||
<< " 分配给 AGV-" << option.agv->name
|
||||
<< " (成本: " << option.total_cost << ")" << std::endl;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// 逐个分配
|
||||
for (auto* task : pending_tasks) {
|
||||
auto evaluations = findBestAGVForTask(task);
|
||||
|
||||
if (!evaluations.empty() && evaluations[0].score > 0) {
|
||||
AGV* best_agv = evaluations[0].agv;
|
||||
std::vector<Path*> path = findPathWithAvoidance(best_agv, task);
|
||||
|
||||
if (!path.empty() && assignSingleTask(best_agv, task)) {
|
||||
assigned_count++;
|
||||
std::cout << "[智能调度] 任务 " << task->id
|
||||
<< " 分配给 AGV-" << best_agv->name
|
||||
<< " (" << evaluations[0].reason << ")" << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return assigned_count;
|
||||
}
|
||||
|
||||
std::vector<AGVEvaluation> EnhancedAGVManager::findBestAGVForTask(Task* task, bool exclude_assigned) {
|
||||
std::vector<AGVEvaluation> evaluations;
|
||||
|
||||
for (const auto& pair : agvs_) {
|
||||
AGV* agv = pair.second; // 修复：不需要.get()
|
||||
|
||||
// 检查AGV是否可用
|
||||
if (agv->state != AGVState::IDLE) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (exclude_assigned && agv->current_task) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 计算距离
|
||||
double distance = calculateDistance(agv, task);
|
||||
|
||||
// 使用当前策略评分
|
||||
auto scoring_func = scoring_functions_[strategy_];
|
||||
double score = scoring_func(agv, task, distance);
|
||||
|
||||
// 估算完成时间
|
||||
double estimated_time = distance / agv->max_speed;
|
||||
|
||||
// 生成选择原因
|
||||
std::string reason = getReasonForSelection(agv, task, distance, score);
|
||||
|
||||
evaluations.emplace_back(agv, score, estimated_time, distance, reason);
|
||||
}
|
||||
|
||||
// 按分数排序（降序）
|
||||
std::sort(evaluations.begin(), evaluations.end(),
|
||||
[](const AGVEvaluation& a, const AGVEvaluation& b) {
|
||||
return a.score > b.score;
|
||||
});
|
||||
|
||||
return evaluations;
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::calculateAssignmentCost(AGV* agv, Task* task, std::vector<Path*>& path) {
|
||||
if (!agv || !task || path.empty()) {
|
||||
return std::numeric_limits<double>::infinity();
|
||||
}
|
||||
|
||||
// 计算基础成本
|
||||
double total_distance = 0;
|
||||
for (Path* p : path) {
|
||||
total_distance += p->length;
|
||||
}
|
||||
|
||||
// 时间成本
|
||||
double time_cost = total_distance / agv->max_speed;
|
||||
|
||||
// 电量成本
|
||||
double battery_cost = calculateBatteryConsumption(agv, task, total_distance);
|
||||
|
||||
// 综合成本（可根据权重调整）
|
||||
double total_cost = weights_.distance_weight * total_distance +
|
||||
weights_.speed_weight * time_cost +
|
||||
weights_.battery_weight * battery_cost;
|
||||
|
||||
return total_cost;
|
||||
}
|
||||
|
||||
std::vector<AssignmentOption> EnhancedAGVManager::optimizeBatchAssignment(
|
||||
std::vector<Task*>& tasks, std::vector<AGV*>& available_agvs) {
|
||||
|
||||
std::vector<AssignmentOption> all_options;
|
||||
|
||||
// 生成所有可能的AGV-任务组合
|
||||
for (Task* task : tasks) {
|
||||
for (AGV* agv : available_agvs) {
|
||||
if (agv->state != AGVState::IDLE) continue;
|
||||
|
||||
std::vector<Path*> path = findPathWithAvoidance(agv, task);
|
||||
if (!path.empty()) {
|
||||
double cost = calculateAssignmentCost(agv, task, path);
|
||||
double duration = cost / agv->max_speed;
|
||||
double battery = calculateBatteryConsumption(agv, task, cost);
|
||||
|
||||
all_options.emplace_back(task, agv, path, cost, duration, battery);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 按成本排序
|
||||
std::sort(all_options.begin(), all_options.end(),
|
||||
[](const AssignmentOption& a, const AssignmentOption& b) {
|
||||
return a.total_cost < b.total_cost;
|
||||
});
|
||||
|
||||
std::vector<AssignmentOption> selected_assignments;
|
||||
std::unordered_set<AGV*> used_agvs;
|
||||
std::unordered_set<Task*> assigned_tasks;
|
||||
|
||||
// 贪心选择最优分配
|
||||
for (const auto& option : all_options) {
|
||||
if (used_agvs.find(option.agv) == used_agvs.end() &&
|
||||
assigned_tasks.find(option.task) == assigned_tasks.end()) {
|
||||
|
||||
selected_assignments.push_back(option);
|
||||
used_agvs.insert(option.agv);
|
||||
assigned_tasks.insert(option.task);
|
||||
|
||||
// 修复：直接设置状态而不是调用不存在的方法
|
||||
option.agv->state = AGVState::ASSIGNED;
|
||||
option.task->status = TaskStatus::ASSIGNED;
|
||||
option.task->assigned_agv = option.agv;
|
||||
}
|
||||
}
|
||||
|
||||
return selected_assignments;
|
||||
}
|
||||
|
||||
std::pair<double, bool> EnhancedAGVManager::predictAvailability(AGV* agv, double future_time) {
|
||||
if (!agv) return {0.0, false};
|
||||
|
||||
// 简化的可用性预测
|
||||
if (agv->state == AGVState::IDLE) {
|
||||
return {0.0, true};
|
||||
} else if (agv->current_task) {
|
||||
// 估算当前任务完成时间
|
||||
double estimated_completion = 5.0; // 简化假设：5分钟完成
|
||||
return {estimated_completion, future_time >= estimated_completion};
|
||||
}
|
||||
|
||||
return {std::numeric_limits<double>::infinity(), false};
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::getAGVLoad(AGV* agv) {
|
||||
if (!agv) return 0.0;
|
||||
|
||||
// 简化的负载计算
|
||||
if (agv->state == AGVState::IDLE) {
|
||||
return 0.0;
|
||||
} else if (agv->current_task) {
|
||||
return 0.8; // 假设任务中负载为80%
|
||||
}
|
||||
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
void EnhancedAGVManager::printAssignmentAnalysis() {
|
||||
std::cout << "\n===== 任务分配分析 =====" << std::endl;
|
||||
std::cout << "当前策略: " << assignmentStrategyToString(strategy_) << std::endl;
|
||||
|
||||
std::cout << "\n分配权重:" << std::endl;
|
||||
std::cout << " 距离权重: " << weights_.distance_weight << std::endl;
|
||||
std::cout << " 电量权重: " << weights_.battery_weight << std::endl;
|
||||
std::cout << " 负载权重: " << weights_.load_weight << std::endl;
|
||||
std::cout << " 速度权重: " << weights_.speed_weight << std::endl;
|
||||
|
||||
std::cout << "\nAGV状态:" << std::endl;
|
||||
for (const auto& pair : agvs_) {
|
||||
AGV* agv = pair.second; // 修复：不需要.get()
|
||||
std::cout << " AGV-" << agv->id << " (" << agv->name << ")"
|
||||
<< ", 状态=" << agvStateToString(agv->state) // 修复：直接访问state
|
||||
<< ", 电量=" << agv->battery_level << "%"
|
||||
<< ", 速度=" << agv->max_speed << "m/s" << std::endl;
|
||||
|
||||
if (agv->current_task) {
|
||||
std::cout << " 当前任务: " << agv->current_task->id
|
||||
<< " (" << agv->current_task->description << ")" << std::endl;
|
||||
}
|
||||
}
|
||||
std::cout << "========================" << std::endl;
|
||||
}
|
||||
|
||||
// 评分函数实现
|
||||
double EnhancedAGVManager::scoreFirstFit(AGV* agv, Task* task, double distance) {
|
||||
return agv->state == AGVState::IDLE ? 1.0 : 0.0;
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::scoreNearestFit(AGV* agv, Task* task, double distance) {
|
||||
return distance > 0 ? 1000.0 / distance : 1000.0;
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::scoreBatteryAware(AGV* agv, Task* task, double distance) {
|
||||
double base_score = scoreNearestFit(agv, task, distance);
|
||||
return base_score * (agv->battery_level / 100.0);
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::scoreLoadBalanced(AGV* agv, Task* task, double distance) {
|
||||
double base_score = scoreNearestFit(agv, task, distance);
|
||||
double load_factor = 1.0 - getAGVLoad(agv);
|
||||
return base_score * load_factor;
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::scorePriorityBased(AGV* agv, Task* task, double distance) {
|
||||
double base_score = scoreNearestFit(agv, task, distance);
|
||||
return base_score * task->priority;
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::scoreCostOptimal(AGV* agv, Task* task, double distance) {
|
||||
// 考虑距离、速度、电量的综合成本
|
||||
double time_cost = distance / agv->max_speed;
|
||||
double battery_cost = calculateBatteryConsumption(agv, task, distance);
|
||||
double total_cost = distance + time_cost + battery_cost;
|
||||
|
||||
return total_cost > 0 ? 1000.0 / total_cost : 0.0;
|
||||
}
|
||||
|
||||
// 辅助函数实现
|
||||
double EnhancedAGVManager::calculateDistance(AGV* agv, Task* task) {
|
||||
if (!agv || !task || !task->start_point) {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// 使用AGV当前位置到任务起点的距离
|
||||
if (agv->current_position) {
|
||||
return agv->current_position->distanceTo(*task->start_point);
|
||||
} else {
|
||||
// 如果AGV没有当前位置，使用坐标距离
|
||||
double dx = agv->x - task->start_point->x;
|
||||
double dy = agv->y - task->start_point->y;
|
||||
return std::sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::calculateBatteryConsumption(AGV* agv, Task* task, double distance) {
|
||||
if (!agv || !task) return 0.0;
|
||||
|
||||
// 简化的电量消耗计算：距离 * 基础消耗率
|
||||
double base_consumption = distance * 0.1; // 每米消耗0.1%电量
|
||||
|
||||
// 根据AGV速度调整
|
||||
double speed_factor = agv->max_speed / 2.0; // 标准速度为2m/s
|
||||
|
||||
return base_consumption * speed_factor;
|
||||
}
|
||||
|
||||
std::vector<Path*> EnhancedAGVManager::findPathWithAvoidance(AGV* agv, Task* task) {
|
||||
if (!agv || !task || !task->start_point || !task->end_point) {
|
||||
return {};
|
||||
}
|
||||
|
||||
// 使用基类的findPath方法
|
||||
return findPath(agv, task);
|
||||
}
|
||||
|
||||
std::string EnhancedAGVManager::getReasonForSelection(AGV* agv, Task* task, double distance, double score) {
|
||||
std::ostringstream oss;
|
||||
|
||||
switch (strategy_) {
|
||||
case AssignmentStrategy::NEAREST_FIT:
|
||||
oss << "距离最近(" << std::fixed << std::setprecision(1) << distance << "m)";
|
||||
break;
|
||||
case AssignmentStrategy::BATTERY_AWARE:
|
||||
oss << "电量充足(" << agv->battery_level << "%)";
|
||||
break;
|
||||
case AssignmentStrategy::PRIORITY_BASED:
|
||||
oss << "优先级匹配(" << task->priority << ")";
|
||||
break;
|
||||
case AssignmentStrategy::LOAD_BALANCED:
|
||||
oss << "负载最低(" << std::fixed << std::setprecision(1) << (1.0-getAGVLoad(agv))*100 << "%可用)";
|
||||
break;
|
||||
case AssignmentStrategy::COST_OPTIMAL:
|
||||
oss << "成本最优(评分:" << std::fixed << std::setprecision(1) << score << ")";
|
||||
break;
|
||||
default:
|
||||
oss << "默认选择";
|
||||
break;
|
||||
}
|
||||
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
std::string EnhancedAGVManager::agvStateToString(AGVState state) {
|
||||
switch (state) {
|
||||
case AGVState::IDLE: return "空闲";
|
||||
case AGVState::ASSIGNED: return "已分配";
|
||||
case AGVState::MOVING: return "移动中";
|
||||
case AGVState::LOADING: return "装载中";
|
||||
case AGVState::UNLOADING: return "卸载中";
|
||||
case AGVState::CHARGING: return "充电中";
|
||||
case AGVState::ERROR: return "错误";
|
||||
case AGVState::MAINTENANCE: return "维护中";
|
||||
default: return "未知";
|
||||
}
|
||||
}
|
||||
|
||||
// 添加缺少的辅助方法
|
||||
bool EnhancedAGVManager::assignSingleTask(AGV* agv, Task* task) {
|
||||
if (!agv || !task || agv->state != AGVState::IDLE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 设置路径
|
||||
std::vector<Path*> path = findPathWithAvoidance(agv, task);
|
||||
if (!path.empty()) {
|
||||
// 如果有ResourceManager，尝试请求路径资源
|
||||
if (resource_manager_) {
|
||||
// 检查所有路径是否可用
|
||||
bool all_paths_available = true;
|
||||
for (Path* p : path) {
|
||||
if (!resource_manager_->isPathAvailable(p, agv)) {
|
||||
all_paths_available = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 如果所有路径都可用，请求资源
|
||||
if (all_paths_available) {
|
||||
for (Path* p : path) {
|
||||
auto status = resource_manager_->requestPath(agv, p, 0); // 0 = 无限等待
|
||||
if (status != ResourceRequestStatus::GRANTED) {
|
||||
// 请求失败，释放已分配的资源
|
||||
for (Path* release_p : path) {
|
||||
if (release_p == p) break;
|
||||
resource_manager_->releasePath(agv, release_p);
|
||||
}
|
||||
std::cout << "[资源管理] AGV-" << agv->id << " 无法获取路径 " << p->id << std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
std::cout << "[资源管理] AGV-" << agv->id << " 成功获取 " << path.size() << " 条路径资源" << std::endl;
|
||||
} else {
|
||||
std::cout << "[资源管理] AGV-" << agv->id << " 路径冲突，任务 " << task->id << " 暂时无法分配" << std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
setAGVPath(agv->id, path);
|
||||
|
||||
// 分配任务
|
||||
agv->current_task = task;
|
||||
agv->state = AGVState::ASSIGNED;
|
||||
task->status = TaskStatus::ASSIGNED;
|
||||
task->assigned_agv = agv;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string EnhancedAGVManager::assignmentStrategyToString(AssignmentStrategy strategy) {
|
||||
switch (strategy) {
|
||||
case AssignmentStrategy::FIRST_FIT: return "FirstFit";
|
||||
case AssignmentStrategy::NEAREST_FIT: return "NearestFit";
|
||||
case AssignmentStrategy::BATTERY_AWARE: return "BatteryAware";
|
||||
case AssignmentStrategy::LOAD_BALANCED: return "LoadBalanced";
|
||||
case AssignmentStrategy::PRIORITY_BASED: return "PriorityBased";
|
||||
case AssignmentStrategy::COST_OPTIMAL: return "CostOptimal";
|
||||
default: return "Unknown";
|
||||
}
|
||||
}
|
||||
|
||||
void EnhancedAGVManager::completeTaskWithResources(AGV* agv, bool success) {
|
||||
if (!agv) return;
|
||||
|
||||
// 释放路径资源
|
||||
if (resource_manager_) {
|
||||
std::vector<Path*> path = getAGVPath(agv->id);
|
||||
if (!path.empty()) {
|
||||
for (Path* p : path) {
|
||||
resource_manager_->releasePath(agv, p);
|
||||
}
|
||||
std::cout << "[资源管理] AGV-" << agv->id << " 释放了 " << path.size() << " 条路径资源" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// 更新任务状态
|
||||
if (agv->current_task) {
|
||||
agv->current_task->status = success ? TaskStatus::COMPLETED : TaskStatus::FAILED;
|
||||
agv->current_task = nullptr;
|
||||
}
|
||||
|
||||
// 更新AGV状态
|
||||
agv->state = AGVState::IDLE;
|
||||
clearAGVPath(agv->id);
|
||||
}
|
||||
204
src/dispatch/enhanced_agv_manager.h
Normal file
204
src/dispatch/enhanced_agv_manager.h
Normal file
@@ -0,0 +1,204 @@
|
||||
#ifndef ENHANCED_AGV_MANAGER_H
|
||||
#define ENHANCED_AGV_MANAGER_H
|
||||
|
||||
#include "agv_manager_base.h"
|
||||
#include "resource_manager.h"
|
||||
#include <unordered_map>
|
||||
#include <functional>
|
||||
|
||||
/**
|
||||
* @brief AGV分配策略枚举
|
||||
*/
|
||||
enum class AssignmentStrategy {
|
||||
FIRST_FIT, // 第一个适配（当前策略）
|
||||
NEAREST_FIT, // 最近适配
|
||||
BATTERY_AWARE, // 考虑电量
|
||||
LOAD_BALANCED, // 负载均衡
|
||||
PRIORITY_BASED, // 基于优先级
|
||||
COST_OPTIMAL // 成本最优
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief AGV评估结果
|
||||
*/
|
||||
struct AGVEvaluation {
|
||||
AGV* agv;
|
||||
double score; // 评估分数
|
||||
double estimated_time; // 预计完成时间
|
||||
double distance_to_task; // 到任务起点的距离
|
||||
std::string reason; // 选择原因
|
||||
|
||||
AGVEvaluation(AGV* a, double s, double t, double d, const std::string& r)
|
||||
: agv(a), score(s), estimated_time(t), distance_to_task(d), reason(r) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 任务分配选项
|
||||
*/
|
||||
struct AssignmentOption {
|
||||
Task* task;
|
||||
AGV* agv;
|
||||
std::vector<Path*> path;
|
||||
double total_cost; // 总成本（时间、距离等）
|
||||
double estimated_duration; // 预计执行时间
|
||||
double battery_consumption; // 预计电量消耗
|
||||
|
||||
AssignmentOption(Task* t, AGV* a, const std::vector<Path*>& p, double c, double d, double b)
|
||||
: task(t), agv(a), path(p), total_cost(c), estimated_duration(d), battery_consumption(b) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 增强的AGV管理器
|
||||
* 支持多种任务分配策略
|
||||
*/
|
||||
class EnhancedAGVManager : public AGVManagerBase {
|
||||
private:
|
||||
AssignmentStrategy strategy_; // 当前分配策略
|
||||
std::unordered_map<AssignmentStrategy, std::function<double(AGV*, Task*, double)>> scoring_functions_;
|
||||
ResourceManager* resource_manager_; // 资源管理器集成
|
||||
|
||||
// 分配权重
|
||||
struct AssignmentWeights {
|
||||
double distance_weight = 0.4; // 距离权重
|
||||
double battery_weight = 0.3; // 电量权重
|
||||
double load_weight = 0.2; // 负载权重
|
||||
double speed_weight = 0.1; // 速度权重
|
||||
} weights_;
|
||||
|
||||
public:
|
||||
EnhancedAGVManager(GraphMap* map, ResourceManager* resource_mgr = nullptr, AssignmentStrategy strategy = AssignmentStrategy::NEAREST_FIT)
|
||||
: AGVManagerBase(map), strategy_(strategy), resource_manager_(resource_mgr) {
|
||||
initializeScoringFunctions();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置资源管理器
|
||||
*/
|
||||
void setResourceManager(ResourceManager* resource_mgr) {
|
||||
resource_manager_ = resource_mgr;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置分配策略
|
||||
*/
|
||||
void setStrategy(AssignmentStrategy strategy) {
|
||||
strategy_ = strategy;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置分配权重
|
||||
*/
|
||||
void setWeights(const AssignmentWeights& weights) {
|
||||
weights_ = weights;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 智能任务分配
|
||||
* 使用当前策略找到最优的AGV-任务匹配
|
||||
*/
|
||||
int assignTasksSmart();
|
||||
|
||||
/**
|
||||
* @brief 为特定任务寻找最佳AGV
|
||||
* @param task 要分配的任务
|
||||
* @param exclude_excluded 排除已分配任务的AGV
|
||||
* @return 评估结果列表，按分数排序
|
||||
*/
|
||||
std::vector<AGVEvaluation> findBestAGVForTask(Task* task, bool exclude_assigned = true);
|
||||
|
||||
/**
|
||||
* @brief 计算任务-AGV匹配的成本
|
||||
*/
|
||||
double calculateAssignmentCost(AGV* agv, Task* task, std::vector<Path*>& path);
|
||||
|
||||
/**
|
||||
* @brief 批量任务分配优化
|
||||
* 同时考虑多个任务，找到全局最优分配
|
||||
*/
|
||||
std::vector<AssignmentOption> optimizeBatchAssignment(std::vector<Task*>& tasks,
|
||||
std::vector<AGV*>& available_agvs);
|
||||
|
||||
/**
|
||||
* @brief 预测AGV在未来时间段的可用性
|
||||
*/
|
||||
std::pair<double, bool> predictAvailability(AGV* agv, double future_time);
|
||||
|
||||
/**
|
||||
* @brief 获取AGV负载情况
|
||||
*/
|
||||
double getAGVLoad(AGV* agv);
|
||||
|
||||
/**
|
||||
* @brief 打印分配策略分析
|
||||
*/
|
||||
void printAssignmentAnalysis();
|
||||
|
||||
/**
|
||||
* @brief 完成任务并释放资源
|
||||
*/
|
||||
void completeTaskWithResources(AGV* agv, bool success = true);
|
||||
|
||||
private:
|
||||
void initializeScoringFunctions();
|
||||
|
||||
// 各种分配策略的评分函数
|
||||
double scoreFirstFit(AGV* agv, Task* task, double distance);
|
||||
double scoreNearestFit(AGV* agv, Task* task, double distance);
|
||||
double scoreBatteryAware(AGV* agv, Task* task, double distance);
|
||||
double scoreLoadBalanced(AGV* agv, Task* task, double distance);
|
||||
double scorePriorityBased(AGV* agv, Task* task, double distance);
|
||||
double scoreCostOptimal(AGV* agv, Task* task, double distance);
|
||||
|
||||
// 辅助函数
|
||||
double calculateDistance(AGV* agv, Task* task);
|
||||
double calculateBatteryConsumption(AGV* agv, Task* task, double distance);
|
||||
double calculateLoadFactor(AGV* agv);
|
||||
std::vector<Path*> findPathWithAvoidance(AGV* agv, Task* task);
|
||||
std::string getReasonForSelection(AGV* agv, Task* task, double distance, double score);
|
||||
std::string agvStateToString(AGVState state);
|
||||
|
||||
// 新增辅助方法
|
||||
bool assignSingleTask(AGV* agv, Task* task);
|
||||
std::string assignmentStrategyToString(AssignmentStrategy strategy);
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 任务分配分析器
|
||||
* 用于分析和优化分配策略
|
||||
*/
|
||||
class AssignmentAnalyzer {
|
||||
private:
|
||||
struct Metrics {
|
||||
double avg_completion_time;
|
||||
double agv_utilization;
|
||||
double battery_efficiency;
|
||||
int task_count;
|
||||
int reassignments;
|
||||
};
|
||||
|
||||
std::unordered_map<AssignmentStrategy, Metrics> strategy_metrics_;
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief 记录任务分配结果
|
||||
*/
|
||||
void recordAssignment(AssignmentStrategy strategy, AGV* agv, Task* task,
|
||||
double completion_time, double battery_used);
|
||||
|
||||
/**
|
||||
* @brief 获取最优策略
|
||||
*/
|
||||
AssignmentStrategy getOptimalStrategy();
|
||||
|
||||
/**
|
||||
* @brief 生成分配报告
|
||||
*/
|
||||
void generateReport();
|
||||
|
||||
/**
|
||||
* @brief 重置统计数据
|
||||
*/
|
||||
void reset();
|
||||
};
|
||||
|
||||
#endif // ENHANCED_AGV_MANAGER_H
|
||||
367
src/dispatch/enhanced_agv_manager_old.cpp
Normal file
367
src/dispatch/enhanced_agv_manager_old.cpp
Normal file
@@ -0,0 +1,367 @@
|
||||
#include "enhanced_agv_manager.h"
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
|
||||
void EnhancedAGVManager::initializeScoringFunctions() {
|
||||
scoring_functions_[AssignmentStrategy::FIRST_FIT] =
|
||||
[this](AGV* agv, Task* task, double distance) { return scoreFirstFit(agv, task, distance); };
|
||||
|
||||
scoring_functions_[AssignmentStrategy::NEAREST_FIT] =
|
||||
[this](AGV* agv, Task* task, double distance) { return scoreNearestFit(agv, task, distance); };
|
||||
|
||||
scoring_functions_[AssignmentStrategy::BATTERY_AWARE] =
|
||||
[this](AGV* agv, Task* task, double distance) { return scoreBatteryAware(agv, task, distance); };
|
||||
|
||||
scoring_functions_[AssignmentStrategy::LOAD_BALANCED] =
|
||||
[this](AGV* agv, Task* task, double distance) { return scoreLoadBalanced(agv, task, distance); };
|
||||
|
||||
scoring_functions_[AssignmentStrategy::PRIORITY_BASED] =
|
||||
[this](AGV* agv, Task* task, double distance) { return scorePriorityBased(agv, task, distance); };
|
||||
|
||||
scoring_functions_[AssignmentStrategy::COST_OPTIMAL] =
|
||||
[this](AGV* agv, Task* task, double distance) { return scoreCostOptimal(agv, task, distance); };
|
||||
}
|
||||
|
||||
int EnhancedAGVManager::assignTasksSmart() {
|
||||
int assigned_count = 0;
|
||||
|
||||
// 获取所有可用AGV
|
||||
std::vector<AGV*> available_agvs = getIdleAGVs();
|
||||
|
||||
// 获取待分配任务
|
||||
std::vector<Task> pending_tasks = getTaskQueueCopy();
|
||||
|
||||
if (available_agvs.empty() || pending_tasks.empty()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 根据策略进行分配
|
||||
if (strategy_ == AssignmentStrategy::COST_OPTIMAL) {
|
||||
// 批量优化分配
|
||||
std::vector<Task*> task_ptrs;
|
||||
for (auto& t : pending_tasks) {
|
||||
task_ptrs.push_back(&t);
|
||||
}
|
||||
|
||||
auto assignments = optimizeBatchAssignment(task_ptrs, available_agvs);
|
||||
|
||||
// 执行分配
|
||||
for (const auto& option : assignments) {
|
||||
if (option.agv->assignTask(option.task, option.path)) {
|
||||
assigned_count++;
|
||||
std::cout << "[智能调度] 任务 " << option.task->id
|
||||
<< " 分配给 AGV-" << option.agv->name
|
||||
<< " (成本: " << option.total_cost << ")" << std::endl;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// 逐个分配
|
||||
for (auto& task : pending_tasks) {
|
||||
auto evaluations = findBestAGVForTask(&task);
|
||||
|
||||
if (!evaluations.empty() && evaluations[0].score > 0) {
|
||||
AGV* best_agv = evaluations[0].agv;
|
||||
std::vector<Path*> path = findPathWithAvoidance(best_agv, &task);
|
||||
|
||||
if (!path.empty() && best_agv->assignTask(&task, path)) {
|
||||
assigned_count++;
|
||||
std::cout << "[智能调度] 任务 " << task.id
|
||||
<< " 分配给 AGV-" << best_agv->name
|
||||
<< " (" << evaluations[0].reason << ")" << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return assigned_count;
|
||||
}
|
||||
|
||||
std::vector<AGVEvaluation> EnhancedAGVManager::findBestAGVForTask(Task* task, bool exclude_assigned) {
|
||||
std::vector<AGVEvaluation> evaluations;
|
||||
|
||||
for (const auto& pair : agvs_) {
|
||||
AGV* agv = pair.second.get();
|
||||
|
||||
// 排除不可用的AGV
|
||||
if (!agv->isAvailable()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (exclude_assigned && agv->current_task) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 计算距离
|
||||
double distance = calculateDistance(agv, task);
|
||||
|
||||
// 使用当前策略评分
|
||||
auto scoring_func = scoring_functions_[strategy_];
|
||||
double score = scoring_func(agv, task, distance);
|
||||
|
||||
if (score > 0) {
|
||||
std::string reason = getReasonForSelection(agv, task, distance, score);
|
||||
evaluations.emplace_back(agv, score, 0, distance, reason);
|
||||
}
|
||||
}
|
||||
|
||||
// 按分数排序（降序）
|
||||
std::sort(evaluations.begin(), evaluations.end(),
|
||||
[](const AGVEvaluation& a, const AGVEvaluation& b) {
|
||||
return a.score > b.score;
|
||||
});
|
||||
|
||||
return evaluations;
|
||||
}
|
||||
|
||||
std::vector<AssignmentOption> EnhancedAGVManager::optimizeBatchAssignment(
|
||||
std::vector<Task*>& tasks, std::vector<AGV*>& available_agvs) {
|
||||
|
||||
std::vector<AssignmentOption> assignments;
|
||||
|
||||
// 使用匈牙利算法或贪心算法进行优化
|
||||
// 这里使用简化的贪心算法
|
||||
|
||||
for (Task* task : tasks) {
|
||||
AGVEvaluation best_eval(nullptr, -1, 0, 0, "");
|
||||
|
||||
for (AGV* agv : available_agvs) {
|
||||
if (!agv->isAvailable()) continue;
|
||||
|
||||
std::vector<Path*> path = findPathWithAvoidance(agv, task);
|
||||
if (!path.empty()) {
|
||||
double cost = calculateAssignmentCost(agv, task, path);
|
||||
double score = scoring_functions_[AssignmentStrategy::COST_OPTIMAL](agv, task, cost);
|
||||
|
||||
if (score > best_eval.score) {
|
||||
double duration = 0;
|
||||
for (auto p : path) {
|
||||
duration += p->length / agv->max_speed;
|
||||
}
|
||||
|
||||
best_eval = AGVEvaluation(agv, score, duration, 0, "最优成本");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (best_eval.agv) {
|
||||
auto path = findPathWithAvoidance(best_eval.agv, task);
|
||||
double duration = 0;
|
||||
double battery = calculateBatteryConsumption(best_eval.agv, task, 0);
|
||||
|
||||
assignments.emplace_back(task, best_eval.agv, path,
|
||||
best_eval.score, best_eval.estimated_time, battery);
|
||||
|
||||
// 标记AGV为已分配
|
||||
best_eval.agv->setState(AGVState::ASSIGNED);
|
||||
}
|
||||
}
|
||||
|
||||
return assignments;
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::calculateAssignmentCost(AGV* agv, Task* task, std::vector<Path*>& path) {
|
||||
double total_cost = 0;
|
||||
|
||||
// 距离成本
|
||||
double distance = 0;
|
||||
for (auto p : path) {
|
||||
distance += p->length;
|
||||
}
|
||||
total_cost += distance * weights_.distance_weight;
|
||||
|
||||
// 电量成本
|
||||
double battery_cost = calculateBatteryConsumption(agv, task, distance);
|
||||
total_cost += battery_cost * weights_.battery_weight;
|
||||
|
||||
// 负载成本
|
||||
double load_cost = calculateLoadFactor(agv);
|
||||
total_cost += load_cost * weights_.load_weight;
|
||||
|
||||
// 时间成本
|
||||
double time_cost = distance / agv->max_speed;
|
||||
total_cost += time_cost * weights_.speed_weight;
|
||||
|
||||
return total_cost;
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::scoreFirstFit(AGV* agv, Task* task, double distance) {
|
||||
return 1.0; // 所有可用的AGV得分相同
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::scoreNearestFit(AGV* agv, Task* task, double distance) {
|
||||
if (distance == 0) return 100.0;
|
||||
return 100.0 / (1.0 + distance); // 距离越近得分越高
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::scoreBatteryAware(AGV* agv, Task* task, double distance) {
|
||||
double battery_score = agv->battery_level / 100.0;
|
||||
double distance_score = distance > 0 ? 10.0 / distance : 10.0;
|
||||
return battery_score * distance_score;
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::scoreLoadBalanced(AGV* agv, Task* task, double distance) {
|
||||
double load_factor = 1.0 - calculateLoadFactor(agv); // 负载越低得分越高
|
||||
double distance_score = distance > 0 ? 10.0 / distance : 10.0;
|
||||
return load_factor * distance_score;
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::scorePriorityBased(AGV* agv, Task* task, double distance) {
|
||||
double priority_score = task->priority / 10.0; // 假设优先级1-10
|
||||
double agv_speed_score = agv->max_speed / 5.0; // 假设最大速度5m/s
|
||||
return priority_score * agv_speed_score;
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::scoreCostOptimal(AGV* agv, Task* task, double distance) {
|
||||
std::vector<Path*> path = findPathWithAvoidance(agv, task);
|
||||
if (path.empty()) return 0;
|
||||
|
||||
double cost = calculateAssignmentCost(agv, task, path);
|
||||
return cost > 0 ? 1000.0 / cost : 0; // 成本越低得分越高
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::calculateDistance(AGV* agv, Task* task) {
|
||||
Point* task_start = graph_map_->getPoint(task->start_point_id);
|
||||
if (!task_start || !agv->current_position) {
|
||||
return std::numeric_limits<double>::max();
|
||||
}
|
||||
|
||||
return agv->current_position->distanceTo(*task_start);
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::calculateBatteryConsumption(AGV* agv, Task* task, double distance) {
|
||||
if (distance == 0) {
|
||||
// 需要计算实际距离
|
||||
Point* task_start = graph_map_->getPoint(task->start_point_id);
|
||||
Point* task_end = graph_map_->getPoint(task->end_point_id);
|
||||
if (task_start && task_end) {
|
||||
distance = task_start->distanceTo(*task_end);
|
||||
}
|
||||
}
|
||||
|
||||
// 简化模型：每米消耗0.1%电量
|
||||
return distance * 0.1;
|
||||
}
|
||||
|
||||
double EnhancedAGVManager::calculateLoadFactor(AGV* agv) {
|
||||
// 基于AGV的历史任务数和当前状态计算负载
|
||||
// 这里简化实现
|
||||
int total_tasks = 0;
|
||||
for (const auto& pair : agvs_) {
|
||||
total_tasks++;
|
||||
}
|
||||
|
||||
return (double)total_tasks / 10.0; // 简化计算
|
||||
}
|
||||
|
||||
std::vector<Path*> EnhancedAGVManager::findPathWithAvoidance(AGV* agv, Task* task) {
|
||||
Point* task_start = graph_map_->getPoint(task->start_point_id);
|
||||
Point* task_end = graph_map_->getPoint(task->end_point_id);
|
||||
|
||||
if (!task_start || !task_end) {
|
||||
return {};
|
||||
}
|
||||
|
||||
std::vector<AGV*> other_agvs;
|
||||
for (const auto& pair : agvs_) {
|
||||
if (pair.second.get() != agv) {
|
||||
other_agvs.push_back(pair.second.get());
|
||||
}
|
||||
}
|
||||
|
||||
// 如果AGV不在起点，需要规划到起点的路径
|
||||
std::vector<Path*> full_path;
|
||||
|
||||
if (agv->current_position != task_start) {
|
||||
auto path_to_start = graph_map_->findPath(agv->current_position, task_start, other_agvs);
|
||||
if (path_to_start.empty()) {
|
||||
return {};
|
||||
}
|
||||
full_path.insert(full_path.end(), path_to_start.begin(), path_to_start.end());
|
||||
}
|
||||
|
||||
// 从起点到终点
|
||||
auto path_to_end = graph_map_->findPath(task_start, task_end, other_agvs);
|
||||
if (path_to_end.empty()) {
|
||||
return {};
|
||||
}
|
||||
|
||||
full_path.insert(full_path.end(), path_to_end.begin(), path_to_end.end());
|
||||
|
||||
return full_path;
|
||||
}
|
||||
|
||||
std::string EnhancedAGVManager::getReasonForSelection(AGV* agv, Task* task,
|
||||
double distance, double score) {
|
||||
std::ostringstream oss;
|
||||
|
||||
switch (strategy_) {
|
||||
case AssignmentStrategy::NEAREST_FIT:
|
||||
oss << "最近距离: " << std::fixed << std::setprecision(2) << distance << "m";
|
||||
break;
|
||||
case AssignmentStrategy::BATTERY_AWARE:
|
||||
oss << "电量: " << agv->battery_level << "%";
|
||||
break;
|
||||
case AssignmentStrategy::LOAD_BALANCED:
|
||||
oss << "负载均衡";
|
||||
break;
|
||||
case AssignmentStrategy::PRIORITY_BASED:
|
||||
oss << "任务优先级: " << task->priority;
|
||||
break;
|
||||
case AssignmentStrategy::COST_OPTIMAL:
|
||||
oss << "最优成本";
|
||||
break;
|
||||
default:
|
||||
oss << "首个可用";
|
||||
}
|
||||
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
std::string EnhancedAGVManager::agvStateToString(AGVState state) {
|
||||
switch (state) {
|
||||
case AGVState::IDLE: return "空闲";
|
||||
case AGVState::ASSIGNED: return "已分配";
|
||||
case AGVState::MOVING: return "移动中";
|
||||
case AGVState::WAITING: return "等待中";
|
||||
case AGVState::CHARGING: return "充电中";
|
||||
case AGVState::ERROR: return "错误";
|
||||
case AGVState::MAINTENANCE: return "维护中";
|
||||
default: return "未知";
|
||||
}
|
||||
}
|
||||
|
||||
void EnhancedAGVManager::printAssignmentAnalysis() {
|
||||
std::cout << "\n===== 任务分配分析 =====" << std::endl;
|
||||
std::cout << "当前策略: ";
|
||||
|
||||
switch (strategy_) {
|
||||
case AssignmentStrategy::FIRST_FIT: std::cout << "首个适配"; break;
|
||||
case AssignmentStrategy::NEAREST_FIT: std::cout << "最近适配"; break;
|
||||
case AssignmentStrategy::BATTERY_AWARE: std::cout << "电量感知"; break;
|
||||
case AssignmentStrategy::LOAD_BALANCED: std::cout << "负载均衡"; break;
|
||||
case AssignmentStrategy::PRIORITY_BASED: std::cout << "基于优先级"; break;
|
||||
case AssignmentStrategy::COST_OPTIMAL: std::cout << "成本最优"; break;
|
||||
}
|
||||
|
||||
std::cout << "\n权重配置:" << std::endl;
|
||||
std::cout << " 距离权重: " << weights_.distance_weight << std::endl;
|
||||
std::cout << " 电量权重: " << weights_.battery_weight << std::endl;
|
||||
std::cout << " 负载权重: " << weights_.load_weight << std::endl;
|
||||
std::cout << " 速度权重: " << weights_.speed_weight << std::endl;
|
||||
|
||||
// 打印各AGV状态
|
||||
std::cout << "\nAGV状态:" << std::endl;
|
||||
for (const auto& pair : agvs_) {
|
||||
AGV* agv = pair.second.get();
|
||||
std::cout << " AGV-" << agv->name << ": ";
|
||||
std::cout << "位置=" << agv->current_position->name;
|
||||
std::cout << ", 电量=" << agv->battery_level << "%";
|
||||
std::cout << ", 状态=" << agvStateToString(agv->getState());
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "========================" << std::endl;
|
||||
}
|
||||
354
src/dispatch/graph_map.cpp
Normal file
354
src/dispatch/graph_map.cpp
Normal 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;
|
||||
}
|
||||
155
src/dispatch/graph_map.h
Normal file
155
src/dispatch/graph_map.h
Normal file
@@ -0,0 +1,155 @@
|
||||
#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);
|
||||
|
||||
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
|
||||
Reference in New Issue
Block a user