270 lines
6.6 KiB
C++
270 lines
6.6 KiB
C++
#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
|