From 7dd762215b373851ed313b46bad08cc973816665 Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期三, 12 十一月 2025 12:26:23 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/CollisionDetector.java | 193 ++++++++---
algo-zkd/src/main/java/com/algo/service/CollisionResolver.java | 664 ++++++++++++++++++++++++++++++++++++++
algo-zkd/src/main/java/com/algo/service/PathPlanningService.java | 34 -
algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java | 16
algo-zkd/src/main/java/com/algo/service/ExecutingTaskExtractor.java | 88 ++++
5 files changed, 899 insertions(+), 96 deletions(-)
diff --git a/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java b/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
index 26c28ef..893f979 100644
--- a/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
+++ b/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
@@ -268,7 +268,7 @@
gScores.put(startKey, 0.0);
// 鏋勫缓绾︽潫妫�鏌ュ櫒
- EnhancedConstraintChecker constraintChecker = new EnhancedConstraintChecker(
+ ConstraintChecker constraintChecker = new ConstraintChecker(
constraints, occupancyMap, pathMapping, physicalConfig
);
@@ -315,7 +315,7 @@
PriorityQueue<SpaceTimeAStarNode> openSet,
Set<String> closedSet, Map<String, Double> gScores,
Map<String, SpaceTimeAStarNode> cameFrom,
- EnhancedConstraintChecker constraintChecker,
+ ConstraintChecker constraintChecker,
CTUPhysicalConfig physicalConfig) {
// 鑾峰彇绌洪棿閭诲眳
@@ -399,7 +399,7 @@
* 妫�鏌ュ綋鍓嶈妭鐐圭殑閭诲眳鏄惁鍦ㄧ煭鏈熷唴閮借闃绘尅
*/
private boolean checkIfAllNeighborsBlocked(SpaceTimeAStarNode current,
- EnhancedConstraintChecker constraintChecker,
+ ConstraintChecker constraintChecker,
CTUPhysicalConfig physicalConfig) {
// 鑾峰彇褰撳墠鑺傜偣鐨勬墍鏈夌┖闂撮偦灞�
List<Map<String, String>> neighbors = getNeighbors(current.code);
@@ -872,16 +872,16 @@
/**
* 绾︽潫妫�鏌ュ櫒
*/
- private static class EnhancedConstraintChecker {
+ private static class ConstraintChecker {
private final List<double[]> staticConstraints;
private final Map<String, String> spaceTimeOccupancyMap;
private final Map<String, Map<String, Integer>> pathMapping;
private final CTUPhysicalConfig physicalConfig;
- public EnhancedConstraintChecker(List<double[]> staticConstraints,
- Map<String, String> spaceTimeOccupancyMap,
- Map<String, Map<String, Integer>> pathMapping,
- CTUPhysicalConfig physicalConfig) {
+ public ConstraintChecker(List<double[]> staticConstraints,
+ Map<String, String> spaceTimeOccupancyMap,
+ Map<String, Map<String, Integer>> pathMapping,
+ CTUPhysicalConfig physicalConfig) {
this.staticConstraints = staticConstraints;
this.spaceTimeOccupancyMap = spaceTimeOccupancyMap != null ? spaceTimeOccupancyMap : new HashMap<>();
this.pathMapping = pathMapping;
diff --git a/algo-zkd/src/main/java/com/algo/service/CollisionDetector.java b/algo-zkd/src/main/java/com/algo/service/CollisionDetector.java
index 79be4b1..1b6e5fe 100644
--- a/algo-zkd/src/main/java/com/algo/service/CollisionDetector.java
+++ b/algo-zkd/src/main/java/com/algo/service/CollisionDetector.java
@@ -70,21 +70,21 @@
System.out.println("寮�濮嬬鎾炴娴嬶紝璺緞鏁伴噺: " + plannedPaths.size());
- // 鏋勫缓楂樼簿搴︽椂绌鸿〃
- Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = buildEnhancedSpaceTimeTable(plannedPaths);
+ // 鏋勫缓鏃剁┖琛�
+ Map<Long, List<SpaceTimeNode>> spaceTimeTable = buildSpaceTimeTable(plannedPaths);
// 妫�娴嬮《鐐瑰啿绐侊紙鍚屼竴鏃堕棿鍚屼竴浣嶇疆锛�
- List<Conflict> vertexConflicts = detectEnhancedVertexConflicts(spaceTimeTable);
+ List<Conflict> vertexConflicts = detectVertexConflicts(spaceTimeTable);
conflicts.addAll(vertexConflicts);
System.out.println("椤剁偣鍐茬獊鏁伴噺: " + vertexConflicts.size());
// 妫�娴嬭竟鍐茬獊锛圕TU浜ゆ崲浣嶇疆锛�
- List<Conflict> edgeConflicts = detectEnhancedEdgeConflicts(spaceTimeTable);
+ List<Conflict> edgeConflicts = detectEdgeConflicts(spaceTimeTable);
conflicts.addAll(edgeConflicts);
System.out.println("杈瑰啿绐佹暟閲�: " + edgeConflicts.size());
// 妫�娴嬭窡闅忓啿绐�
- List<Conflict> followingConflicts = detectEnhancedFollowingConflicts(spaceTimeTable);
+ List<Conflict> followingConflicts = detectFollowingConflicts(spaceTimeTable);
conflicts.addAll(followingConflicts);
System.out.println("璺熼殢鍐茬獊鏁伴噺: " + followingConflicts.size());
@@ -103,8 +103,8 @@
* @param plannedPaths 瑙勫垝璺緞鍒楄〃
* @return 鏃剁┖琛�
*/
- private Map<Long, List<EnhancedSpaceTimeNode>> buildEnhancedSpaceTimeTable(List<PlannedPath> plannedPaths) {
- Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = new HashMap<>();
+ private Map<Long, List<SpaceTimeNode>> buildSpaceTimeTable(List<PlannedPath> plannedPaths) {
+ Map<Long, List<SpaceTimeNode>> spaceTimeTable = new HashMap<>();
for (PlannedPath path : plannedPaths) {
String agvId = path.getAgvId();
@@ -133,7 +133,7 @@
if (coordinates != null) {
// 鍒涘缓鏃堕棿娈靛唴鐨勫涓椂闂寸偣锛堜娇鐢ㄥ疄闄呮椂闂存埑锛�
for (long timePoint = arrivalTime; timePoint <= departureTime; timePoint += timeResolution) {
- EnhancedSpaceTimeNode node = new EnhancedSpaceTimeNode(
+ SpaceTimeNode node = new SpaceTimeNode(
agvId, position, coordinates, timePoint, arrivalTime, departureTime, physicalConfig
);
@@ -261,36 +261,36 @@
* @param spaceTimeTable 鏃剁┖琛�
* @return 椤剁偣鍐茬獊鍒楄〃
*/
- private List<Conflict> detectEnhancedVertexConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
+ private List<Conflict> detectVertexConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
List<Conflict> conflicts = new ArrayList<>();
- for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
+ for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
long timePoint = entry.getKey();
- List<EnhancedSpaceTimeNode> nodes = entry.getValue();
+ List<SpaceTimeNode> nodes = entry.getValue();
// 鎸変綅缃垎缁�
- Map<String, List<EnhancedSpaceTimeNode>> positionGroups = new HashMap<>();
- for (EnhancedSpaceTimeNode node : nodes) {
+ Map<String, List<SpaceTimeNode>> positionGroups = new HashMap<>();
+ for (SpaceTimeNode node : nodes) {
positionGroups.computeIfAbsent(node.position, k -> new ArrayList<>()).add(node);
}
// 妫�鏌ユ瘡涓綅缃槸鍚︽湁澶氫釜CTU
- for (Map.Entry<String, List<EnhancedSpaceTimeNode>> posEntry : positionGroups.entrySet()) {
+ for (Map.Entry<String, List<SpaceTimeNode>> posEntry : positionGroups.entrySet()) {
String position = posEntry.getKey();
- List<EnhancedSpaceTimeNode> ctuNodes = posEntry.getValue();
+ List<SpaceTimeNode> ctuNodes = posEntry.getValue();
if (ctuNodes.size() > 1) {
// 杩涗竴姝ユ鏌ユ椂闂撮噸鍙�
for (int i = 0; i < ctuNodes.size(); i++) {
for (int j = i + 1; j < ctuNodes.size(); j++) {
- EnhancedSpaceTimeNode node1 = ctuNodes.get(i);
- EnhancedSpaceTimeNode node2 = ctuNodes.get(j);
+ SpaceTimeNode node1 = ctuNodes.get(i);
+ SpaceTimeNode node2 = ctuNodes.get(j);
// 妫�鏌ユ椂闂存鏄惁閲嶅彔
if (timeRangeOverlap(node1.arrivalTime, node1.departureTime,
node2.arrivalTime, node2.departureTime)) {
Conflict conflict = new Conflict(
- "enhanced_vertex",
+ "vertex",
node1.agvId,
node2.agvId,
(int) (timePoint / 1000), // 杞崲涓虹鐢ㄤ簬鏄剧ず
@@ -316,7 +316,7 @@
* @param spaceTimeTable 鏃剁┖琛�
* @return 杈瑰啿绐佸垪琛�
*/
- private List<Conflict> detectEnhancedEdgeConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
+ private List<Conflict> detectEdgeConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
List<Conflict> conflicts = new ArrayList<>();
if (spaceTimeTable.isEmpty()) {
@@ -332,8 +332,8 @@
// 鍙鏌ヨ繛缁殑鏃堕棿鐐�
if (nextTime - currentTime <= timeResolution * 2) {
- List<EnhancedSpaceTimeNode> currentNodes = spaceTimeTable.get(currentTime);
- List<EnhancedSpaceTimeNode> nextNodes = spaceTimeTable.get(nextTime);
+ List<SpaceTimeNode> currentNodes = spaceTimeTable.get(currentTime);
+ List<SpaceTimeNode> nextNodes = spaceTimeTable.get(nextTime);
conflicts.addAll(detectPositionSwapping(currentNodes, nextNodes, currentTime, nextTime));
}
@@ -351,19 +351,19 @@
* @param nextTime 涓嬩竴鏃堕棿
* @return 鍐茬獊鍒楄〃
*/
- private List<Conflict> detectPositionSwapping(List<EnhancedSpaceTimeNode> currentNodes,
- List<EnhancedSpaceTimeNode> nextNodes,
+ private List<Conflict> detectPositionSwapping(List<SpaceTimeNode> currentNodes,
+ List<SpaceTimeNode> nextNodes,
long currentTime, long nextTime) {
List<Conflict> conflicts = new ArrayList<>();
Map<String, String> currentPositions = new HashMap<>();
Map<String, String> nextPositions = new HashMap<>();
- for (EnhancedSpaceTimeNode node : currentNodes) {
+ for (SpaceTimeNode node : currentNodes) {
currentPositions.put(node.agvId, node.position);
}
- for (EnhancedSpaceTimeNode node : nextNodes) {
+ for (SpaceTimeNode node : nextNodes) {
nextPositions.put(node.agvId, node.position);
}
@@ -386,7 +386,7 @@
pos1Current.equals(pos2Next) && pos2Current.equals(pos1Next)) {
Conflict conflict = new Conflict(
- "enhanced_edge",
+ "edge",
ctu1,
ctu2,
(int) (currentTime / 1000),
@@ -409,17 +409,18 @@
* @param spaceTimeTable 鏃剁┖琛�
* @return 璺熼殢鍐茬獊鍒楄〃
*/
- private List<Conflict> detectEnhancedFollowingConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
+ private List<Conflict> detectFollowingConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
List<Conflict> conflicts = new ArrayList<>();
- for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
+ // 绗竴閮ㄥ垎锛氭鏌ュ悓涓�鏃堕棿鐐圭殑鑺傜偣
+ for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
long timePoint = entry.getKey();
- List<EnhancedSpaceTimeNode> nodes = entry.getValue();
+ List<SpaceTimeNode> nodes = entry.getValue();
for (int i = 0; i < nodes.size(); i++) {
for (int j = i + 1; j < nodes.size(); j++) {
- EnhancedSpaceTimeNode node1 = nodes.get(i);
- EnhancedSpaceTimeNode node2 = nodes.get(j);
+ SpaceTimeNode node1 = nodes.get(i);
+ SpaceTimeNode node2 = nodes.get(j);
double distance = calculateDistance(node1.coordinates, node2.coordinates);
double minSafeDistance = Math.max(
@@ -429,7 +430,7 @@
if (distance > 0 && distance < minSafeDistance) {
Conflict conflict = new Conflict(
- "enhanced_follow",
+ "follow",
node1.agvId,
node2.agvId,
(int) (timePoint / 1000),
@@ -441,22 +442,18 @@
conflicts.add(conflict);
}
- // 妫�鏌ュ悓涓�浣嶇疆鎴栫浉閭讳綅缃殑鏃堕棿闂撮殧鏄惁瓒冲
- // 濡傛灉涓や釜AGV鍦ㄥ悓涓�浣嶇疆鎴栫浉閭讳綅缃紝瑕佹眰鏈�灏忔椂闂撮棿闅�
- if (distance <= minSafeDistance) {
+ // 妫�鏌ュ悓涓�鏃堕棿鐐圭殑鏃堕棿鑼冨洿閲嶅彔锛堝悓鏃跺崰鐢級
+ if (distance <= minSafeDistance && timeRangeOverlap(
+ node1.arrivalTime, node1.departureTime,
+ node2.arrivalTime, node2.departureTime)) {
double normalSpeed = Math.min(
node1.physicalConfig.getNormalSpeed(),
node2.physicalConfig.getNormalSpeed()
);
- // 鏈�灏忔椂闂撮棿闅� = 瀹夊叏璺濈/閫熷害 + 棰濆瀹夊叏缂撳啿(10绉�)
- // 鎬绘椂闂撮棿闅旂害12~13绉掞紝鐢ㄤ簬闃叉璁＄畻璇樊
long minTimeGap = (long) ((minSafeDistance / normalSpeed + 10.0) * 1000);
- // 妫�鏌ユ椂闂撮棿闅�
long timeGap = Math.abs(node1.arrivalTime - node2.arrivalTime);
- if (timeGap < minTimeGap && timeRangeOverlap(
- node1.arrivalTime, node1.departureTime,
- node2.arrivalTime, node2.departureTime)) {
+ if (timeGap < minTimeGap) {
Conflict conflict = new Conflict(
"time_gap_insufficient",
node1.agvId,
@@ -464,12 +461,104 @@
(int) (timePoint / 1000),
node1.position,
node2.position,
- String.format("CTU %s 鍜� %s 鍦ㄤ綅缃� %s/%s 鏃堕棿闂撮殧涓嶈冻 (%.2f绉� < %.2f绉�)",
+ String.format("CTU %s 鍜� %s 鍦ㄤ綅缃� %s/%s 鍚屾椂鍗犵敤涓旀椂闂撮棿闅斾笉瓒� (%.2f绉� < %.2f绉�)",
node1.agvId, node2.agvId, node1.position, node2.position,
timeGap / 1000.0, minTimeGap / 1000.0)
);
conflicts.add(conflict);
}
+ }
+ }
+ }
+ }
+
+ // 绗簩閮ㄥ垎锛氭鏌ュ悓涓�浣嶇疆杩炵画缁忚繃鐨勬椂闂撮棿闅�
+ Map<String, Map<String, SpaceTimeNode>> positionAgvMap = new HashMap<>();
+ for (List<SpaceTimeNode> nodes : spaceTimeTable.values()) {
+ for (SpaceTimeNode node : nodes) {
+ positionAgvMap.computeIfAbsent(node.position, k -> new HashMap<>())
+ .compute(node.agvId, (k, existing) -> {
+ if (existing == null) {
+ return node;
+ } else {
+ // 鍚堝苟鏃堕棿鑼冨洿
+ long minArrival = Math.min(existing.arrivalTime, node.arrivalTime);
+ long maxDeparture = Math.max(existing.departureTime, node.departureTime);
+ return new SpaceTimeNode(
+ node.agvId, node.position, node.coordinates,
+ minArrival, minArrival, maxDeparture, node.physicalConfig
+ );
+ }
+ });
+ }
+ }
+
+ // 瀵逛簬姣忎釜浣嶇疆锛屾鏌ユ墍鏈夌粡杩囪浣嶇疆鐨凙GV鐨勬椂闂撮棿闅�
+ Set<String> processedPairs = new HashSet<>();
+ for (Map.Entry<String, Map<String, SpaceTimeNode>> positionEntry : positionAgvMap.entrySet()) {
+ String position = positionEntry.getKey();
+ Map<String, SpaceTimeNode> agvNodes = positionEntry.getValue();
+
+ List<SpaceTimeNode> nodes = new ArrayList<>(agvNodes.values());
+ nodes.sort((n1, n2) -> Long.compare(n1.arrivalTime, n2.arrivalTime));
+
+ for (int i = 0; i < nodes.size(); i++) {
+ for (int j = i + 1; j < nodes.size(); j++) {
+ SpaceTimeNode node1 = nodes.get(i);
+ SpaceTimeNode node2 = nodes.get(j);
+
+ if (node1.agvId.equals(node2.agvId)) {
+ continue;
+ }
+
+ String pairKey1 = node1.agvId + "_" + node2.agvId + "_" + position;
+ String pairKey2 = node2.agvId + "_" + node1.agvId + "_" + position;
+ if (processedPairs.contains(pairKey1) || processedPairs.contains(pairKey2)) {
+ continue;
+ }
+
+ // 璁＄畻鏃堕棿闂撮殧锛歯ode1绂诲紑鍚庯紝node2鍒拌揪鐨勬椂闂撮棿闅�
+ long timeGap;
+ if (node1.departureTime <= node2.arrivalTime) {
+ timeGap = node2.arrivalTime - node1.departureTime;
+ } else if (node2.departureTime <= node1.arrivalTime) {
+ timeGap = node1.arrivalTime - node2.departureTime;
+ } else {
+ continue;
+ }
+
+ // 璁＄畻鏈�灏忓畨鍏ㄦ椂闂撮棿闅�
+ double minSafeDistance = Math.max(
+ node1.physicalConfig.getMinSafetyDistance(),
+ node2.physicalConfig.getMinSafetyDistance()
+ );
+ double normalSpeed = Math.min(
+ node1.physicalConfig.getNormalSpeed(),
+ node2.physicalConfig.getNormalSpeed()
+ );
+ // 鏈�灏忔椂闂撮棿闅� = 瀹夊叏璺濈/閫熷害 + 棰濆瀹夊叏缂撳啿(7绉�)
+ long minTimeGap = (long) ((minSafeDistance / normalSpeed + 7.0) * 1000);
+
+ // 妫�鏌ユ椂闂撮棿闅旀槸鍚﹁冻澶�
+ if (timeGap < minTimeGap) {
+ String firstAgv = node1.arrivalTime < node2.arrivalTime ? node1.agvId : node2.agvId;
+ String secondAgv = node1.arrivalTime < node2.arrivalTime ? node2.agvId : node1.agvId;
+ long secondArrival = node1.arrivalTime < node2.arrivalTime ? node2.arrivalTime : node1.arrivalTime;
+
+ Conflict conflict = new Conflict(
+ "time_gap_insufficient",
+ firstAgv,
+ secondAgv,
+ (int) (secondArrival / 1000),
+ position,
+ position,
+ String.format("CTU %s 鍜� %s 鍦ㄤ綅缃� %s 杩炵画缁忚繃鏃堕棿闂撮殧涓嶈冻 (%.2f绉� < %.2f绉�, %s绂诲紑鍚�%.2f绉�%s鍒拌揪)",
+ firstAgv, secondAgv, position,
+ timeGap / 1000.0, minTimeGap / 1000.0,
+ firstAgv, timeGap / 1000.0, secondAgv)
+ );
+ conflicts.add(conflict);
+ processedPairs.add(pairKey1);
}
}
}
@@ -484,17 +573,17 @@
* @param spaceTimeTable 鏃剁┖琛�
* @return 鐗╃悊鍐茬獊鍒楄〃
*/
- private List<Conflict> detectPhysicalSizeConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
+ private List<Conflict> detectPhysicalSizeConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
List<Conflict> conflicts = new ArrayList<>();
- for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
+ for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
long timePoint = entry.getKey();
- List<EnhancedSpaceTimeNode> nodes = entry.getValue();
+ List<SpaceTimeNode> nodes = entry.getValue();
for (int i = 0; i < nodes.size(); i++) {
for (int j = i + 1; j < nodes.size(); j++) {
- EnhancedSpaceTimeNode node1 = nodes.get(i);
- EnhancedSpaceTimeNode node2 = nodes.get(j);
+ SpaceTimeNode node1 = nodes.get(i);
+ SpaceTimeNode node2 = nodes.get(j);
if (checkPhysicalCollision(node1, node2)) {
Conflict conflict = new Conflict(
@@ -523,7 +612,7 @@
* @param node2 CTU鑺傜偣2
* @return 鏄惁纰版挒
*/
- private boolean checkPhysicalCollision(EnhancedSpaceTimeNode node1, EnhancedSpaceTimeNode node2) {
+ private boolean checkPhysicalCollision(SpaceTimeNode node1, SpaceTimeNode node2) {
double distance = calculateDistance(node1.coordinates, node2.coordinates);
// 鑰冭檻CTU鐨勭墿鐞嗗昂瀵�
@@ -566,7 +655,7 @@
/**
* 鏃剁┖鑺傜偣鍐呴儴绫�
*/
- private static class EnhancedSpaceTimeNode {
+ private static class SpaceTimeNode {
final String agvId;
final String position;
final int[] coordinates;
@@ -575,9 +664,9 @@
final long departureTime;
final CTUPhysicalConfig physicalConfig;
- public EnhancedSpaceTimeNode(String agvId, String position, int[] coordinates,
- long timePoint, long arrivalTime, long departureTime,
- CTUPhysicalConfig physicalConfig) {
+ public SpaceTimeNode(String agvId, String position, int[] coordinates,
+ long timePoint, long arrivalTime, long departureTime,
+ CTUPhysicalConfig physicalConfig) {
this.agvId = agvId;
this.position = position;
this.coordinates = coordinates;
diff --git a/algo-zkd/src/main/java/com/algo/service/CollisionResolver.java b/algo-zkd/src/main/java/com/algo/service/CollisionResolver.java
index 1c70c81..537e513 100644
--- a/algo-zkd/src/main/java/com/algo/service/CollisionResolver.java
+++ b/algo-zkd/src/main/java/com/algo/service/CollisionResolver.java
@@ -1,10 +1,12 @@
package com.algo.service;
+import com.algo.model.AGVStatus;
import com.algo.model.Conflict;
import com.algo.model.ExecutingTask;
import com.algo.model.PathCode;
import com.algo.model.PlannedPath;
import com.algo.model.CTUPhysicalConfig;
+import com.algo.util.JsonUtils;
import com.algo.util.PathTimeCalculator;
import java.util.*;
@@ -24,6 +26,16 @@
* 璺緞鏃堕棿璁＄畻鍣�
*/
private PathTimeCalculator timeCalculator;
+
+ /**
+ * 璺緞瑙勫垝鍣�
+ */
+ private PathPlanner pathPlanner;
+
+ /**
+ * 璺緞鏄犲皠琛紙鐢ㄤ簬鍧愭爣杞崲锛�
+ */
+ private Map<String, Map<String, Integer>> pathMapping;
/**
* 鏋勯�犲嚱鏁�
@@ -40,6 +52,20 @@
public void setTimeCalculator(PathTimeCalculator timeCalculator) {
this.timeCalculator = timeCalculator;
}
+
+ /**
+ * 璁剧疆璺緞瑙勫垝鍣�
+ */
+ public void setPathPlanner(PathPlanner pathPlanner) {
+ this.pathPlanner = pathPlanner;
+ }
+
+ /**
+ * 璁剧疆璺緞鏄犲皠琛�
+ */
+ public void setPathMapping(Map<String, Map<String, Integer>> pathMapping) {
+ this.pathMapping = pathMapping;
+ }
/**
* 瑙ｅ喅璺緞鍐茬獊
@@ -47,11 +73,17 @@
* @param plannedPaths 瑙勫垝璺緞鍒楄〃
* @param conflicts 鍐茬獊鍒楄〃
* @param executingTasks 鎵ц涓换鍔″垪琛�
+ * @param agvStatusList AGV鐘舵�佸垪琛紙鐢ㄤ簬鍒ゆ柇鍓╀綑璺緞锛�
* @return 瑙ｅ喅鍐茬獊鍚庣殑璺緞鍒楄〃
+ */
+ /**
+ * 瑙ｅ喅璺緞鍐茬獊
*/
public List<PlannedPath> resolveConflicts(List<PlannedPath> plannedPaths,
List<Conflict> conflicts,
- List<ExecutingTask> executingTasks) {
+ List<ExecutingTask> executingTasks,
+ List<AGVStatus> agvStatusList,
+ long unifiedTimestamp) {
if (conflicts == null || conflicts.isEmpty()) {
return plannedPaths;
}
@@ -63,6 +95,28 @@
for (PlannedPath path : plannedPaths) {
pathsMap.put(path.getAgvId(), path);
}
+
+ // 鏋勫缓AGV鐘舵�佸瓧鍏�
+ Map<String, AGVStatus> agvStatusMap = new HashMap<>();
+ if (agvStatusList != null) {
+ for (AGVStatus agv : agvStatusList) {
+ if (agv.getAgvId() != null) {
+ agvStatusMap.put(agv.getAgvId(), agv);
+ }
+ }
+ }
+
+ // 鏋勫缓鎵�鏈夊凡瑙勫垝璺緞鐨勫崰鐢ㄤ綅缃泦鍚堬紙鐢ㄤ簬鏌ユ壘绛夊緟浣嶇疆鏃堕伩寮�锛�
+ Set<String> allOccupiedPositions = new HashSet<>();
+ for (PlannedPath path : plannedPaths) {
+ if (path.getCodeList() != null) {
+ for (PathCode code : path.getCodeList()) {
+ if (code.getCode() != null) {
+ allOccupiedPositions.add(code.getCode());
+ }
+ }
+ }
+ }
// 鎸夋椂闂存鎺掑簭澶勭悊鍐茬獊
List<Conflict> sortedConflicts = new ArrayList<>(conflicts);
@@ -70,7 +124,9 @@
// 閫愪釜瑙ｅ喅鍐茬獊
for (Conflict conflict : sortedConflicts) {
- resolveSingleConflict(pathsMap, conflict, executingTasks);
+ // 瀵逛簬姣忎釜鍐茬獊锛岃绠椾袱杞﹁矾寰勯噸鍚堢殑浣嶇疆
+ Set<String> overlappingPositions = findOverlappingPositions(pathsMap, conflict);
+ resolveSingleConflict(pathsMap, conflict, executingTasks, agvStatusMap, overlappingPositions, unifiedTimestamp);
}
List<PlannedPath> resolvedPaths = new ArrayList<>(pathsMap.values());
@@ -81,17 +137,30 @@
return resolvedPaths;
}
+
+ /**
+ * 瑙ｅ喅璺緞鍐茬獊
+ *
+ * @param plannedPaths 瑙勫垝璺緞鍒楄〃
+ * @param conflicts 鍐茬獊鍒楄〃
+ * @param executingTasks 鎵ц涓换鍔″垪琛�
+ * @return 瑙ｅ喅鍐茬獊鍚庣殑璺緞鍒楄〃
+ */
+ public List<PlannedPath> resolveConflicts(List<PlannedPath> plannedPaths,
+ List<Conflict> conflicts,
+ List<ExecutingTask> executingTasks) {
+ return resolveConflicts(plannedPaths, conflicts, executingTasks, null, System.currentTimeMillis() / 1000);
+ }
/**
* 瑙ｅ喅鍗曚釜鍐茬獊
- *
- * @param pathsMap 璺緞鏄犲皠
- * @param conflict 鍐茬獊
- * @param executingTasks 鎵ц涓换鍔″垪琛�
*/
private void resolveSingleConflict(Map<String, PlannedPath> pathsMap,
Conflict conflict,
- List<ExecutingTask> executingTasks) {
+ List<ExecutingTask> executingTasks,
+ Map<String, AGVStatus> agvStatusMap,
+ Set<String> overlappingPositions,
+ long unifiedTimestamp) {
String conflictType = conflict.getType();
switch (conflictType) {
@@ -103,6 +172,9 @@
break;
case "follow":
resolveFollowingConflict(pathsMap, conflict, executingTasks);
+ break;
+ case "time_gap_insufficient":
+ resolveTimeGapConflict(pathsMap, conflict, executingTasks, agvStatusMap, overlappingPositions, unifiedTimestamp);
break;
default:
System.out.println("鏈煡鍐茬獊绫诲瀷: " + conflictType);
@@ -177,8 +249,8 @@
* @param executingTasks 鎵ц涓换鍔″垪琛�
*/
private void resolveFollowingConflict(Map<String, PlannedPath> pathsMap,
- Conflict conflict,
- List<ExecutingTask> executingTasks) {
+ Conflict conflict,
+ List<ExecutingTask> executingTasks) {
String agv1 = conflict.getAgv1();
String agv2 = conflict.getAgv2();
int timeStep = conflict.getTimeStep();
@@ -200,7 +272,577 @@
}
/**
- * 璇勪及AGV浼樺厛绾� - 浼樺寲鐗堟湰锛屽彧淇濈暀蹇呰瀛楁
+ * 鎵惧埌涓よ溅璺緞閲嶅悎鐨勪綅缃�
+ */
+ private Set<String> findOverlappingPositions(Map<String, PlannedPath> pathsMap, Conflict conflict) {
+ Set<String> overlappingPositions = new HashSet<>();
+
+ PlannedPath path1 = pathsMap.get(conflict.getAgv1());
+ PlannedPath path2 = pathsMap.get(conflict.getAgv2());
+
+ if (path1 == null || path2 == null ||
+ path1.getCodeList() == null || path2.getCodeList() == null) {
+ return overlappingPositions;
+ }
+
+ // 鏀堕泦path1鐨勬墍鏈変綅缃�
+ Set<String> positions1 = new HashSet<>();
+ for (PathCode code : path1.getCodeList()) {
+ if (code.getCode() != null) {
+ positions1.add(code.getCode());
+ }
+ }
+
+ // 鏀堕泦path2鐨勬墍鏈変綅缃�
+ Set<String> positions2 = new HashSet<>();
+ for (PathCode code : path2.getCodeList()) {
+ if (code.getCode() != null) {
+ positions2.add(code.getCode());
+ }
+ }
+
+ // 鎵惧埌閲嶅悎鐨勪綅缃�
+ for (String pos : positions1) {
+ if (positions2.contains(pos)) {
+ overlappingPositions.add(pos);
+ }
+ }
+
+ System.out.println("[璋冭瘯] 涓よ溅璺緞閲嶅悎浣嶇疆: " + overlappingPositions);
+ return overlappingPositions;
+ }
+
+ /**
+ * 瑙ｅ喅鏃堕棿闂撮殧涓嶈冻鍐茬獊
+ * 绛栫暐锛氭湁鍓╀綑璺緞鐨凙GV缁х画鎵ц锛屾柊璺緞鐨凙GV绉诲姩鍒板畨鍏ㄧ瓑寰呬綅缃�
+ */
+ private void resolveTimeGapConflict(Map<String, PlannedPath> pathsMap,
+ Conflict conflict,
+ List<ExecutingTask> executingTasks,
+ Map<String, AGVStatus> agvStatusMap,
+ Set<String> overlappingPositions,
+ long unifiedTimestamp) {
+ String firstAgv = conflict.getAgv1(); // 鍏堢粡杩囩殑AGV
+ String secondAgv = conflict.getAgv2(); // 鍚庣粡杩囩殑AGV
+ String conflictPosition = conflict.getPosition1(); // 鍐茬獊浣嶇疆
+
+ PlannedPath path1 = pathsMap.get(firstAgv);
+ PlannedPath path2 = pathsMap.get(secondAgv);
+
+ if (path1 == null || path2 == null) {
+ System.out.println("璀﹀憡: 鏃犳硶瑙ｅ喅鍐茬獊锛岃矾寰勪笉瀛樺湪");
+ return;
+ }
+
+ // 鎵撳嵃鍘熷璺緞淇℃伅锛堣皟璇曠敤锛�
+ System.out.println("[鍐茬獊瑙ｅ喅debug] 鍐茬獊浣嶇疆: " + conflictPosition);
+ System.out.println("[鍐茬獊瑙ｅ喅debug] AGV " + firstAgv + " 鍘熷璺緞: " +
+ (path1.getCodeList() != null && !path1.getCodeList().isEmpty() ?
+ path1.getCodeList().get(0).getCode() + " -> " +
+ path1.getCodeList().get(path1.getCodeList().size() - 1).getCode() +
+ " (鍏�" + path1.getCodeList().size() + "鐐�)" : "绌鸿矾寰�"));
+ System.out.println("[鍐茬獊瑙ｅ喅debug] AGV " + secondAgv + " 鍘熷璺緞: " +
+ (path2.getCodeList() != null && !path2.getCodeList().isEmpty() ?
+ path2.getCodeList().get(0).getCode() + " -> " +
+ path2.getCodeList().get(path2.getCodeList().size() - 1).getCode() +
+ " (鍏�" + path2.getCodeList().size() + "鐐�)" : "绌鸿矾寰�"));
+
+ // 妫�鏌ュ師濮嬭矾寰勬槸鍚︾粡杩囧啿绐佷綅缃�
+ if (path1.getCodeList() != null) {
+ boolean path1HasConflictPos = path1.getCodeList().stream()
+ .anyMatch(code -> conflictPosition.equals(code.getCode()));
+ System.out.println("[鍐茬獊瑙ｅ喅debug] AGV " + firstAgv + " 璺緞鏄惁缁忚繃鍐茬獊浣嶇疆 " + conflictPosition + ": " + path1HasConflictPos);
+ }
+ if (path2.getCodeList() != null) {
+ boolean path2HasConflictPos = path2.getCodeList().stream()
+ .anyMatch(code -> conflictPosition.equals(code.getCode()));
+ System.out.println("[鍐茬獊瑙ｅ喅debug] AGV " + secondAgv + " 璺緞鏄惁缁忚繃鍐茬獊浣嶇疆 " + conflictPosition + ": " + path2HasConflictPos);
+ }
+
+ // 鍒ゆ柇鍝釜AGV鏈夊墿浣欒矾寰�
+ AGVStatus agv1Status = agvStatusMap != null ? agvStatusMap.get(firstAgv) : null;
+ AGVStatus agv2Status = agvStatusMap != null ? agvStatusMap.get(secondAgv) : null;
+
+ boolean agv1HasRemainingPath = agv1Status != null && agv1Status.hasRemainingPath();
+ boolean agv2HasRemainingPath = agv2Status != null && agv2Status.hasRemainingPath();
+
+ // 纭畾闇�瑕佺瓑寰呯殑AGV
+ String waitingAgv;
+ PlannedPath waitingPath;
+ AGVStatus waitingAgvStatus;
+
+ if (agv1HasRemainingPath && !agv2HasRemainingPath) {
+ // AGV1鏈夊墿浣欒矾寰勶紝AGV2鏄柊璺緞 -> AGV2绛夊緟
+ waitingAgv = secondAgv;
+ waitingPath = path2;
+ waitingAgvStatus = agv2Status;
+ System.out.println("鍐茬獊瑙ｅ喅: AGV " + firstAgv + " 鏈夊墿浣欒矾寰勭户缁墽琛岋紝AGV " + secondAgv + " 鏂拌矾寰勯渶瑕佺瓑寰�");
+ } else if (!agv1HasRemainingPath && agv2HasRemainingPath) {
+ // AGV1鏄柊璺緞锛孉GV2鏈夊墿浣欒矾寰� -> AGV1绛夊緟
+ waitingAgv = firstAgv;
+ waitingPath = path1;
+ waitingAgvStatus = agv1Status;
+ System.out.println("鍐茬獊瑙ｅ喅: AGV " + secondAgv + " 鏈夊墿浣欒矾寰勭户缁墽琛岋紝AGV " + firstAgv + " 鏂拌矾寰勯渶瑕佺瓑寰�");
+ } else if (agv1HasRemainingPath && agv2HasRemainingPath) {
+ // 涓や釜閮芥湁鍓╀綑璺緞(鏈�濂戒笉鍑虹幇杩欎釜鎯呭喌)
+ AGVPriority priority1 = evaluateAgvPriority(firstAgv, path1, executingTasks);
+ AGVPriority priority2 = evaluateAgvPriority(secondAgv, path2, executingTasks);
+
+ if (priority1.priorityScore >= priority2.priorityScore) {
+ // AGV1浼樺厛绾ч珮锛孉GV2绛夊緟
+ waitingAgv = secondAgv;
+ waitingPath = path2;
+ waitingAgvStatus = agv2Status;
+ System.out.println("鍐茬獊瑙ｅ喅: 涓や釜閮芥湁鍓╀綑璺緞锛孉GV " + firstAgv + " 浼樺厛绾ч珮缁х画鎵ц锛孉GV " + secondAgv + " 绛夊緟");
+ } else {
+ // AGV2浼樺厛绾ч珮锛孉GV1绛夊緟
+ waitingAgv = firstAgv;
+ waitingPath = path1;
+ waitingAgvStatus = agv1Status;
+ System.out.println("鍐茬獊瑙ｅ喅: 涓や釜閮芥湁鍓╀綑璺緞锛孉GV " + secondAgv + " 浼樺厛绾ч珮缁х画鎵ц锛孉GV " + firstAgv + " 绛夊緟");
+ }
+ } else {
+ // 涓や釜閮芥槸鏂拌矾寰� -> 鏍规嵁浼樺厛绾у喅瀹�
+ AGVPriority priority1 = evaluateAgvPriority(firstAgv, path1, executingTasks);
+ AGVPriority priority2 = evaluateAgvPriority(secondAgv, path2, executingTasks);
+
+ if (priority1.priorityScore >= priority2.priorityScore) {
+ waitingAgv = secondAgv;
+ waitingPath = path2;
+ waitingAgvStatus = agv2Status;
+ System.out.println("鍐茬獊瑙ｅ喅: 涓や釜閮芥槸鏂拌矾寰勶紝AGV " + firstAgv + " 浼樺厛绾ч珮缁х画鎵ц锛孉GV " + secondAgv + " 绛夊緟");
+ } else {
+ waitingAgv = firstAgv;
+ waitingPath = path1;
+ waitingAgvStatus = agv1Status;
+ System.out.println("鍐茬獊瑙ｅ喅: 涓や釜閮芥槸鏂拌矾寰勶紝AGV " + secondAgv + " 浼樺厛绾ч珮缁х画鎵ц锛孉GV " + firstAgv + " 绛夊緟");
+ }
+ }
+
+ // 鎵惧埌绛夊緟AGV鐨勫綋鍓嶄綅缃紙璺緞璧风偣锛�
+ String currentPosition = null;
+ if (waitingPath != null && !waitingPath.getCodeList().isEmpty()) {
+ currentPosition = waitingPath.getCodeList().get(0).getCode();
+ }
+ if (currentPosition == null && waitingAgvStatus != null) {
+ currentPosition = waitingAgvStatus.getPosition();
+ }
+
+ if (currentPosition == null || conflictPosition == null) {
+ System.out.println("璀﹀憡: 鏃犳硶鎵惧埌褰撳墠浣嶇疆鎴栧啿绐佷綅缃紝璺宠繃鍐茬獊瑙ｅ喅");
+ return;
+ }
+
+ // 妫�鏌ョ瓑寰匒GV鐨勮矾寰勬槸鍚︾粡杩囧啿绐佷綅缃�
+ boolean waitingPathHasConflictPos = false;
+ if (waitingPath != null && waitingPath.getCodeList() != null) {
+ waitingPathHasConflictPos = waitingPath.getCodeList().stream()
+ .anyMatch(code -> conflictPosition.equals(code.getCode()));
+ }
+
+ if (!waitingPathHasConflictPos) {
+ System.out.println("AGV " + waitingAgv + " 鐨勮矾寰勪笉缁忚繃鍐茬獊浣嶇疆 " + conflictPosition + "锛岃烦杩囧啿绐佽В鍐�");
+ return;
+ }
+
+ String waitPosition = findWaitPositionOnOriginalPath(waitingPath, conflictPosition, waitingAgvStatus, overlappingPositions);
+ boolean waitPositionOnOriginalPath = (waitPosition != null);
+
+ if (waitPosition == null) {
+ System.out.println("鍘熷璺緞涓婃病鏈夊悎閫傜殑绛夊緟浣嶇疆锛屽皾璇曞鎵惧叾浠栦綅缃�");
+ waitPosition = findSafeWaitingPosition(currentPosition, conflictPosition, waitingAgvStatus, overlappingPositions);
+ }
+
+ if (waitPosition == null) {
+ System.out.println("璀﹀憡: 鏃犳硶鎵惧埌瀹夊叏绛夊緟浣嶇疆锛孉GV " + waitingAgv + " 淇濇寔褰撳墠浣嶇疆");
+ // 濡傛灉鎵句笉鍒扮瓑寰呬綅缃紝鑷冲皯鎴柇璺緞锛屽彧淇濈暀鍒板綋鍓嶄綅缃�
+ truncatePathToCurrentPosition(waitingPath, currentPosition);
+ return;
+ }
+
+ if (waitPositionOnOriginalPath && waitingPath != null) {
+ truncatePathToWaitPosition(waitingPath, waitPosition);
+ System.out.println("AGV " + waitingAgv + " 璺緞宸叉埅鏂埌绛夊緟浣嶇疆 " + waitPosition + "锛堝湪鍘熷璺緞涓婏紝璺濈鍐茬獊鐐� " + conflictPosition + " 鑷冲皯5姝ワ級");
+ return;
+ }
+
+ String originalSegId = waitingPath != null ? waitingPath.getSegId() : null;
+ ExecutingTask waitingTask = null;
+ if (executingTasks != null) {
+ for (ExecutingTask task : executingTasks) {
+ if (task.getAgvId().equals(waitingAgv)) {
+ waitingTask = task;
+ break;
+ }
+ }
+ }
+
+ // 瑙勫垝鍒扮瓑寰呬綅缃殑璺緞
+ PlannedPath newWaitPath = planPathToWaitPosition(waitingAgv, currentPosition, waitPosition,
+ waitingAgvStatus, originalSegId, waitingTask, unifiedTimestamp);
+
+ if (newWaitPath != null) {
+ pathsMap.put(waitingAgv, newWaitPath);
+ System.out.println("AGV " + waitingAgv + " 宸茶鍒掑埌绛夊緟浣嶇疆 " + waitPosition + "锛堣窛绂诲啿绐佺偣 " + conflictPosition + " 鑷冲皯5姝ワ級");
+ } else {
+ System.out.println("璀﹀憡: 鏃犳硶瑙勫垝鍒扮瓑寰呬綅缃殑璺緞锛孉GV " + waitingAgv + " 淇濇寔褰撳墠浣嶇疆");
+ truncatePathToCurrentPosition(waitingPath, currentPosition);
+ }
+ }
+
+ /**
+ * 鎵惧埌璺濈鍐茬獊鐐瑰畨鍏ㄨ窛绂荤殑瀹夊叏绛夊緟浣嶇疆
+ */
+ private String findSafeWaitingPosition(String currentPosition, String conflictPosition,
+ AGVStatus agvStatus, Set<String> overlappingPositions) {
+ if (pathPlanner == null) {
+ System.out.println("璀﹀憡: 璺緞瑙勫垝鍣ㄦ湭璁剧疆锛屾棤娉曟煡鎵剧瓑寰呬綅缃�");
+ return null;
+ }
+
+ Map<String, Map<String, Integer>> pathMappingToUse = pathMapping;
+ if (pathMappingToUse == null && pathPlanner != null) {
+ pathMappingToUse = pathPlanner.getPathMapping();
+ }
+
+ if (pathMappingToUse == null) {
+ System.out.println("璀﹀憡: 璺緞鏄犲皠琛ㄦ湭璁剧疆锛屾棤娉曟煡鎵剧瓑寰呬綅缃�");
+ return null;
+ }
+
+ CTUPhysicalConfig config = agvStatus != null && agvStatus.getPhysicalConfig() != null
+ ? agvStatus.getPhysicalConfig()
+ : new CTUPhysicalConfig();
+
+ // 瀹夊叏璺濈瑕佹眰
+ int minTargetDistanceSteps = 5;
+
+ int[] conflictCoord = JsonUtils.getCoordinate(conflictPosition, pathMappingToUse);
+ if (conflictCoord == null) {
+ System.out.println("璀﹀憡: 鏃犳硶鑾峰彇鍐茬獊浣嶇疆鐨勫潗鏍�");
+ return null;
+ }
+
+ // 浣跨敤BFS鎼滅储婊¤冻璺濈瑕佹眰鐨勪綅缃�
+ Queue<String> queue = new LinkedList<>();
+ Map<String, Integer> visited = new HashMap<>();
+ Set<String> blockedPositions = new HashSet<>();
+ blockedPositions.add(conflictPosition);
+
+ if (overlappingPositions != null) {
+ blockedPositions.addAll(overlappingPositions);
+ }
+
+ int[] currentCoord = JsonUtils.getCoordinate(currentPosition, pathMappingToUse);
+ if (currentCoord == null) {
+ System.out.println("璀﹀憡: 鏃犳硶鑾峰彇褰撳墠浣嶇疆鐨勫潗鏍�");
+ return null;
+ }
+
+ queue.offer(currentPosition);
+ visited.put(currentPosition, 0);
+
+ final int MAX_SEARCH_DEPTH = 20;
+
+ List<CandidatePosition> candidates = new ArrayList<>();
+
+ while (!queue.isEmpty()) {
+ String pos = queue.poll();
+ int depth = visited.get(pos);
+
+ if (depth >= MAX_SEARCH_DEPTH) {
+ break;
+ }
+
+ int[] posCoord = JsonUtils.getCoordinate(pos, pathMappingToUse);
+ if (posCoord == null) {
+ continue;
+ }
+
+ int distanceToConflict = Math.abs(posCoord[0] - conflictCoord[0]) +
+ Math.abs(posCoord[1] - conflictCoord[1]);
+
+ int distanceToCurrent = Math.abs(posCoord[0] - currentCoord[0]) +
+ Math.abs(posCoord[1] - currentCoord[1]);
+
+ if (distanceToConflict >= minTargetDistanceSteps) {
+ if (!blockedPositions.contains(pos)) {
+ candidates.add(new CandidatePosition(pos, distanceToConflict, distanceToCurrent));
+ }
+ }
+
+ List<Map<String, String>> neighbors = pathPlanner.getNeighbors(pos);
+ if (neighbors != null) {
+ for (Map<String, String> neighbor : neighbors) {
+ String neighborPos = neighbor.get("code");
+ if (neighborPos == null || visited.containsKey(neighborPos)) {
+ continue;
+ }
+ visited.put(neighborPos, depth + 1);
+ queue.offer(neighborPos);
+ }
+ }
+ }
+
+ if (!candidates.isEmpty()) {
+ candidates.sort((c1, c2) -> {
+ int compare = Integer.compare(c1.distanceToCurrent, c2.distanceToCurrent);
+ if (compare != 0) {
+ return compare;
+ }
+ // 濡傛灉璺濈褰撳墠浣嶇疆鐩稿悓锛岄�夋嫨璺濈鍐茬獊鐐规洿杩戠殑
+ return Integer.compare(c1.distanceToConflict, c2.distanceToConflict);
+ });
+
+ CandidatePosition best = candidates.get(0);
+ System.out.println("鎵惧埌瀹夊叏绛夊緟浣嶇疆: " + best.position +
+ "锛岃窛绂诲啿绐佺偣 " + best.distanceToConflict + " 姝�" +
+ "锛岃窛绂诲綋鍓嶄綅缃� " + best.distanceToCurrent + " 姝�");
+ return best.position;
+ }
+
+ System.out.println("璀﹀憡: 鏃犳硶鎵惧埌婊¤冻璺濈瑕佹眰鐨勫畨鍏ㄧ瓑寰呬綅缃�");
+ return null;
+ }
+
+ /**
+ * 鍦ㄥ師濮嬭矾寰勪笂鎵句竴涓窛绂诲啿绐佺偣瓒冲杩滅殑浣嶇疆
+ */
+ private String findWaitPositionOnOriginalPath(PlannedPath originalPath, String conflictPosition,
+ AGVStatus agvStatus, Set<String> overlappingPositions) {
+ if (originalPath == null || originalPath.getCodeList() == null || originalPath.getCodeList().isEmpty()) {
+ return null;
+ }
+
+ Map<String, Map<String, Integer>> pathMappingToUse = pathMapping;
+ if (pathMappingToUse == null && pathPlanner != null) {
+ pathMappingToUse = pathPlanner.getPathMapping();
+ }
+ if (pathMappingToUse == null) {
+ return null;
+ }
+
+ int[] conflictCoord = JsonUtils.getCoordinate(conflictPosition, pathMappingToUse);
+ if (conflictCoord == null) {
+ return null;
+ }
+
+ // 瀹夊叏璺濈瑕佹眰锛氳窛绂诲啿绐佺偣鑷冲皯5姝�
+ int minTargetDistanceSteps = 5;
+
+ List<PathCode> codeList = originalPath.getCodeList();
+ List<CandidatePosition> candidates = new ArrayList<>();
+
+ int conflictIndex = -1;
+ for (int i = 0; i < codeList.size(); i++) {
+ if (conflictPosition.equals(codeList.get(i).getCode())) {
+ conflictIndex = i;
+ break;
+ }
+ }
+
+ if (conflictIndex < 0) {
+ System.out.println("[璋冭瘯] 鍘熷璺緞涓湭鎵惧埌鍐茬獊浣嶇疆 " + conflictPosition);
+ return null;
+ }
+
+ int checkedCount = 0;
+ int blockedCount = 0;
+ int tooCloseCount = 0;
+ for (int i = 0; i < conflictIndex; i++) {
+ String pos = codeList.get(i).getCode();
+ checkedCount++;
+
+ if (overlappingPositions != null && overlappingPositions.contains(pos)) {
+ blockedCount++;
+ continue;
+ }
+
+ int[] posCoord = JsonUtils.getCoordinate(pos, pathMappingToUse);
+ if (posCoord == null) {
+ continue;
+ }
+
+ int distanceToConflict = Math.abs(posCoord[0] - conflictCoord[0]) +
+ Math.abs(posCoord[1] - conflictCoord[1]);
+
+ int distanceToStart = i;
+
+ if (distanceToConflict >= minTargetDistanceSteps) {
+ candidates.add(new CandidatePosition(pos, distanceToConflict, distanceToStart));
+ } else {
+ tooCloseCount++;
+ }
+ }
+
+ System.out.println("[璋冭瘯] 鍘熷璺緞妫�鏌�: 鍐茬獊浣嶇疆=" + conflictPosition +
+ ", 绱㈠紩=" + conflictIndex +
+ ", 妫�鏌ヤ簡" + checkedCount + "涓偣, " +
+ "閲嶅悎浣嶇疆=" + blockedCount + ", " +
+ "璺濈澶繎=" + tooCloseCount + ", " +
+ "鍊欓�変綅缃�=" + candidates.size());
+
+ if (!candidates.isEmpty()) {
+ candidates.sort((c1, c2) -> {
+ int compare = Integer.compare(c1.distanceToConflict, c2.distanceToConflict);
+ if (compare != 0) {
+ return compare;
+ }
+ return Integer.compare(c2.distanceToCurrent, c1.distanceToCurrent);
+ });
+
+ CandidatePosition best = candidates.get(0);
+ System.out.println("鍦ㄥ師濮嬭矾寰勪笂鎵惧埌绛夊緟浣嶇疆: " + best.position +
+ "锛岃窛绂诲啿绐佺偣 " + best.distanceToConflict + " 姝�" +
+ "锛屽湪璺緞涓殑浣嶇疆: 绗�" + (best.distanceToCurrent + 1) + "涓偣");
+ return best.position;
+ }
+
+ System.out.println("[璋冭瘯] 鍘熷璺緞涓婃病鏈夋壘鍒板悎閫傜殑绛夊緟浣嶇疆锛堣窛绂诲啿绐佺偣鑷冲皯5姝ワ級");
+ return null;
+ }
+
+ /**
+ * 鍊欓�変綅缃唴閮ㄧ被
+ */
+ private static class CandidatePosition {
+ final String position;
+ final int distanceToConflict;
+ final int distanceToCurrent;
+
+ CandidatePosition(String position, int distanceToConflict, int distanceToCurrent) {
+ this.position = position;
+ this.distanceToConflict = distanceToConflict;
+ this.distanceToCurrent = distanceToCurrent;
+ }
+ }
+
+ /**
+ * 瑙勫垝鍒扮瓑寰呬綅缃殑璺緞
+ */
+ private PlannedPath planPathToWaitPosition(String agvId, String currentPosition,
+ String waitPosition, AGVStatus agvStatus,
+ String originalSegId, ExecutingTask task,
+ long unifiedTimestamp) {
+ if (pathPlanner == null) {
+ return null;
+ }
+
+ CTUPhysicalConfig config = agvStatus != null && agvStatus.getPhysicalConfig() != null
+ ? agvStatus.getPhysicalConfig()
+ : new CTUPhysicalConfig();
+
+ try {
+ PlannedPath path = pathPlanner.planPath(currentPosition, waitPosition);
+ if (path != null) {
+ path.setAgvId(agvId);
+
+ String segId;
+ if (originalSegId != null && !originalSegId.isEmpty()) {
+ segId = originalSegId;
+ } else if (task != null && task.getTaskId() != null) {
+ segId = task.getTaskId() + "_" + agvId + "_" + (task.getTaskType() != null ? task.getTaskType() : "waiting");
+ } else {
+ segId = "UNKNOWN_" + agvId + "_waiting";
+ System.out.println("璀﹀憡: AGV " + agvId + " 鏃犳硶鑾峰彇浠诲姟淇℃伅锛屼娇鐢ㄩ粯璁egId: " + segId);
+ }
+ path.setSegId(segId);
+
+ // 浣跨敤缁熶竴鏃堕棿鎴筹紙杞崲涓烘绉掞級
+ long startTimeMs = unifiedTimestamp * 1000;
+ if (timeCalculator != null && path.getCodeList() != null) {
+ timeCalculator.calculatePathTiming(path, startTimeMs, config, 0.0);
+ }
+
+ if (!path.getCodeList().isEmpty()) {
+ PathCode lastCode = path.getCodeList().get(path.getCodeList().size() - 1);
+ lastCode.setTargetPoint(true);
+ lastCode.setActionType(null);
+ lastCode.setPosType(null);
+ }
+
+ return path;
+ }
+ } catch (Exception e) {
+ System.out.println("瑙勫垝鍒扮瓑寰呬綅缃殑璺緞澶辫触: " + e.getMessage());
+ }
+
+ return null;
+ }
+
+ /**
+ * 鎴柇璺緞鍒扮瓑寰呬綅缃�
+ *
+ * @param path 璺緞
+ * @param waitPosition 绛夊緟浣嶇疆
+ */
+ private void truncatePathToWaitPosition(PlannedPath path, String waitPosition) {
+ if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) {
+ return;
+ }
+
+ List<PathCode> codeList = path.getCodeList();
+ List<PathCode> truncatedList = new ArrayList<>();
+
+ int waitIndex = -1;
+ for (int i = 0; i < codeList.size(); i++) {
+ if (codeList.get(i).getCode().equals(waitPosition)) {
+ waitIndex = i;
+ break;
+ }
+ }
+
+ if (waitIndex >= 0) {
+ truncatedList.addAll(codeList.subList(0, waitIndex + 1));
+
+ if (!truncatedList.isEmpty()) {
+ PathCode lastCode = truncatedList.get(truncatedList.size() - 1);
+ lastCode.setTargetPoint(true);
+ lastCode.setActionType(null);
+ lastCode.setPosType(null);
+ }
+
+ path.setCodeList(truncatedList);
+ System.out.println("璺緞宸叉埅鏂埌绛夊緟浣嶇疆 " + waitPosition);
+ }
+ }
+
+ /**
+ * 鎴柇璺緞鍒板綋鍓嶄綅缃紙
+ */
+ private void truncatePathToCurrentPosition(PlannedPath path, String currentPosition) {
+ if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) {
+ return;
+ }
+
+ List<PathCode> codeList = path.getCodeList();
+ List<PathCode> truncatedList = new ArrayList<>();
+
+ int currentIndex = -1;
+ for (int i = 0; i < codeList.size(); i++) {
+ if (codeList.get(i).getCode().equals(currentPosition)) {
+ currentIndex = i;
+ break;
+ }
+ }
+
+ if (currentIndex >= 0) {
+ truncatedList.addAll(codeList.subList(0, currentIndex + 1));
+
+ if (!truncatedList.isEmpty()) {
+ PathCode lastCode = truncatedList.get(truncatedList.size() - 1);
+ lastCode.setTargetPoint(true);
+ lastCode.setActionType(null);
+ lastCode.setPosType(null);
+ }
+
+ path.setCodeList(truncatedList);
+ System.out.println("璺緞宸叉埅鏂埌褰撳墠浣嶇疆 " + currentPosition);
+ }
+ }
+
+ /**
+ * 璇勪及AGV浼樺厛绾�
*
* @param agvId AGV缂栧彿
* @param path 璺緞
@@ -257,7 +899,7 @@
}
/**
- * 涓鸿矾寰勬坊鍔犲欢杩燂紙澧炲己鐗堬紝姝ｇ‘閲嶈绠楁椂闂寸獥锛�
+ * 涓鸿矾寰勬坊鍔犲欢杩�
*
* @param path 璺緞
* @param timeStep 寤惰繜寮�濮嬬殑鏃堕棿姝�
diff --git a/algo-zkd/src/main/java/com/algo/service/ExecutingTaskExtractor.java b/algo-zkd/src/main/java/com/algo/service/ExecutingTaskExtractor.java
index 233b0f0..7ab0b75 100644
--- a/algo-zkd/src/main/java/com/algo/service/ExecutingTaskExtractor.java
+++ b/algo-zkd/src/main/java/com/algo/service/ExecutingTaskExtractor.java
@@ -106,17 +106,38 @@
*/
private List<ExecutingTask> extractAgvTasks(AGVStatus agvStatus) {
List<ExecutingTask> tasks = new ArrayList<>();
-
- // 妫�鏌GV鏄惁鍙敤
- if (!agvStatus.isAvailable()) {
- return tasks;
- }
+
+ 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;
}
+
+ // 妫�鏌ユ槸鍚︽湁宸茶杞界殑浠诲姟锛堝嵆浣緼GV涓嶅彲鐢紝濡傛灉鏈夊凡瑁呰浇浠诲姟涔熼渶瑕佽鍒掕矾寰勶級
+ 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;
+ }
+
+ // 濡傛灉鏈夊凡瑁呰浇浠诲姟浣咥GV涓嶅彲鐢紝璁板綍鏃ュ織浣嗙户缁鐞�
+ if (!agvStatus.isAvailable() && hasLoadedTasks) {
+ System.out.println(" [璋冭瘯] AGV " + agvStatus.getAgvId() + " 涓嶅彲鐢ㄤ絾鏈夊凡瑁呰浇浠诲姟锛岀户缁彁鍙栦换鍔�");
+ }
+
+ System.out.println(" [璋冭瘯] AGV " + agvStatus.getAgvId() + " 鑳岀瘬鏁伴噺: " + backpack.size());
// 鍒嗗埆鏀堕泦宸茶杞藉拰鏈杞界殑浠诲姟
List<BackpackData> loadedTasks = new ArrayList<>();
@@ -124,21 +145,39 @@
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(" [璋冭瘯] 璀﹀憡锛歵askId=" + bp.getTaskId() +
+ " 鍦╰askDataMap涓湭鎵惧埌锛�");
+ System.out.println(" [璋冭瘯] taskDataMap鍖呭惈鐨刱ey: " + 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;
@@ -157,28 +196,47 @@
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(" [璋冭瘯] 璀﹀憡锛歍askData鎴杄nd涓簄ull锛岃烦杩�");
}
}
// 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(" [璋冭瘯] 璀﹀憡锛氳窛绂讳负璐燂紝璺宠繃姝ゅ彇璐т换鍔�");
}
}
}
@@ -196,7 +254,9 @@
}
// 4. 閫夋嫨鎴愭湰鏈�浣庣殑浠诲姟
+ System.out.println(" [璋冭瘯] 鎬讳换鍔￠�夐」鏁伴噺: " + taskOptions.size());
if (taskOptions.isEmpty()) {
+ System.out.println(" [璋冭瘯] 璀﹀憡锛氭病鏈夊彲閫変换鍔★紝杩斿洖null");
return null;
}
@@ -409,17 +469,33 @@
*/
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;
}
- return JsonUtils.calculateManhattanDistance(coord1, coord2);
+ double distance = JsonUtils.calculateManhattanDistance(coord1, coord2);
+ System.out.println(" [璋冭瘯] 鏇煎搱椤胯窛绂�: " + distance + " (" + pos1 + " -> " + pos2 + ")");
+ return distance;
}
/**
diff --git a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
index 54b0e54..b728d19 100644
--- a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
+++ b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
@@ -84,8 +84,10 @@
Map<String, double[]> realCoordinateMapping = JsonUtils.loadRealCoordinateMapping("man_code.json");
this.timeCalculator = new PathTimeCalculator(envDataConfig.getPathMapping(), realCoordinateMapping);
- // 涓虹鎾炶В鍐冲櫒璁剧疆鏃堕棿璁＄畻鍣�
+ // 涓虹鎾炶В鍐冲櫒璁剧疆鏃堕棿璁＄畻鍣ㄣ�佽矾寰勮鍒掑櫒鍜岃矾寰勬槧灏勮〃
this.collisionResolver.setTimeCalculator(timeCalculator);
+ this.collisionResolver.setPathPlanner((PathPlanner) pathPlanner);
+ this.collisionResolver.setPathMapping(envDataConfig.getPathMapping());
}
/**
@@ -127,7 +129,6 @@
// 3. 鍒嗙被澶勭悊CTU锛氭湁鍓╀綑璺緞鐨勫拰闇�瑕佹柊璺緞鐨�
List<AGVStatus> ctuWithRemainingPaths = new ArrayList<>();
List<AGVStatus> ctuNeedingNewPaths = new ArrayList<>();
- List<AGVStatus> unhandledAgvs = new ArrayList<>(); // 鏈鐞嗙殑AGV
for (AGVStatus agv : agvStatusList) {
// 鏁呴殰杞﹀凡鍦ㄦ椂绌哄崰鐢ㄨ〃涓鐞嗭紝鏍囪涓哄凡澶勭悊浣嗕笉瑙勫垝璺緞
@@ -137,32 +138,27 @@
}
if (agv.hasRemainingPath()) {
+ // 鏈夊墿浣欒矾寰勭殑AGV锛屽厛澶勭悊鍓╀綑璺緞
ctuWithRemainingPaths.add(agv);
- } else if (agv.canAcceptNewTask()) {
- // 鍙互鎺ュ彈鏂颁换鍔＄殑AGV锛堟湁浠诲姟鎴栧彲浠ユ帴鍙楁柊浠诲姟锛�
- ctuNeedingNewPaths.add(agv);
- } else if (includeIdleAgv && isAgvIdle(agv)) {
- // 瀹屽叏绌洪棽鐨凙GV锛堟棤浠诲姟锛屾棤鍓╀綑璺緞锛�
- ctuNeedingNewPaths.add(agv);
} else {
- // 鍏朵粬鎯呭喌锛氫笉鍙敤銆佹湁鍓╀綑璺緞浣�>1銆佹湁浠诲姟浣嗕笉鍙敤绛�
- unhandledAgvs.add(agv);
+ // 鍙涓嶆槸鏁呴殰杞︿笖娌℃湁鍓╀綑璺緞锛屽弬涓庤矾寰勮鍒�
+ ctuNeedingNewPaths.add(agv);
}
}
System.out.println("CTU鍒嗙被瀹屾垚: 鏈夊墿浣欒矾寰�=" + ctuWithRemainingPaths.size() +
- ", 闇�瑕佹柊璺緞=" + ctuNeedingNewPaths.size() +
- ", 鏈鐞�=" + unhandledAgvs.size());
+ ", 闇�瑕佹柊璺緞=" + ctuNeedingNewPaths.size());
- // 璁板綍鏈鐞嗙殑AGV淇℃伅锛坉ebug锛�
- if (!unhandledAgvs.isEmpty()) {
- for (AGVStatus agv : unhandledAgvs) {
+ // 璁板綍闇�瑕佹柊璺緞鐨凙GV淇℃伅锛坉ebug锛�
+ if (!ctuNeedingNewPaths.isEmpty()) {
+ System.out.println("闇�瑕佹柊璺緞鐨凙GV璇︽儏:");
+ for (AGVStatus agv : ctuNeedingNewPaths) {
System.out.println(" - AGV " + agv.getAgvId() +
": error=" + agv.getError() +
+ ", status=" + agv.getStatus() +
", hasRemainingPath=" + agv.hasRemainingPath() +
- ", canAcceptNewTask=" + agv.canAcceptNewTask() +
- ", isIdle=" + isAgvIdle(agv) +
- ", isAvailable=" + agv.isAvailable());
+ ", isAvailable=" + agv.isAvailable() +
+ ", position=" + agv.getPosition());
}
}
@@ -308,7 +304,7 @@
if (!conflicts.isEmpty()) {
System.out.println("鍙戠幇 " + conflicts.size() + " 涓鎾烇紝寮�濮嬭В鍐�");
- plannedPaths = collisionResolver.resolveConflicts(plannedPaths, conflicts, executingTasks);
+ plannedPaths = collisionResolver.resolveConflicts(plannedPaths, conflicts, executingTasks, agvStatusList, unifiedTimestamp);
// 閲嶆柊妫�娴嬪啿绐�
List<Conflict> remainingConflicts = collisionDetector.detectConflicts(plannedPaths);
--
Gitblit v1.9.1