package com.algo.service;
|
|
import com.algo.model.AGVStatus;
|
import com.algo.model.CTUPhysicalConfig;
|
import com.algo.model.PathCode;
|
import com.algo.model.PlannedPath;
|
import com.algo.util.JsonUtils;
|
|
import java.util.ArrayList;
|
import java.util.HashMap;
|
import java.util.List;
|
import java.util.Map;
|
|
/**
|
* 剩余路径处理器
|
* 负责处理CTU未完成路径的续接逻辑，确保新路径与剩余路径的平滑连接
|
*/
|
public class RemainingPathProcessor {
|
|
/**
|
* 路径映射表
|
*/
|
private final Map<String, Map<String, Integer>> pathMapping;
|
|
/**
|
* 系统当前时间基准（毫秒）
|
*/
|
private long systemBaseTime;
|
|
public RemainingPathProcessor(Map<String, Map<String, Integer>> pathMapping) {
|
this.pathMapping = pathMapping;
|
this.systemBaseTime = System.currentTimeMillis();
|
}
|
|
/**
|
* 处理所有CTU的剩余路径，构建时空占用表
|
*
|
* @param agvStatusList CTU状态列表
|
* @return 时空占用表，key为"x,y,timeSlot"，value为CTU编号
|
*/
|
public Map<String, String> buildSpaceTimeOccupancyMap(List<AGVStatus> agvStatusList) {
|
Map<String, String> occupancyMap = new HashMap<>();
|
|
for (AGVStatus agv : agvStatusList) {
|
if (agv.hasRemainingPath()) {
|
processRemainingPathOccupancy(agv, occupancyMap);
|
}
|
}
|
|
return occupancyMap;
|
}
|
|
/**
|
* 处理单个CTU的剩余路径时空占用
|
*
|
* @param agv CTU状态
|
* @param occupancyMap 时空占用表
|
*/
|
private void processRemainingPathOccupancy(AGVStatus agv, Map<String, String> occupancyMap) {
|
PlannedPath remainingPath = agv.getRemainingPath();
|
List<PathCode> codeList = remainingPath.getCodeList();
|
CTUPhysicalConfig config = agv.getPhysicalConfig();
|
|
// 从当前位置开始计算时空占用
|
long currentTime = agv.getNextPointArrivalTime();
|
int startIndex = agv.getCurrentPathIndex();
|
|
for (int i = startIndex; i < codeList.size(); i++) {
|
PathCode pathCode = codeList.get(i);
|
String position = pathCode.getCode();
|
|
// 获取位置坐标
|
int[] coord = JsonUtils.getCoordinate(position, pathMapping);
|
if (coord == null) continue;
|
|
// 计算在此位置的停留时间
|
double stayDuration = calculateStayDuration(agv, i, codeList, config);
|
|
// 将停留时间转换为时间片
|
long startTimeSlot = currentTime / 1000; // 转换为秒
|
long endTimeSlot = (long) (currentTime / 1000 + stayDuration);
|
|
// 占用时空
|
for (long timeSlot = startTimeSlot; timeSlot <= endTimeSlot; timeSlot++) {
|
String spaceTimeKey = coord[0] + "," + coord[1] + "," + timeSlot;
|
occupancyMap.put(spaceTimeKey, agv.getAgvId());
|
|
// 考虑CTU的物理尺寸，占用周围的格子
|
occupyAdjacentSpaces(coord, timeSlot, agv.getAgvId(), occupancyMap, config);
|
}
|
|
// 更新到下一个位置的时间
|
currentTime += (long) (stayDuration * 1000);
|
if (i < codeList.size() - 1) {
|
PathCode nextPathCode = codeList.get(i + 1);
|
double travelTime = calculateTravelTime(pathCode, nextPathCode, config);
|
currentTime += (long) (travelTime * 1000);
|
}
|
}
|
}
|
|
/**
|
* 占用相邻空间（考虑CTU物理尺寸）
|
*
|
* @param centerCoord 中心坐标
|
* @param timeSlot 时间片
|
* @param agvId CTU编号
|
* @param occupancyMap 占用表
|
* @param config 物理配置
|
*/
|
private void occupyAdjacentSpaces(int[] centerCoord, long timeSlot, String agvId,
|
Map<String, String> occupancyMap, CTUPhysicalConfig config) {
|
// 根据CTU的长度和宽度计算需要占用的格子数
|
int lengthGrids = (int) Math.ceil(config.getCtuLength() / config.getStandardPointDistance());
|
int widthGrids = (int) Math.ceil(config.getCtuWidth() / config.getStandardPointDistance());
|
|
// 占用周围的格子
|
for (int dx = -lengthGrids / 2; dx <= lengthGrids / 2; dx++) {
|
for (int dy = -widthGrids / 2; dy <= widthGrids / 2; dy++) {
|
if (dx == 0 && dy == 0) continue; // 中心点已经占用
|
|
int adjX = centerCoord[0] + dx;
|
int adjY = centerCoord[1] + dy;
|
String spaceTimeKey = adjX + "," + adjY + "," + timeSlot;
|
|
// 只有当该位置未被占用时才占用
|
if (!occupancyMap.containsKey(spaceTimeKey)) {
|
occupancyMap.put(spaceTimeKey, agvId);
|
}
|
}
|
}
|
}
|
|
/**
|
* 计算CTU在指定路径点的停留时间
|
*
|
* @param agv CTU状态
|
* @param pathIndex 路径点索引
|
* @param codeList 完整路径代码列表
|
* @param config 物理配置
|
* @return 停留时间（秒）
|
*/
|
private double calculateStayDuration(AGVStatus agv, int pathIndex,
|
List<PathCode> codeList, CTUPhysicalConfig config) {
|
PathCode currentCode = codeList.get(pathIndex);
|
|
// 基础停留时间
|
double stayTime = 0.5; // 默认0.5秒
|
|
// 如果需要转向，增加转向时间
|
if (pathIndex < codeList.size() - 1) {
|
PathCode nextCode = codeList.get(pathIndex + 1);
|
String currentDirection = currentCode.getDirection();
|
String nextDirection = nextCode.getDirection();
|
|
if (!currentDirection.equals(nextDirection)) {
|
stayTime += config.getTurnTime(currentDirection, nextDirection);
|
}
|
}
|
|
// 如果是动作点（取货、放货等），增加操作时间
|
if (currentCode.getActionType() != null) {
|
switch (currentCode.getActionType()) {
|
case "1": // 取货
|
stayTime += 3.0; // 取货需要3秒
|
break;
|
case "2": // 放货
|
stayTime += 2.0; // 放货需要2秒
|
break;
|
case "3": // 充电
|
stayTime += 10.0; // 充电停靠需要10秒
|
break;
|
default:
|
stayTime += 1.0; // 其他动作需要1秒
|
break;
|
}
|
}
|
|
return stayTime;
|
}
|
|
/**
|
* 计算两个路径点之间的移动时间
|
*
|
* @param fromCode 起始路径点
|
* @param toCode 目标路径点
|
* @param config 物理配置
|
* @return 移动时间（秒）
|
*/
|
private double calculateTravelTime(PathCode fromCode, PathCode toCode, CTUPhysicalConfig config) {
|
// 获取坐标
|
int[] fromCoord = JsonUtils.getCoordinate(fromCode.getCode(), pathMapping);
|
int[] toCoord = JsonUtils.getCoordinate(toCode.getCode(), pathMapping);
|
|
if (fromCoord == null || toCoord == null) {
|
return config.getStandardPointDistance() / config.getNormalSpeed();
|
}
|
|
// 计算距离
|
double distance = Math.sqrt(Math.pow(toCoord[0] - fromCoord[0], 2) +
|
Math.pow(toCoord[1] - fromCoord[1], 2)) *
|
config.getStandardPointDistance();
|
|
// 计算移动时间
|
return config.getTravelTime(distance, config.getNormalSpeed());
|
}
|
|
/**
|
* 为新路径查找合适的起始时间，避免与现有路径冲突
|
*
|
* @param newPath 新路径
|
* @param occupancyMap 现有时空占用表
|
* @param config 物理配置
|
* @return 建议的起始时间（毫秒）
|
*/
|
public long findSafeStartTime(PlannedPath newPath, Map<String, String> occupancyMap,
|
CTUPhysicalConfig config) {
|
if (newPath.getCodeList() == null || newPath.getCodeList().isEmpty()) {
|
return systemBaseTime;
|
}
|
|
// 从当前时间开始，逐步向后搜索安全的起始时间
|
long currentTime = systemBaseTime;
|
long timeStep = 1000; // 1秒步长
|
long maxSearchTime = currentTime + 300000; // 最多搜索5分钟
|
|
while (currentTime < maxSearchTime) {
|
if (isPathTimeSafe(newPath, currentTime, occupancyMap, config)) {
|
return currentTime;
|
}
|
currentTime += timeStep;
|
}
|
|
// 如果找不到安全时间，返回较远的未来时间
|
return maxSearchTime;
|
}
|
|
/**
|
* 检查指定时间开始的路径是否安全
|
*
|
* @param path 路径
|
* @param startTime 起始时间
|
* @param occupancyMap 占用表
|
* @param config 物理配置
|
* @return true如果安全
|
*/
|
private boolean isPathTimeSafe(PlannedPath path, long startTime,
|
Map<String, String> occupancyMap, CTUPhysicalConfig config) {
|
List<PathCode> codeList = path.getCodeList();
|
long currentTime = startTime;
|
|
for (int i = 0; i < codeList.size(); i++) {
|
PathCode pathCode = codeList.get(i);
|
int[] coord = JsonUtils.getCoordinate(pathCode.getCode(), pathMapping);
|
if (coord == null) continue;
|
|
// 计算在此位置的停留时间
|
double stayDuration = 2.0; // 简化计算，使用固定停留时间
|
long startTimeSlot = currentTime / 1000;
|
long endTimeSlot = (long) (currentTime / 1000 + stayDuration);
|
|
// 检查时空冲突
|
for (long timeSlot = startTimeSlot; timeSlot <= endTimeSlot; timeSlot++) {
|
String spaceTimeKey = coord[0] + "," + coord[1] + "," + timeSlot;
|
if (occupancyMap.containsKey(spaceTimeKey)) {
|
return false; // 发现冲突
|
}
|
|
// 检查安全距离内的冲突
|
if (!isSafeFromOtherCTUs(coord, timeSlot, occupancyMap, config)) {
|
return false;
|
}
|
}
|
|
// 更新到下一个位置的时间
|
currentTime += (long) (stayDuration * 1000);
|
if (i < codeList.size() - 1) {
|
currentTime += (long) (config.getStandardPointDistance() / config.getNormalSpeed() * 1000);
|
}
|
}
|
|
return true;
|
}
|
|
/**
|
* 检查指定位置和时间是否与其他CTU保持安全距离
|
*
|
* @param coord 位置坐标
|
* @param timeSlot 时间片
|
* @param occupancyMap 占用表
|
* @param config 物理配置
|
* @return true如果安全
|
*/
|
private boolean isSafeFromOtherCTUs(int[] coord, long timeSlot,
|
Map<String, String> occupancyMap, CTUPhysicalConfig config) {
|
int safetyRadius = (int) Math.ceil(config.getMinSafetyDistance() / config.getStandardPointDistance());
|
|
// 检查安全半径内是否有其他CTU
|
for (int dx = -safetyRadius; dx <= safetyRadius; dx++) {
|
for (int dy = -safetyRadius; dy <= safetyRadius; dy++) {
|
if (dx == 0 && dy == 0) continue;
|
|
String checkKey = (coord[0] + dx) + "," + (coord[1] + dy) + "," + timeSlot;
|
if (occupancyMap.containsKey(checkKey)) {
|
// 计算实际距离
|
double distance = Math.sqrt(dx * dx + dy * dy) * config.getStandardPointDistance();
|
if (distance < config.getMinSafetyDistance()) {
|
return false; // 距离太近
|
}
|
}
|
}
|
}
|
|
return true;
|
}
|
|
/**
|
* 连接剩余路径和新路径
|
*
|
* @param agv CTU状态
|
* @param newPath 新规划的路径
|
* @return 连接后的完整路径
|
*/
|
public PlannedPath connectRemainingAndNewPath(AGVStatus agv, PlannedPath newPath) {
|
if (!agv.hasRemainingPath()) {
|
// 没有剩余路径，直接返回新路径
|
return newPath;
|
}
|
|
PlannedPath remainingPath = agv.getRemainingPath();
|
List<PathCode> remainingCodes = new ArrayList<>();
|
|
// 添加剩余路径（从当前位置开始）
|
List<PathCode> originalCodes = remainingPath.getCodeList();
|
for (int i = agv.getCurrentPathIndex(); i < originalCodes.size(); i++) {
|
remainingCodes.add(originalCodes.get(i));
|
}
|
|
// 连接新路径
|
if (newPath != null && newPath.getCodeList() != null) {
|
// 检查连接点是否一致，避免重复
|
if (!remainingCodes.isEmpty() && !newPath.getCodeList().isEmpty()) {
|
PathCode lastRemaining = remainingCodes.get(remainingCodes.size() - 1);
|
PathCode firstNew = newPath.getCodeList().get(0);
|
|
if (lastRemaining.getCode().equals(firstNew.getCode())) {
|
// 跳过重复的连接点
|
for (int i = 1; i < newPath.getCodeList().size(); i++) {
|
remainingCodes.add(newPath.getCodeList().get(i));
|
}
|
} else {
|
// 直接连接
|
remainingCodes.addAll(newPath.getCodeList());
|
}
|
} else {
|
remainingCodes.addAll(newPath.getCodeList());
|
}
|
}
|
|
// 创建连接后的路径
|
PlannedPath connectedPath = new PlannedPath();
|
connectedPath.setAgvId(agv.getAgvId());
|
connectedPath.setCodeList(remainingCodes);
|
|
// 生成新的段落ID
|
String segId = agv.getAgvId() + "_CONNECTED_" + System.currentTimeMillis();
|
if (newPath != null && newPath.getSegId() != null) {
|
segId = newPath.getSegId() + "_EXTENDED";
|
}
|
connectedPath.setSegId(segId);
|
|
return connectedPath;
|
}
|
|
/**
|
* 更新系统基准时间
|
*
|
* @param baseTime 新的基准时间
|
*/
|
public void updateSystemBaseTime(long baseTime) {
|
this.systemBaseTime = baseTime;
|
}
|
|
/**
|
* 获取CTU预计完成剩余路径的时间
|
*
|
* @param agv CTU状态
|
* @return 预计完成时间（毫秒）
|
*/
|
public long getEstimatedCompletionTime(AGVStatus agv) {
|
if (!agv.hasRemainingPath()) {
|
return System.currentTimeMillis();
|
}
|
|
PlannedPath remainingPath = agv.getRemainingPath();
|
List<PathCode> codeList = remainingPath.getCodeList();
|
CTUPhysicalConfig config = agv.getPhysicalConfig();
|
|
long currentTime = agv.getNextPointArrivalTime();
|
|
// 计算完成剩余路径所需的时间
|
for (int i = agv.getCurrentPathIndex(); i < codeList.size(); i++) {
|
double stayDuration = calculateStayDuration(agv, i, codeList, config);
|
currentTime += (long) (stayDuration * 1000);
|
|
if (i < codeList.size() - 1) {
|
PathCode currentCode = codeList.get(i);
|
PathCode nextCode = codeList.get(i + 1);
|
double travelTime = calculateTravelTime(currentCode, nextCode, config);
|
currentTime += (long) (travelTime * 1000);
|
}
|
}
|
|
return currentTime;
|
}
|
}
|
