上传文件至 src/dispatch

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模块现在拥有更清晰、更高效的代码结构，为后续开发和维护奠定了良好基础。

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 <thread>
+#include <atomic>
+#include <condition_variable>
+#include <chrono>
+#include <functional>
+#include <map>
+
+/**
+ * @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<ExecutionState> state_;
+    std::atomic<bool> should_stop_;
+    std::atomic<bool> is_paused_;
+
+    // 路径信息
+    std::vector<Path*> 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<uint64_t> total_paths_executed_;
+    std::atomic<uint64_t> total_wait_time_;
+    std::atomic<uint64_t> total_execution_time_;
+
+    // 回调函数
+    std::function<void(int, const Path*)> path_started_callback_;
+    std::function<void(int, const Path*)> path_completed_callback_;
+    std::function<void(int, const std::string&)> 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*>& 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<size_t, size_t> getProgress() const;
+
+    /**
+     * @brief 获取统计信息
+     */
+    void getStatistics(uint64_t& total_paths, uint64_t& wait_time,
+                       uint64_t& execution_time) const;
+
+    /**
+     * @brief 设置路径开始回调
+     */
+    void setPathStartedCallback(std::function<void(int, const Path*)> callback) {
+        path_started_callback_ = callback;
+    }
+
+    /**
+     * @brief 设置路径完成回调
+     */
+    void setPathCompletedCallback(std::function<void(int, const Path*)> callback) {
+        path_completed_callback_ = callback;
+    }
+
+    /**
+     * @brief 设置错误回调
+     */
+    void setErrorCallback(std::function<void(int, const std::string&)> 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<int, std::unique_ptr<AGVExecutor>> 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<int, std::pair<size_t, size_t>>& progress) const;
+
+    /**
+     * @brief 打印所有执行器状态
+     */
+    void printStatus() const;
+};
+
+#endif // AGV_EXECUTOR_H

src/dispatch/agv_manager_base.cpp

@@ -0,0 +1,215 @@
+#include "agv_manager_base.h"
+#include <iostream>
+#include <algorithm>
+
+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<AGV*> AGVManagerBase::getAllAGVs() {
+    std::vector<AGV*> result;
+    for (const auto& pair : agvs_) {
+        result.push_back(pair.second);
+    }
+    return result;
+}
+
+std::vector<AGV*> AGVManagerBase::getIdleAGVs() {
+    std::vector<AGV*> 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<Task*> AGVManagerBase::getPendingTasks() {
+    std::vector<Task*> 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*>& path) {
+    agv_paths_[agv_id] = path;
+}
+
+std::vector<Path*> AGVManagerBase::getAGVPath(int agv_id) {
+    auto it = agv_paths_.find(agv_id);
+    return it != agv_paths_.end() ? it->second : std::vector<Path*>();
+}
+
+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*> 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<Path*> 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;
+}

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 <unordered_map>
+#include <vector>
+#include <memory>
+
+/**
+ * @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<int, AGV*> agvs_;
+    std::vector<std::unique_ptr<Task>> task_queue_;
+    std::unordered_map<int, std::vector<Path*>> 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<AGV*> getAllAGVs();
+
+    /**
+     * @brief 获取空闲AGV列表
+     */
+    std::vector<AGV*> 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<Task*> getPendingTasks();
+
+    /**
+     * @brief 为AGV设置路径
+     */
+    void setAGVPath(int agv_id, const std::vector<Path*>& path);
+
+    /**
+     * @brief 获取AGV的当前路径
+     */
+    std::vector<Path*> 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<Path*> findPath(AGV* agv, Task* task);
+
+    /**
+     * @brief 计算两点间距离
+     */
+    double calculateDistance(AGV* agv, Task* task);
+};
+
+#endif // AGV_MANAGER_BASE_H

src/dispatch/agv_manager_standalone.cpp

@@ -0,0 +1,257 @@
+#include "agv_manager_standalone.h"
+#include <iostream>
+#include <algorithm>
+
+AGVManager::AGVManager(GraphMap* map) : graph_map_(map) {
+    initializeTaskQueue();
+}
+
+void AGVManager::initializeTaskQueue() {
+    task_queue_ = std::priority_queue<Task*, std::vector<Task*>,
+        std::function<bool(Task*, Task*)>([](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<std::mutex> 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<AGV>(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<std::mutex> lock(agv_mutex_);
+    auto it = agvs_.find(id);
+    return (it != agvs_.end()) ? it->second.get() : nullptr;
+}
+
+std::vector<AGV*> AGVManager::getAllAGVs() {
+    std::lock_guard<std::mutex> lock(agv_mutex_);
+    std::vector<AGV*> result;
+    result.reserve(agvs_.size());
+
+    for (const auto& pair : agvs_) {
+        result.push_back(pair.second.get());
+    }
+
+    return result;
+}
+
+std::vector<AGV*> AGVManager::getIdleAGVs() {
+    std::lock_guard<std::mutex> lock(agv_mutex_);
+    std::vector<AGV*> 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<std::mutex> lock(task_mutex_);
+
+    auto task = std::make_unique<Task>(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<double>::max();
+
+        // 获取下一个任务
+        {
+            std::lock_guard<std::mutex> 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<std::mutex> lock(task_mutex_);
+                task_queue_.push(task);
+                break;
+            }
+        } else {
+            // 没有空闲AGV，重新加入队列
+            std::lock_guard<std::mutex> 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<AGV*> 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<std::mutex> lock(agv_mutex_);
+    return agvs_.size();
+}
+
+size_t AGVManager::getTaskQueueSize() const {
+    std::lock_guard<std::mutex> lock(task_mutex_);
+    return task_queue_.size();
+}
+
+std::vector<Task*> AGVManager::getPendingTasks() const {
+    std::lock_guard<std::mutex> lock(task_mutex_);
+    std::vector<Task*> result;
+
+    // 复制队列以便返回（注意：这里简化实现，实际中需要更复杂的处理）
+    std::priority_queue<Task*, std::vector<Task*>, std::function<bool(Task*, Task*)>> 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<std::mutex> 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<std::mutex> 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<double>::max();
+    }
+
+    auto start_point = graph_map_->getPoint(task->start_point_id);
+    if (!start_point) {
+        return std::numeric_limits<double>::max();
+    }
+
+    return agv->current_position->distanceTo(*start_point);
+}