package com.algo.service;
|
|
import com.algo.model.AGVStatus;
|
import com.algo.model.BackpackData;
|
import com.algo.model.ExecutingTask;
|
import com.algo.model.TaskData;
|
import com.algo.util.JsonUtils;
|
|
import java.util.*;
|
|
/**
|
* 执行中任务提取器
|
* 负责从AGV状态中提取当前正在执行的任务
|
*/
|
public class ExecutingTaskExtractor {
|
|
/**
|
* 路径映射表
|
*/
|
private Map<String, Map<String, Integer>> pathMapping;
|
|
/**
|
* 任务数据映射表
|
*/
|
private Map<String, TaskData> taskDataMap;
|
|
/**
|
* 坐标到路径编号的映射
|
*/
|
private Map<String, String> coordToCode;
|
|
/**
|
* 取货位置列表
|
*/
|
private List<String> pickupPositions;
|
|
/**
|
* 充电位置列表
|
*/
|
private List<String> chargingPositions;
|
|
/**
|
* 送货位置列表
|
*/
|
private List<String> deliveryPositions;
|
|
/**
|
* 待机位置列表
|
*/
|
private List<String> standbyPositions;
|
|
/**
|
* 构造函数
|
*
|
* @param pathMapping 路径映射表
|
* @param taskDataList 任务数据列表
|
*/
|
public ExecutingTaskExtractor(Map<String, Map<String, Integer>> pathMapping, List<TaskData> taskDataList) {
|
this.pathMapping = pathMapping;
|
this.coordToCode = new HashMap<>();
|
this.taskDataMap = new HashMap<>();
|
|
// 构建任务数据映射表
|
if (taskDataList != null) {
|
for (TaskData taskData : taskDataList) {
|
taskDataMap.put(taskData.getTaskId(), taskData);
|
}
|
}
|
|
// 预计算坐标映射
|
buildCoordToCodeMapping();
|
|
// 预分类位置类型
|
classifyPositions();
|
|
System.out.println("任务提取器初始化完成，分类位置：取货=" + pickupPositions.size() +
|
", 充电=" + chargingPositions.size() +
|
", 送货=" + deliveryPositions.size() +
|
", 待机=" + standbyPositions.size() +
|
", 任务数据=" + taskDataMap.size());
|
}
|
|
/**
|
* 提取执行中的任务
|
*
|
* @param agvStatusList AGV状态列表
|
* @return 执行中任务列表
|
*/
|
public List<ExecutingTask> extractExecutingTasks(List<AGVStatus> agvStatusList) {
|
List<ExecutingTask> executingTasks = new ArrayList<>();
|
|
for (AGVStatus agvStatus : agvStatusList) {
|
List<ExecutingTask> agvTasks = extractAgvTasks(agvStatus);
|
executingTasks.addAll(agvTasks);
|
}
|
|
System.out.println("提取到 " + executingTasks.size() + " 个执行中任务");
|
return executingTasks;
|
}
|
|
/**
|
* 提取单个AGV的执行中任务
|
*
|
* @param agvStatus AGV状态
|
* @return 执行中任务列表
|
*/
|
private List<ExecutingTask> extractAgvTasks(AGVStatus agvStatus) {
|
List<ExecutingTask> tasks = new ArrayList<>();
|
|
System.out.println(" [调试] 正在提取 AGV " + agvStatus.getAgvId() + " 的任务");
|
System.out.println(" [调试] isAvailable: " + agvStatus.isAvailable());
|
|
// 分析背篓状态
|
List<BackpackData> backpack = agvStatus.getBackpack();
|
if (backpack == null || backpack.isEmpty()) {
|
System.out.println(" [调试] AGV " + agvStatus.getAgvId() + " 背篓为空，跳过");
|
return tasks;
|
}
|
|
// 检查是否有已装载的任务（即使AGV不可用，如果有已装载任务也需要规划路径）
|
boolean hasLoadedTasks = false;
|
for (BackpackData bp : backpack) {
|
if (bp.isLoaded() && bp.getTaskId() != null && !bp.getTaskId().trim().isEmpty()) {
|
hasLoadedTasks = true;
|
break;
|
}
|
}
|
|
// 如果AGV不可用且没有已装载任务，跳过
|
if (!agvStatus.isAvailable() && !hasLoadedTasks) {
|
System.out.println(" [调试] AGV " + agvStatus.getAgvId() + " 不可用且无已装载任务，跳过");
|
return tasks;
|
}
|
|
// 如果有已装载任务但AGV不可用，记录日志但继续处理
|
if (!agvStatus.isAvailable() && hasLoadedTasks) {
|
System.out.println(" [调试] AGV " + agvStatus.getAgvId() + " 不可用但有已装载任务，继续提取任务");
|
}
|
|
System.out.println(" [调试] AGV " + agvStatus.getAgvId() + " 背篓数量: " + backpack.size());
|
|
// 分别收集已装载和未装载的任务
|
List<BackpackData> loadedTasks = new ArrayList<>();
|
List<BackpackData> unloadedTasks = new ArrayList<>();
|
|
for (BackpackData bp : backpack) {
|
if (bp.getTaskId() != null && !bp.getTaskId().trim().isEmpty()) {
|
System.out.println(" [调试] 发现背篓任务: taskId=" + bp.getTaskId() +
|
", loaded=" + bp.isLoaded() + ", index=" + bp.getIndex());
|
|
TaskData taskData = getTaskData(bp.getTaskId());
|
if (taskData != null) {
|
System.out.println(" [调试] 成功获取TaskData: " + taskData);
|
if (bp.isLoaded()) {
|
loadedTasks.add(bp);
|
} else {
|
unloadedTasks.add(bp);
|
}
|
} else {
|
System.out.println(" [调试] 警告：taskId=" + bp.getTaskId() +
|
" 在taskDataMap中未找到！");
|
System.out.println(" [调试] taskDataMap包含的key: " + taskDataMap.keySet());
|
}
|
}
|
}
|
|
System.out.println(" [调试] AGV " + agvStatus.getAgvId() +
|
" - 已装载任务: " + loadedTasks.size() +
|
", 未装载任务: " + unloadedTasks.size());
|
|
// 确定下一步最优行动
|
ExecutingTask nextTask = determineNextBestAction(agvStatus, loadedTasks, unloadedTasks);
|
if (nextTask != null) {
|
System.out.println(" [调试] AGV " + agvStatus.getAgvId() +
|
" 生成任务: " + nextTask.getTaskType() +
|
" " + nextTask.getCurrentPosition() + " -> " + nextTask.getTargetPosition());
|
tasks.add(nextTask);
|
} else {
|
System.out.println(" [调试] AGV " + agvStatus.getAgvId() +
|
" determineNextBestAction返回null");
|
}
|
|
return tasks;
|
}
|
|
/**
|
* 确定AGV下一步最优行动
|
* 使用加权成本算法综合考虑距离和任务类型
|
*
|
* @param agvStatus AGV状态
|
* @param loadedTasks 已装载任务
|
* @param unloadedTasks 未装载任务
|
* @return 下一步执行任务
|
*/
|
private ExecutingTask determineNextBestAction(AGVStatus agvStatus,
|
List<BackpackData> loadedTasks,
|
List<BackpackData> unloadedTasks) {
|
String currentPosition = agvStatus.getPosition();
|
System.out.println(" [调试] determineNextBestAction - AGV " + agvStatus.getAgvId() +
|
" 当前位置: " + currentPosition);
|
|
List<TaskOption> taskOptions = new ArrayList<>();
|
|
// 1. 收集所有可选的送货任务
|
System.out.println(" [调试] 处理已装载任务数量: " + loadedTasks.size());
|
for (BackpackData task : loadedTasks) {
|
TaskData taskData = getTaskData(task.getTaskId());
|
if (taskData != null && taskData.getEnd() != null) {
|
System.out.println(" [调试] 计算送货距离: " + currentPosition + " -> " + taskData.getEnd());
|
double distance = calculateDistance(currentPosition, taskData.getEnd());
|
System.out.println(" [调试] 距离计算结果: " + distance);
|
|
if (distance >= 0) {
|
double cost = calculateTaskCost(distance, "delivery", taskData.getPriority(), agvStatus);
|
taskOptions.add(new TaskOption(task, true, distance, cost, "delivery"));
|
System.out.println(" [调试] 添加送货选项，成本: " + cost);
|
} else {
|
System.out.println(" [调试] 警告：距离为负，跳过此送货任务");
|
}
|
} else {
|
System.out.println(" [调试] 警告：TaskData或end为null，跳过");
|
}
|
}
|
|
// 2. 收集所有可选的取货任务
|
System.out.println(" [调试] 处理未装载任务数量: " + unloadedTasks.size());
|
for (BackpackData task : unloadedTasks) {
|
TaskData taskData = getTaskData(task.getTaskId());
|
if (taskData != null && taskData.getStart() != null) {
|
System.out.println(" [调试] 计算取货距离: " + currentPosition + " -> " + taskData.getStart());
|
double distance = calculateDistance(currentPosition, taskData.getStart());
|
System.out.println(" [调试] 距离计算结果: " + distance);
|
|
if (distance >= 0) {
|
double cost = calculateTaskCost(distance, "pickup", taskData.getPriority(), agvStatus);
|
taskOptions.add(new TaskOption(task, false, distance, cost, "pickup"));
|
System.out.println(" [调试] 添加取货选项，成本: " + cost);
|
} else {
|
System.out.println(" [调试] 警告：距离为负，跳过此取货任务");
|
}
|
}
|
}
|
|
// 3. 如果电量不足，添加充电选项
|
if (agvStatus.needsCharging()) {
|
String nearestCharging = findNearestChargingPosition(currentPosition);
|
if (nearestCharging != null && !nearestCharging.equals(currentPosition)) {
|
double distance = calculateDistance(currentPosition, nearestCharging);
|
if (distance >= 0) {
|
double cost = calculateChargingCost(distance, agvStatus.getVol(), agvStatus.getLowVol());
|
taskOptions.add(new TaskOption(null, false, distance, cost, "charging"));
|
}
|
}
|
}
|
|
// 4. 选择成本最低的任务
|
System.out.println(" [调试] 总任务选项数量: " + taskOptions.size());
|
if (taskOptions.isEmpty()) {
|
System.out.println(" [调试] 警告：没有可选任务，返回null");
|
return null;
|
}
|
|
// 按成本排序，选择最优选项
|
taskOptions.sort(Comparator.comparingDouble(option -> option.cost));
|
TaskOption bestOption = taskOptions.get(0);
|
|
// 输出决策日志
|
System.out.println("AGV " + agvStatus.getAgvId() + " 任务决策：");
|
for (int i = 0; i < Math.min(3, taskOptions.size()); i++) {
|
TaskOption option = taskOptions.get(i);
|
System.out.println(" " + (i == 0 ? "[选中]" : " ") +
|
" 类型: " + option.taskType +
|
", 距离: " + String.format("%.1f", option.distance) +
|
", 成本: " + String.format("%.2f", option.cost));
|
}
|
|
// 创建执行任务
|
if ("charging".equals(bestOption.taskType)) {
|
String nearestCharging = findNearestChargingPosition(currentPosition);
|
return createChargingTask(agvStatus, nearestCharging);
|
} else {
|
return createExecutingTask(agvStatus, bestOption.backpackData, bestOption.isLoaded);
|
}
|
}
|
|
/**
|
* 计算任务成本
|
*
|
* @param distance 距离
|
* @param taskType 任务类型
|
* @param taskPriority 任务优先级
|
* @param agvStatus AGV状态
|
* @return 任务成本
|
*/
|
private double calculateTaskCost(double distance, String taskType, int taskPriority, AGVStatus agvStatus) {
|
// 基础距离成本
|
double distanceCost = distance;
|
|
// 任务类型权重
|
double taskTypeWeight;
|
switch (taskType) {
|
case "delivery":
|
taskTypeWeight = 10.0; // 送货任务基础权重较低，优先执行
|
break;
|
case "pickup":
|
taskTypeWeight = 15.0; // 取货任务基础权重适中
|
break;
|
default:
|
taskTypeWeight = 20.0;
|
}
|
|
// 任务优先级影响（优先级越高，权重越低）
|
double priorityWeight = Math.max(1.0, 6.0 - taskPriority);
|
|
// 电量影响（电量低时偏向近距离任务）
|
double batteryFactor = 1.0;
|
if (agvStatus.needsCharging()) {
|
batteryFactor = 1.0 + (100.0 - agvStatus.getVol()) / 100.0; // 电量越低，距离成本越高
|
}
|
|
// 综合成本计算
|
double totalCost = (distanceCost * batteryFactor) + (taskTypeWeight * priorityWeight);
|
|
return totalCost;
|
}
|
|
/**
|
* 计算充电任务成本
|
*
|
* @param distance 到充电点的距离
|
* @param currentVol 当前电量
|
* @param lowVol 最低电量阈值
|
* @return 充电任务成本
|
*/
|
private double calculateChargingCost(double distance, int currentVol, int lowVol) {
|
// 基础距离成本
|
double distanceCost = distance;
|
|
// 电量紧急程度（电量越低，充电成本越低，即优先级越高）
|
double batteryUrgency;
|
if (currentVol <= lowVol) {
|
batteryUrgency = 5.0; // 必须充电，极高优先级
|
} else {
|
// 电量在lowVol到autoCharge之间，线性计算紧急程度
|
batteryUrgency = 30.0 - (currentVol - lowVol) * 0.5; // 电量越低优先级越高
|
}
|
|
return distanceCost + batteryUrgency;
|
}
|
|
/**
|
* 任务选项内部类
|
*/
|
private static class TaskOption {
|
BackpackData backpackData;
|
boolean isLoaded;
|
double distance;
|
double cost;
|
String taskType;
|
|
public TaskOption(BackpackData backpackData, boolean isLoaded, double distance, double cost, String taskType) {
|
this.backpackData = backpackData;
|
this.isLoaded = isLoaded;
|
this.distance = distance;
|
this.cost = cost;
|
this.taskType = taskType;
|
}
|
}
|
|
/**
|
* 获取任务数据
|
*
|
* @param taskId 任务ID
|
* @return 任务数据
|
*/
|
private TaskData getTaskData(String taskId) {
|
return taskDataMap.get(taskId);
|
}
|
|
|
/**
|
* 查找最近的充电位置
|
*
|
* @param currentPosition 当前位置
|
* @return 最近的充电位置
|
*/
|
private String findNearestChargingPosition(String currentPosition) {
|
String nearest = null;
|
double minDistance = Double.MAX_VALUE;
|
|
for (String chargingPos : chargingPositions) {
|
if (!chargingPos.equals(currentPosition)) {
|
double distance = calculateDistance(currentPosition, chargingPos);
|
if (distance >= 0 && distance < minDistance) {
|
minDistance = distance;
|
nearest = chargingPos;
|
}
|
}
|
}
|
|
return nearest;
|
}
|
|
/**
|
* 创建执行任务
|
*
|
* @param agvStatus AGV状态
|
* @param backpackData 背篓数据
|
* @param isLoaded 是否已装载
|
* @return 执行任务
|
*/
|
private ExecutingTask createExecutingTask(AGVStatus agvStatus, BackpackData backpackData, boolean isLoaded) {
|
TaskData taskData = getTaskData(backpackData.getTaskId());
|
if (taskData == null) {
|
return null;
|
}
|
|
ExecutingTask task = new ExecutingTask();
|
task.setAgvId(agvStatus.getAgvId());
|
task.setTaskId(backpackData.getTaskId());
|
task.setCurrentPosition(agvStatus.getPosition());
|
task.setLoaded(isLoaded);
|
task.setBackpackIndex(backpackData.getIndex());
|
task.setPriority(taskData.getPriority());
|
|
// 设置目标位置
|
if (isLoaded) {
|
task.setTargetPosition(taskData.getEnd());
|
task.setTaskType("delivery");
|
} else {
|
task.setTargetPosition(taskData.getStart());
|
task.setTaskType("pickup");
|
}
|
|
// 计算距离
|
task.setDistanceToStart(calculateDistance(agvStatus.getPosition(), taskData.getStart()));
|
task.setDistanceToEnd(calculateDistance(agvStatus.getPosition(), taskData.getEnd()));
|
|
return task;
|
}
|
|
/**
|
* 创建充电任务
|
*
|
* @param agvStatus AGV状态
|
* @param chargingPosition 充电位置
|
* @return 充电任务
|
*/
|
private ExecutingTask createChargingTask(AGVStatus agvStatus, String chargingPosition) {
|
ExecutingTask task = new ExecutingTask();
|
task.setAgvId(agvStatus.getAgvId());
|
task.setTaskId("CHARGING_" + agvStatus.getAgvId());
|
task.setCurrentPosition(agvStatus.getPosition());
|
task.setTargetPosition(chargingPosition);
|
task.setTaskType("charging");
|
task.setLoaded(false);
|
task.setBackpackIndex(-1);
|
task.setPriority(1); // 充电任务优先级较高
|
|
return task;
|
}
|
|
/**
|
* 计算两个位置之间的距离
|
*
|
* @param pos1 位置1
|
* @param pos2 位置2
|
* @return 距离值
|
*/
|
private double calculateDistance(String pos1, String pos2) {
|
if (pos1 == null || pos2 == null) {
|
System.out.println(" [调试] 警告：位置为null: pos1=" + pos1 + ", pos2=" + pos2);
|
return -1;
|
}
|
|
// 如果位置相同，返回0
|
if (pos1.equals(pos2)) {
|
System.out.println(" [调试] 位置相同，返回距离0");
|
return 0;
|
}
|
|
int[] coord1 = JsonUtils.getCoordinate(pos1, pathMapping);
|
int[] coord2 = JsonUtils.getCoordinate(pos2, pathMapping);
|
|
if (coord1 == null || coord2 == null) {
|
System.out.println(" [调试] 警告：坐标查找失败!");
|
System.out.println(" pos1=" + pos1 + " -> coord1=" +
|
(coord1 != null ? "[" + coord1[0] + "," + coord1[1] + "]" : "null"));
|
System.out.println(" pos2=" + pos2 + " -> coord2=" +
|
(coord2 != null ? "[" + coord2[0] + "," + coord2[1] + "]" : "null"));
|
System.out.println(" pathMapping包含pos1? " + pathMapping.containsKey(pos1));
|
System.out.println(" pathMapping包含pos2? " + pathMapping.containsKey(pos2));
|
return -1;
|
}
|
|
double distance = JsonUtils.calculateManhattanDistance(coord1, coord2);
|
System.out.println(" [调试] 曼哈顿距离: " + distance + " (" + pos1 + " -> " + pos2 + ")");
|
return distance;
|
}
|
|
/**
|
* 构建坐标到路径编号的映射
|
*/
|
private void buildCoordToCodeMapping() {
|
for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
|
String code = entry.getKey();
|
Map<String, Integer> coordMap = entry.getValue();
|
|
if (coordMap.containsKey("x") && coordMap.containsKey("y")) {
|
String coordKey = coordMap.get("x") + "," + coordMap.get("y");
|
coordToCode.put(coordKey, code);
|
}
|
}
|
}
|
|
/**
|
* 预分类位置类型
|
*/
|
private void classifyPositions() {
|
pickupPositions = new ArrayList<>();
|
chargingPositions = new ArrayList<>();
|
deliveryPositions = new ArrayList<>();
|
standbyPositions = new ArrayList<>();
|
|
for (String position : pathMapping.keySet()) {
|
if (position.startsWith("1")) {
|
pickupPositions.add(position);
|
} else if (position.startsWith("2")) {
|
chargingPositions.add(position);
|
} else if (position.startsWith("3")) {
|
deliveryPositions.add(position);
|
} else if (position.startsWith("4")) {
|
standbyPositions.add(position);
|
}
|
}
|
}
|
|
// Getter方法
|
public List<String> getPickupPositions() {
|
return pickupPositions;
|
}
|
|
public List<String> getChargingPositions() {
|
return chargingPositions;
|
}
|
|
public List<String> getDeliveryPositions() {
|
return deliveryPositions;
|
}
|
|
public List<String> getStandbyPositions() {
|
return standbyPositions;
|
}
|
}
|
