From 38f4fdd93dfc40ac1d9eae31a52d7ae03e789991 Mon Sep 17 00:00:00 2001 From: TY <1431301249@qq.com> Date: Fri, 9 Jan 2026 15:01:36 +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/CLEANUP_NOTES.md | 92 ++++++++ src/dispatch/agv_executor.h | 270 ++++++++++++++++++++++++ src/dispatch/agv_manager_base.cpp | 215 +++++++++++++++++++ src/dispatch/agv_manager_base.h | 171 +++++++++++++++ src/dispatch/agv_manager_standalone.cpp | 257 ++++++++++++++++++++++ 5 files changed, 1005 insertions(+) create mode 100644 src/dispatch/CLEANUP_NOTES.md create mode 100644 src/dispatch/agv_executor.h create mode 100644 src/dispatch/agv_manager_base.cpp create mode 100644 src/dispatch/agv_manager_base.h create mode 100644 src/dispatch/agv_manager_standalone.cpp diff --git a/src/dispatch/CLEANUP_NOTES.md b/src/dispatch/CLEANUP_NOTES.md new file mode 100644 index 0000000..32715d4 --- /dev/null +++ b/src/dispatch/CLEANUP_NOTES.md @@ -0,0 +1,92 @@ +# Dispatch模块代码清理说明 + +## 📅 清理时间：2025年12月22日 + +## 🎯 清理目标 +删除冗余的旧版本代码，保留最新、功能最完整的版本。 + +## ❌ 已删除的冗余代码 + +### 1. dispatcher.h/cpp (旧版调度器) +- **删除原因**：依赖硬件层（agv_model.h, path_tracker.h, CurtisMotorController.h） +- **替代方案**：advanced_dispatcher.h/cpp - 基于纯图结构的通用调度器 +- **功能缺失**：旧版不支持多AGV资源管理和防碰撞 + +### 2. agv_manager.h/cpp (基础版AGV管理器) +- **删除原因**：功能被enhanced_agv_manager完全包含 +- **替代方案**：enhanced_agv_manager.h/cpp - 支持6种任务分配策略 +- **功能增强**：新版本提供更智能的任务分配算法 + +## ✅ 保留的核心代码 + +### 核心架构组件 +1. **graph_map.h/cpp** - 地图图结构 + - A*路径规划算法 + - 节点和路径管理 + - 资源占用机制 + +2. **resource_manager.h/cpp** - 资源管理器 + - 路径和节点资源互斥 + - 等待队列管理 + - 死锁检测 + +3. **agv_executor.h/cpp** - AGV执行器 + - 路径分段执行 + - 反馈处理机制 + - 线程安全控制 + +4. **enhanced_agv_manager.h/cpp** - 增强版AGV管理器 + - 6种任务分配策略 + - 智能AGV选择算法 + - 综合评分系统 + +5. **advanced_dispatcher.h/cpp** - 高级调度器 + - 多线程实时调度 + - 动态任务分配 + - 系统状态监控 + +## 🔧 更新的文件 + +### 1. CMakeLists.txt +- 移除：`src/dispatch/dispatcher.cpp`, `src/dispatch/agv_manager.cpp` +- 保留：新版本的所有源文件 + +### 2. compile_dispatcher.bat +- 更新源文件列表，移除已删除的文件 + +### 3. 引用修复 +- `examples/dispatcher_demo.cpp`: dispatcher.h → advanced_dispatcher.h +- `test_dispatch_integration.cpp`: agv_manager.h → enhanced_agv_manager.h +- 相关类型名称同步更新 + +## 📊 清理效果 + +### 文件数量对比 +- **清理前**：14个文件（8个.h + 6个.cpp） +- **清理后**：10个文件（5个.h + 5个.cpp） +- **减少**：4个冗余文件（28.6%减少） + +### 代码架构优化 +- ✅ **消除功能重叠**：避免多版本维护 +- ✅ **简化依赖关系**：移除硬件层依赖 +- ✅ **提升代码质量**：保留功能最完整的版本 +- ✅ **便于维护**：统一使用新版本API + +## 🚀 新架构优势 + +1. **模块化设计**：各组件职责清晰，易于扩展 +2. **通用性**：不依赖特定硬件，可适配多种AGV类型 +3. **功能完整**：包含任务分配、路径规划、资源管理、执行控制 +4. **高性能**：多线程并发处理，支持实时调度 +5. **可扩展**：支持多种分配策略，便于后续优化 + +## 📝 后续建议 + +1. **统一API**：确保所有测试程序使用新版本API +2. **文档更新**：更新相关文档，反映新的模块结构 +3. **测试验证**：创建基于新架构的综合测试 +4. **性能优化**：针对新架构进行性能调优 + +## 🎉 清理完成！ + +Dispatch模块现在拥有更清晰、更高效的代码结构，为后续开发和维护奠定了良好基础。 \ No newline at end of file diff --git a/src/dispatch/agv_executor.h b/src/dispatch/agv_executor.h new file mode 100644 index 0000000..02aef5d --- /dev/null +++ b/src/dispatch/agv_executor.h @@ -0,0 +1,270 @@ +#ifndef AGV_EXECUTOR_H +#define AGV_EXECUTOR_H + +#include "resource_manager.h" +#include "enhanced_agv_manager.h" +#include +#include +#include +#include +#include +#include + +/** + * @brief AGV执行器 + * 负责AGV路径的分段执行和资源管理 + */ +class AGVExecutor { +public: + /** + * @brief 执行状态 + */ + enum class ExecutionState { + IDLE, // 空闲 + PLANNING, // 规划中 + REQUESTING, // 请求资源 + EXECUTING, // 执行中 + WAITING, // 等待资源 + COMPLETED, // 已完成 + ERROR, // 错误 + PAUSED // 暂停 + }; + +private: + AGV* agv_; // AGV指针 + ResourceManager* resource_manager_; // 资源管理器 + GraphMap* graph_map_; // 地图 + + // 执行状态 + std::atomic state_; + std::atomic should_stop_; + std::atomic is_paused_; + + // 路径信息 + std::vector planned_path_; // 完整规划路径 + size_t current_path_index_; // 当前路径索引 + Path* current_executing_path_; // 当前执行路径 + + // 执行线程 + std::thread execution_thread_; + mutable std::mutex execution_mutex_; + std::condition_variable execution_cv_; + + // 配置参数 + int request_timeout_ms_; // 资源请求超时时间（毫秒） + int execution_interval_ms_; // 执行间隔（毫秒） + double speed_factor_; // 速度因子（实际速度/最大速度） + + // 统计信息 + std::atomic total_paths_executed_; + std::atomic total_wait_time_; + std::atomic total_execution_time_; + + // 回调函数 + std::function path_started_callback_; + std::function path_completed_callback_; + std::function error_callback_; + +public: + AGVExecutor(AGV* agv, ResourceManager* resource_manager, GraphMap* graph_map); + + ~AGVExecutor(); + + /** + * @brief 设置执行参数 + */ + void setParameters(int request_timeout_ms = 5000, + int execution_interval_ms = 100, + double speed_factor = 1.0); + + /** + * @brief 开始执行路径 + * @param path 要执行的路径列表 + * @return 是否成功开始执行 + */ + bool startExecution(const std::vector& path); + + /** + * @brief 暂停执行 + */ + void pauseExecution(); + + /** + * @brief 恢复执行 + */ + void resumeExecution(); + + /** + * @brief 停止执行 + */ + void stopExecution(); + + /** + * @brief 强制停止（紧急情况） + */ + void forceStop(); + + /** + * @brief 获取执行状态 + */ + ExecutionState getState() const { return state_.load(); } + + /** + * @brief 获取当前执行的路径 + */ + Path* getCurrentPath() const; + + /** + * @brief 获取进度 + * @return 已完成的路径数 / 总路径数 + */ + std::pair getProgress() const; + + /** + * @brief 获取统计信息 + */ + void getStatistics(uint64_t& total_paths, uint64_t& wait_time, + uint64_t& execution_time) const; + + /** + * @brief 设置路径开始回调 + */ + void setPathStartedCallback(std::function callback) { + path_started_callback_ = callback; + } + + /** + * @brief 设置路径完成回调 + */ + void setPathCompletedCallback(std::function callback) { + path_completed_callback_ = callback; + } + + /** + * @brief 设置错误回调 + */ + void setErrorCallback(std::function callback) { + error_callback_ = callback; + } + + /** + * @brief 是否正在执行 + */ + bool isExecuting() const { + ExecutionState s = state_.load(); + return s == ExecutionState::PLANNING || s == ExecutionState::REQUESTING || + s == ExecutionState::EXECUTING || s == ExecutionState::WAITING; + } + + /** + * @brief 状态转换为字符串 + */ + static std::string stateToString(ExecutionState state); + +private: + /** + * @brief 执行线程主循环 + */ + void executionLoop(); + + /** + * @brief 执行单段路径 + * @param path 要执行的路径 + * @return 是否成功 + */ + bool executePathSegment(Path* path); + + /** + * @brief 请求路径资源 + * @param path 要请求的路径 + * @return 是否成功获取资源 + */ + bool requestPathResource(Path* path); + + /** + * @brief 释放路径资源 + * @param path 要释放的路径 + */ + void releasePathResource(Path* path); + + /** + * @brief 移动到下一个路径 + * @return 是否成功 + */ + bool moveToNextPath(); + + /** + * @brief 模拟路径执行 + * @param path 要执行的路径 + * @return 执行时间（毫秒） + */ + int simulatePathExecution(Path* path); + + /** + * @brief 处理错误 + * @param error_message 错误信息 + */ + void handleError(const std::string& error_message); + + /** + * @brief 更新AGV位置 + * @param path 执行完成的路径 + */ + void updateAGVPosition(Path* path); +}; + +/** + * @brief AGV执行器管理器 + * 管理所有AGV的执行器 + */ +class AGVExecutorManager { +private: + std::unordered_map> executors_; + ResourceManager* resource_manager_; + GraphMap* graph_map_; + +public: + AGVExecutorManager(ResourceManager* resource_manager, GraphMap* graph_map); + + /** + * @brief 添加AGV执行器 + */ + void addAGVExecutor(AGV* agv); + + /** + * @brief 移除AGV执行器 + */ + void removeAGVExecutor(int agv_id); + + /** + * @brief 获取AGV执行器 + */ + AGVExecutor* getExecutor(int agv_id); + + /** + * @brief 停止所有执行器 + */ + void stopAll(); + + /** + * @brief 暂停所有执行器 + */ + void pauseAll(); + + /** + * @brief 恢复所有执行器 + */ + void resumeAll(); + + /** + * @brief 获取执行统计 + */ + void getStatistics(std::map>& progress) const; + + /** + * @brief 打印所有执行器状态 + */ + void printStatus() const; +}; + +#endif // AGV_EXECUTOR_H \ No newline at end of file diff --git a/src/dispatch/agv_manager_base.cpp b/src/dispatch/agv_manager_base.cpp new file mode 100644 index 0000000..70dd0f3 --- /dev/null +++ b/src/dispatch/agv_manager_base.cpp @@ -0,0 +1,215 @@ +#include "agv_manager_base.h" +#include +#include + +void AGVManagerBase::addAGV(int id, double x, double y, const std::string& name) { + // 创建新AGV + AGV* agv = new AGV(); + agv->id = id; + agv->x = x; + agv->y = y; + agv->name = name.empty() ? "AGV-" + std::to_string(id) : name; + agv->state = AGVState::IDLE; + agv->current_position = nullptr; // 将在AGV移动到第一个节点时设置 + agv->battery_level = 100.0; + agv->max_speed = 2.0; + agv->current_task = nullptr; + + agvs_[id] = agv; + + std::cout << "[AGV管理器] 添加AGV-" << id << " 在位置(" << x << ", " << y << ")" << std::endl; +} + +void AGVManagerBase::removeAGV(int id) { + auto it = agvs_.find(id); + if (it != agvs_.end()) { + delete it->second; + agvs_.erase(it); + agv_paths_.erase(id); + std::cout << "[AGV管理器] 移除AGV-" << id << std::endl; + } +} + +AGV* AGVManagerBase::getAGV(int id) { + auto it = agvs_.find(id); + return it != agvs_.end() ? it->second : nullptr; +} + +std::vector AGVManagerBase::getAllAGVs() { + std::vector result; + for (const auto& pair : agvs_) { + result.push_back(pair.second); + } + return result; +} + +std::vector AGVManagerBase::getIdleAGVs() { + std::vector idle_agvs; + for (const auto& pair : agvs_) { + if (pair.second->state == AGVState::IDLE) { + idle_agvs.push_back(pair.second); + } + } + return idle_agvs; +} + +void AGVManagerBase::addTask(int task_id, int start_point_id, int end_point_id, + const std::string& description, int priority) { + Point* start_point = map_->getPointById(start_point_id); + Point* end_point = map_->getPointById(end_point_id); + + if (start_point && end_point) { + Task* task = new Task(task_id, priority, start_point, end_point, description); + task_queue_.emplace_back(task); + std::cout << "[AGV管理器] 添加任务 " << task_id << ": " << description << std::endl; + } else { + std::cout << "[AGV管理器] 错误: 任务 " << task_id << " 的节点不存在" << std::endl; + } +} + +std::vector AGVManagerBase::getPendingTasks() { + std::vector pending_tasks; + for (const auto& task : task_queue_) { + if (task->status == TaskStatus::PENDING) { + pending_tasks.push_back(task.get()); + } + } + return pending_tasks; +} + +void AGVManagerBase::setAGVPath(int agv_id, const std::vector& path) { + agv_paths_[agv_id] = path; +} + +std::vector AGVManagerBase::getAGVPath(int agv_id) { + auto it = agv_paths_.find(agv_id); + return it != agv_paths_.end() ? it->second : std::vector(); +} + +void AGVManagerBase::clearAGVPath(int agv_id) { + agv_paths_.erase(agv_id); +} + +int AGVManagerBase::assignTasks() { + int assigned_count = 0; + + // 获取空闲AGV + auto idle_agvs = getIdleAGVs(); + if (idle_agvs.empty()) { + return 0; + } + + // 获取待处理任务，按优先级排序 + auto pending_tasks = getPendingTasks(); + if (pending_tasks.empty()) { + return 0; + } + + // 按优先级排序任务 + std::sort(pending_tasks.begin(), pending_tasks.end(), + [](const Task* a, const Task* b) { + return a->priority > b->priority; // 高优先级在前 + }); + + // 简单的FIFO分配：第一个空闲AGV分配给最高优先级任务 + for (Task* task : pending_tasks) { + if (!idle_agvs.empty()) { + AGV* agv = idle_agvs[0]; + + // 为AGV规划路径 + std::vector path = findPath(agv, task); + if (!path.empty()) { + // 分配任务 + agv->current_task = task; + agv->state = AGVState::ASSIGNED; + task->status = TaskStatus::ASSIGNED; + task->assigned_agv = agv; + + // 设置路径 + setAGVPath(agv->id, path); + + // 从空闲列表中移除 + idle_agvs.erase(idle_agvs.begin()); + + assigned_count++; + std::cout << "[AGV管理器] 分配任务 " << task->id << " 给 AGV-" << agv->id + << " (路径长度: " << path.size() << ")" << std::endl; + } + } + } + + return assigned_count; +} + +std::vector AGVManagerBase::findPath(AGV* agv, Task* task) { + if (!agv || !task || !task->start_point || !task->end_point) { + return {}; + } + + // 使用GraphMap的A*算法寻找路径 + return map_->findPath(task->start_point, task->end_point, {}); +} + +double AGVManagerBase::calculateDistance(AGV* agv, Task* task) { + if (!agv || !task || !task->start_point) { + return 0.0; + } + + // 简单的欧几里得距离 + double dx = agv->x - task->start_point->x; + double dy = agv->y - task->start_point->y; + return std::sqrt(dx * dx + dy * dy); +} + +void AGVManagerBase::printAGVStatus() { + std::cout << "\n===== AGV状态 =====" << std::endl; + for (const auto& pair : agvs_) { + AGV* agv = pair.second; + std::string state_str; + switch (agv->state) { + case AGVState::IDLE: state_str = "空闲"; break; + case AGVState::ASSIGNED: state_str = "已分配"; break; + case AGVState::MOVING: state_str = "移动中"; break; + case AGVState::LOADING: state_str = "装载中"; break; + case AGVState::UNLOADING: state_str = "卸载中"; break; + case AGVState::CHARGING: state_str = "充电中"; break; + case AGVState::ERROR: state_str = "错误"; break; + case AGVState::MAINTENANCE: state_str = "维护中"; break; + default: state_str = "未知"; break; + } + + std::cout << "AGV-" << agv->id << " (" << agv->name << "): " + << state_str << " | 电量: " << agv->battery_level << "%" + << " | 位置: (" << agv->x << ", " << agv->y << ")"; + + if (agv->current_task) { + std::cout << " | 任务: " << agv->current_task->id; + } + std::cout << std::endl; + } + std::cout << "=================" << std::endl; +} + +void AGVManagerBase::printTaskQueue() { + std::cout << "\n===== 任务队列 =====" << std::endl; + for (const auto& task : task_queue_) { + std::string status_str; + switch (task->status) { + case TaskStatus::PENDING: status_str = "待处理"; break; + case TaskStatus::ASSIGNED: status_str = "已分配"; break; + case TaskStatus::IN_PROGRESS: status_str = "执行中"; break; + case TaskStatus::COMPLETED: status_str = "已完成"; break; + case TaskStatus::FAILED: status_str = "失败"; break; + default: status_str = "未知"; break; + } + + std::cout << "任务 " << task->id << ": " << task->description + << " | " << status_str << " | 优先级: " << task->priority; + + if (task->assigned_agv) { + std::cout << " | 分配给: AGV-" << task->assigned_agv->id; + } + std::cout << std::endl; + } + std::cout << "=================" << std::endl; +} \ No newline at end of file diff --git a/src/dispatch/agv_manager_base.h b/src/dispatch/agv_manager_base.h new file mode 100644 index 0000000..b9585df --- /dev/null +++ b/src/dispatch/agv_manager_base.h @@ -0,0 +1,171 @@ +#ifndef AGV_MANAGER_BASE_H +#define AGV_MANAGER_BASE_H + +#include "graph_map.h" +#include +#include +#include + +/** + * @brief AGV状态枚举 + */ +enum class AGVState { + IDLE, // 空闲 + ASSIGNED, // 已分配任务 + MOVING, // 移动中 + LOADING, // 装载中 + UNLOADING, // 卸载中 + CHARGING, // 充电中 + ERROR, // 错误状态 + MAINTENANCE // 维护中 +}; + +/** + * @brief 任务状态枚举 + */ +enum class TaskStatus { + PENDING, // 待处理 + ASSIGNED, // 已分配 + IN_PROGRESS, // 执行中 + COMPLETED, // 已完成 + FAILED // 失败 +}; + +// 前向声明以解决循环依赖 +struct Task; +struct AGV; + +/** + * @brief AGV小车结构 + */ +struct AGV { + int id; + std::string name; + AGVState state; + Point* current_position; + double x, y; // 坐标位置 + double battery_level; // 电量百分比 + double max_speed; // 最大速度 + Task* current_task; // 当前任务 + + AGV() + : id(0), name(""), state(AGVState::IDLE), current_position(nullptr), + x(0), y(0), battery_level(100.0), max_speed(2.0), current_task(nullptr) {} +}; + +/** + * @brief 任务结构 + */ +struct Task { + int id; + int priority; + Point* start_point; + Point* end_point; + std::string description; + TaskStatus status; + AGV* assigned_agv; + + Task(int task_id, int prio, Point* start, Point* end, const std::string& desc) + : id(task_id), priority(prio), start_point(start), end_point(end), + description(desc), status(TaskStatus::PENDING), assigned_agv(nullptr) {} +}; + +/** + * @brief AGV基础管理器 + * 提供AGV管理的基本功能，供EnhancedAGVManager继承 + */ +class AGVManagerBase { +protected: + GraphMap* map_; + std::unordered_map agvs_; + std::vector> task_queue_; + std::unordered_map> agv_paths_; + +public: + AGVManagerBase(GraphMap* map) : map_(map) {} + virtual ~AGVManagerBase() = default; + + /** + * @brief 添加AGV到管理器 + */ + virtual void addAGV(int id, double x, double y, const std::string& name = ""); + + /** + * @brief 移除AGV + */ + virtual void removeAGV(int id); + + /** + * @brief 获取AGV + */ + AGV* getAGV(int id); + + /** + * @brief 获取所有AGV + */ + std::vector getAllAGVs(); + + /** + * @brief 获取空闲AGV列表 + */ + std::vector getIdleAGVs(); + + /** + * @brief 获取AGV数量 + */ + size_t getAGVCount() const { return agvs_.size(); } + + /** + * @brief 添加任务到队列 + */ + virtual void addTask(int task_id, int start_point_id, int end_point_id, + const std::string& description, int priority = 1); + + /** + * @brief 获取待处理任务 + */ + std::vector getPendingTasks(); + + /** + * @brief 为AGV设置路径 + */ + void setAGVPath(int agv_id, const std::vector& path); + + /** + * @brief 获取AGV的当前路径 + */ + std::vector getAGVPath(int agv_id); + + /** + * @brief 清除AGV路径 + */ + void clearAGVPath(int agv_id); + + /** + * @brief 任务分配（基类实现简单的FIFO分配） + */ + virtual int assignTasks(); + + /** + * @brief 打印AGV状态 + */ + virtual void printAGVStatus(); + + /** + * @brief 打印任务队列状态 + */ + virtual void printTaskQueue(); + +protected: + /** + * @brief 寻找路径（使用GraphMap的A*算法） + */ + std::vector findPath(AGV* agv, Task* task); + + /** + * @brief 计算两点间距离 + */ + double calculateDistance(AGV* agv, Task* task); +}; + +#endif // AGV_MANAGER_BASE_H \ No newline at end of file diff --git a/src/dispatch/agv_manager_standalone.cpp b/src/dispatch/agv_manager_standalone.cpp new file mode 100644 index 0000000..0f8d95e --- /dev/null +++ b/src/dispatch/agv_manager_standalone.cpp @@ -0,0 +1,257 @@ +#include "agv_manager_standalone.h" +#include +#include + +AGVManager::AGVManager(GraphMap* map) : graph_map_(map) { + initializeTaskQueue(); +} + +void AGVManager::initializeTaskQueue() { + task_queue_ = std::priority_queue, + std::function([](Task* a, Task* b) { + return a->priority < b->priority; // 高优先级在前 + }); +} + +AGV* AGVManager::addAGV(int id, const std::string& name, int start_point_id) { + std::lock_guard lock(agv_mutex_); + + auto point = graph_map_->getPoint(start_point_id); + if (!point) { + std::cerr << "[错误] 节点" << start_point_id << "不存在" << std::endl; + return nullptr; + } + + auto agv = std::make_unique(id, name, start_point_id, point); + AGV* ptr = agv.get(); + agvs_[id] = std::move(agv); + + return ptr; +} + +AGV* AGVManager::getAGV(int id) { + std::lock_guard lock(agv_mutex_); + auto it = agvs_.find(id); + return (it != agvs_.end()) ? it->second.get() : nullptr; +} + +std::vector AGVManager::getAllAGVs() { + std::lock_guard lock(agv_mutex_); + std::vector result; + result.reserve(agvs_.size()); + + for (const auto& pair : agvs_) { + result.push_back(pair.second.get()); + } + + return result; +} + +std::vector AGVManager::getIdleAGVs() { + std::lock_guard lock(agv_mutex_); + std::vector result; + + for (const auto& pair : agvs_) { + AGV* agv = pair.second.get(); + if (agv->state == AGVState::IDLE) { + result.push_back(agv); + } + } + + return result; +} + +int AGVManager::addTask(int priority, int start_point_id, int end_point_id, const std::string& description) { + std::lock_guard lock(task_mutex_); + + auto task = std::make_unique(next_task_id_++, priority, start_point_id, end_point_id, description); + + if (!isValidTask(task.get())) { + return -1; + } + + task_queue_.push(task.get()); + return task.get()->id; +} + +bool AGVManager::assignTask(AGV* agv, Task* task) { + if (!canAssignTask(agv, task)) { + return false; + } + + if (!planPath(agv, task)) { + return false; + } + + agv->current_task = task; + agv->state = AGVState::ASSIGNED; + return true; +} + +int AGVManager::assignTasks() { + int assigned_count = 0; + + while (true) { + Task* task = nullptr; + AGV* selected_agv = nullptr; + double min_distance = std::numeric_limits::max(); + + // 获取下一个任务 + { + std::lock_guard lock(task_mutex_); + if (task_queue_.empty()) { + break; + } + task = task_queue_.top(); + task_queue_.pop(); + } + + // 选择最近的空闲AGV + auto idle_agvs = getIdleAGVs(); + for (AGV* agv : idle_agvs) { + double distance = calculateDistance(agv, task); + if (distance < min_distance) { + min_distance = distance; + selected_agv = agv; + } + } + + if (selected_agv) { + if (assignTask(selected_agv, task)) { + assigned_count++; + } else { + // 如果无法分配，重新加入队列 + std::lock_guard lock(task_mutex_); + task_queue_.push(task); + break; + } + } else { + // 没有空闲AGV，重新加入队列 + std::lock_guard lock(task_mutex_); + task_queue_.push(task); + break; + } + } + + return assigned_count; +} + +void AGVManager::completeTask(AGV* agv, bool success) { + if (!agv || !agv->current_task) { + return; + } + + Task* completed_task = agv->current_task; + int task_id = completed_task->id; + + if (task_complete_callback_) { + task_complete_callback_(*completed_task, agv->id); + } + + agv->current_task = nullptr; + agv->planned_path.clear(); + agv->path_index = 0; + agv->state = success ? AGVState::IDLE : AGVManager::ERROR; + + delete completed_task; +} + +bool AGVManager::planPath(AGV* agv, Task* task) { + auto start = graph_map_->getPoint(task->start_point_id); + auto end = graph_map_->getPoint(task->end_point_id); + + if (!start || !end) { + std::cerr << "[错误] 任务" << task->id << "的节点不存在" << std::endl; + return false; + } + + // 使用A*算法规划路径（简化版，避开其他AGV） + std::vector empty_avoid_list; + auto path_segments = graph_map_->findPath(start, end, empty_avoid_list); + + if (path_segments.empty()) { + std::cerr << "[错误] 无法为任务" << task->id << "规划路径" << std::endl; + return false; + } + + agv->planned_path = path_segments; + agv->path_index = 0; + return true; +} + +size_t AGVManager::getAGVCount() const { + std::lock_guard lock(agv_mutex_); + return agvs_.size(); +} + +size_t AGVManager::getTaskQueueSize() const { + std::lock_guard lock(task_mutex_); + return task_queue_.size(); +} + +std::vector AGVManager::getPendingTasks() const { + std::lock_guard lock(task_mutex_); + std::vector result; + + // 复制队列以便返回（注意：这里简化实现，实际中需要更复杂的处理） + std::priority_queue, std::function> temp_queue = task_queue_; + while (!temp_queue.empty()) { + result.push_back(temp_queue.top()); + temp_queue.pop(); + } + + return result; +} + +AGVManager::Statistics AGVManager::getStatistics() const { + Statistics stats; + + std::lock_guard agv_lock(agv_mutex_); + stats.total_agvs = agvs_.size(); + + double total_battery = 0; + for (const auto& pair : agvs_) { + const AGV* agv = pair.second.get(); + if (agv->state == AGVState::IDLE) { + stats.idle_agvs++; + } else { + stats.busy_agvs++; + } + total_battery += agv->battery_level; + } + + stats.average_battery_level = stats.total_agvs > 0 ? total_battery / stats.total_agvs : 0.0; + + std::lock_guard task_lock(task_mutex_); + stats.pending_tasks = task_queue_.size(); + + return stats; +} + +bool AGVManager::isValidTask(const Task* task) const { + if (!task) return false; + + auto start = graph_map_->getPoint(task->start_point_id); + auto end = graph_map_->getPoint(task->end_point_id); + + return start != nullptr && end != nullptr && start != end; +} + +bool AGVManager::canAssignTask(const AGV* agv, const Task* task) const { + return agv != nullptr && task != nullptr && + agv->state == AGVState::IDLE && + agv->battery_level > 20.0; // 电量至少20% +} + +double AGVManager::calculateDistance(AGV* agv, const Task* task) const { + if (!agv || !task || !agv->current_position) { + return std::numeric_limits::max(); + } + + auto start_point = graph_map_->getPoint(task->start_point_id); + if (!start_point) { + return std::numeric_limits::max(); + } + + return agv->current_position->distanceTo(*start_point); +} \ No newline at end of file