From de2450e77519f3dd2f9ede3f582c56efc4d76c56 Mon Sep 17 00:00:00 2001 From: TY <1431301249@qq.com> Date: Fri, 9 Jan 2026 15:01:58 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E6=96=87=E4=BB=B6=E8=87=B3?= =?UTF-8?q?=20src/dispatch?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/dispatch/enhanced_agv_manager.cpp | 454 ++++++++++++++++++++++ src/dispatch/enhanced_agv_manager.h | 204 ++++++++++ src/dispatch/enhanced_agv_manager_old.cpp | 367 +++++++++++++++++ src/dispatch/graph_map.cpp | 354 +++++++++++++++++ src/dispatch/graph_map.h | 155 ++++++++ 5 files changed, 1534 insertions(+) create mode 100644 src/dispatch/enhanced_agv_manager.cpp create mode 100644 src/dispatch/enhanced_agv_manager.h create mode 100644 src/dispatch/enhanced_agv_manager_old.cpp create mode 100644 src/dispatch/graph_map.cpp create mode 100644 src/dispatch/graph_map.h diff --git a/src/dispatch/enhanced_agv_manager.cpp b/src/dispatch/enhanced_agv_manager.cpp new file mode 100644 index 0000000..e309d98 --- /dev/null +++ b/src/dispatch/enhanced_agv_manager.cpp @@ -0,0 +1,454 @@ +#include "enhanced_agv_manager.h" +#include +#include +#include +#include +#include + +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 available_agvs = getIdleAGVs(); + + // 获取待分配任务 + std::vector 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 = 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 EnhancedAGVManager::findBestAGVForTask(Task* task, bool exclude_assigned) { + std::vector 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) { + if (!agv || !task || path.empty()) { + return std::numeric_limits::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 EnhancedAGVManager::optimizeBatchAssignment( + std::vector& tasks, std::vector& available_agvs) { + + std::vector all_options; + + // 生成所有可能的AGV-任务组合 + for (Task* task : tasks) { + for (AGV* agv : available_agvs) { + if (agv->state != AGVState::IDLE) continue; + + std::vector 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 selected_assignments; + std::unordered_set used_agvs; + std::unordered_set 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 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::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 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 = 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 = 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); +} \ No newline at end of file diff --git a/src/dispatch/enhanced_agv_manager.h b/src/dispatch/enhanced_agv_manager.h new file mode 100644 index 0000000..a238838 --- /dev/null +++ b/src/dispatch/enhanced_agv_manager.h @@ -0,0 +1,204 @@ +#ifndef ENHANCED_AGV_MANAGER_H +#define ENHANCED_AGV_MANAGER_H + +#include "agv_manager_base.h" +#include "resource_manager.h" +#include +#include + +/** + * @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; + double total_cost; // 总成本（时间、距离等） + double estimated_duration; // 预计执行时间 + double battery_consumption; // 预计电量消耗 + + AssignmentOption(Task* t, AGV* a, const std::vector& 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> 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 findBestAGVForTask(Task* task, bool exclude_assigned = true); + + /** + * @brief 计算任务-AGV匹配的成本 + */ + double calculateAssignmentCost(AGV* agv, Task* task, std::vector& path); + + /** + * @brief 批量任务分配优化 + * 同时考虑多个任务，找到全局最优分配 + */ + std::vector optimizeBatchAssignment(std::vector& tasks, + std::vector& available_agvs); + + /** + * @brief 预测AGV在未来时间段的可用性 + */ + std::pair 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 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 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 \ No newline at end of file diff --git a/src/dispatch/enhanced_agv_manager_old.cpp b/src/dispatch/enhanced_agv_manager_old.cpp new file mode 100644 index 0000000..fe896cb --- /dev/null +++ b/src/dispatch/enhanced_agv_manager_old.cpp @@ -0,0 +1,367 @@ +#include "enhanced_agv_manager.h" +#include +#include +#include +#include +#include + +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 available_agvs = getIdleAGVs(); + + // 获取待分配任务 + std::vector pending_tasks = getTaskQueueCopy(); + + if (available_agvs.empty() || pending_tasks.empty()) { + return 0; + } + + // 根据策略进行分配 + if (strategy_ == AssignmentStrategy::COST_OPTIMAL) { + // 批量优化分配 + std::vector 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 = 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 EnhancedAGVManager::findBestAGVForTask(Task* task, bool exclude_assigned) { + std::vector 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 EnhancedAGVManager::optimizeBatchAssignment( + std::vector& tasks, std::vector& available_agvs) { + + std::vector assignments; + + // 使用匈牙利算法或贪心算法进行优化 + // 这里使用简化的贪心算法 + + for (Task* task : tasks) { + AGVEvaluation best_eval(nullptr, -1, 0, 0, ""); + + for (AGV* agv : available_agvs) { + if (!agv->isAvailable()) continue; + + std::vector 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) { + 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 = 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::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 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 other_agvs; + for (const auto& pair : agvs_) { + if (pair.second.get() != agv) { + other_agvs.push_back(pair.second.get()); + } + } + + // 如果AGV不在起点，需要规划到起点的路径 + std::vector 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; +} \ No newline at end of file diff --git a/src/dispatch/graph_map.cpp b/src/dispatch/graph_map.cpp new file mode 100644 index 0000000..1d83510 --- /dev/null +++ b/src/dispatch/graph_map.cpp @@ -0,0 +1,354 @@ +#include "graph_map.h" +#include +#include +#include + +// A*路径规划实现（简化版，不考虑其他AGV） +std::vector GraphMap::findPath(Point* start, Point* end, const std::vector& 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 open_set; + std::unordered_map came_from; + std::unordered_map 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_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& 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 lock(map_mutex_); + + // 如果路径未被占用，则可用 + if (!path->isOccupied()) { + return true; + } + + // 如果被请求者自己占用，也认为可用（已在路上） + return path->occupied_by == requester; +} + +void GraphMap::reservePath(Path* path, AGV* agv) { + std::lock_guard lock(map_mutex_); + path->occupied_by = agv; +} + +void GraphMap::releasePath(Path* path, AGV* agv) { + std::lock_guard lock(map_mutex_); + if (path->occupied_by == agv) { + path->occupied_by = nullptr; + } +} + +// K最短路径实现 (简化版Yen's算法变体) +std::vector> GraphMap::findKShortestPaths( + Point* start, + Point* end, + int K, + const std::vector& avoid_agvs +) { + std::vector> results; + + if (K <= 0 || start == nullptr || end == nullptr || start == end) { + return results; + } + + // 1. 找到最短路径 (使用标准A*) + std::vector 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 edge_penalties; + + for (int k = 1; k < K; ++k) { + std::vector best_alternative; + double best_cost = 1e9; + + // 尝试惩罚前一条路径中的每条边，寻找替代方案 + const std::vector& 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 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 GraphMap::_findPathWithPenalties( + Point* start, + Point* end, + const std::unordered_map& penalties, + const std::vector& 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 open_set; + std::unordered_map came_from; + std::unordered_map 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_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& 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& path1, const std::vector& 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) { + 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; +} \ No newline at end of file diff --git a/src/dispatch/graph_map.h b/src/dispatch/graph_map.h new file mode 100644 index 0000000..f5eac04 --- /dev/null +++ b/src/dispatch/graph_map.h @@ -0,0 +1,155 @@ +#ifndef GRAPH_MAP_H +#define GRAPH_MAP_H + +#include +#include +#include +#include +#include +#include +#include + +// 前向声明 +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> points_; + std::unordered_map> paths_; + std::unordered_map> 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 lock(map_mutex_); + auto point = std::make_unique(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 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(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& getAdjacentPaths(Point* point) { + static std::vector empty; + auto it = adj_paths_.find(point); + return it != adj_paths_.end() ? it->second : empty; + } + + // 获取所有路径（用于清理AGV占用） + std::vector getAllPaths() { + std::lock_guard lock(map_mutex_); + std::vector all_paths; + for (const auto& pair : paths_) { + all_paths.push_back(pair.second.get()); + } + return all_paths; + } + + // A*路径规划 + std::vector findPath(Point* start, Point* end, const std::vector& 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 lock(map_mutex_); + num_points = points_.size(); + num_paths = paths_.size(); + } +}; + +#endif // GRAPH_MAP_H \ No newline at end of file