diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java index 05a1d25..0d26e95 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java @@ -2,11 +2,13 @@ package com.galaxis.rcs.plan.path; import com.galaxis.rcs.common.enums.LCCDirection; import com.google.common.collect.Maps; +import lombok.extern.slf4j.Slf4j; import java.util.*; import static com.galaxis.rcs.plan.path.PathUtils.*; +@Slf4j public class AStarPathPlanner { private static final float ROTATION_COST_PER_DEGREE = 0.01f; private static final float BLOCKED_COST = 10000f; // 阻塞系数成本 @@ -20,6 +22,29 @@ public class AStarPathPlanner { this.graph = graph; } + public record TryFindPathResult( + String startNodeId, + LCCDirection startDirection, + PtrPathPlanner.NodeDirection end, + List path) { + } + + public TryFindPathResult findPathList(String startNodeId, LCCDirection startDirection, List endList) { + // 尝试所有起点和终点组合 + for (PtrPathPlanner.NodeDirection end : endList) { + try { + List path = findPath(startNodeId, startDirection, end.node().id(), end.direction()); + return new TryFindPathResult(startNodeId, startDirection, end, path); + } catch (PathNotFoundException e) { + // 继续尝试下一个组合 + log.warn("Path not found from {} to {}: {}", startNodeId + "(" + startDirection.toString() + ")", end, e.getMessage()); + } + } + throw new RuntimeException( + "No valid path found for any combination of start and end nodes." + ); + } + // 路径规划状态 public List findPath(String startId, LCCDirection startDirection, String endId, LCCDirection endDirection) { Node start = graph.getNode(startId); diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java index bc609d0..3f7b784 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java @@ -4,6 +4,7 @@ import com.galaxis.rcs.common.enums.LCCDirection; import com.galaxis.rcs.plan.PlanTaskSequence; import com.galaxis.rcs.plan.task.*; import com.google.common.base.Strings; +import com.google.common.collect.Lists; import com.yvan.logisticsModel.StaticItem; import org.clever.core.Conv; @@ -76,21 +77,24 @@ public class PtrPathPlanner { // 取货点 String loadRackId = task.from().rackId(); int pickupBay = task.from().bay(); - NodeDirection loadNodeDirection = findNodeForStore(loadRackId, pickupBay); - if (loadNodeDirection == null) { + List loadNodeDirectionList = findNodeForStore(loadRackId, pickupBay); + if (loadNodeDirectionList.isEmpty()) { throw new RuntimeException("Pickup node not found for rackId=" + loadRackId + ", bay=" + pickupBay); } // 放货点 String unloadRackId = task.to().rackId(); int unloadBay = task.to().bay(); - NodeDirection unloadNodeDirection = findNodeForStore(unloadRackId, unloadBay); - if (unloadNodeDirection == null) { + List unloadNodeDirectionList = findNodeForStore(unloadRackId, unloadBay); + if (unloadNodeDirectionList.isEmpty()) { throw new RuntimeException("Drop node not found for rackId=" + unloadRackId + ", bay=" + unloadBay); } // 规划到取货点路径 - List toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction()); + var toLoadPathResult = astar.findPathList(startNodeId, startDirection, loadNodeDirectionList); + + var toLoadPath = toLoadPathResult.path(); + var loadNodeDirection = toLoadPathResult.end(); // 检查方向是否匹配,如果不匹配则插入旋转点 if (!toLoadPath.isEmpty()) { @@ -119,7 +123,8 @@ public class PtrPathPlanner { } // 规划到放货点路径 - List toUnloadPath = astar.findPath(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirection.node().id(), unloadNodeDirection.direction()); + var toUnloadPathResult = astar.findPathList(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirectionList); + var toUnloadPath = toUnloadPathResult.path(); // 生成指令序列 generateMoves(plan, toLoadPath); @@ -138,13 +143,14 @@ public class PtrPathPlanner { // 放货点 String unloadRackId = task.to().rackId(); int unloadBay = task.to().bay(); - NodeDirection unloadNodeDirection = findNodeForStore(unloadRackId, unloadBay); - if (unloadNodeDirection == null) { + var unloadNodeDirectionList = findNodeForStore(unloadRackId, unloadBay); + if (unloadNodeDirectionList.isEmpty()) { throw new RuntimeException("Drop node not found for rackId=" + unloadRackId + ", bay=" + unloadBay); } // 规划到放货点路径 - List toUnloadPath = astar.findPath(startNodeId, startDirection, unloadNodeDirection.node().id(), unloadNodeDirection.direction()); + var toUnloadPathList = astar.findPathList(startNodeId, startDirection, unloadNodeDirectionList); + var toUnloadPath = toUnloadPathList.path(); // 检查方向是否匹配,如果不匹配则插入旋转点 // State lastState = toUnloadPath.get(toUnloadPath.size() - 1); @@ -184,13 +190,14 @@ public class PtrPathPlanner { // 放货点 String loadRackId = task.to().rackId(); int unloadBay = task.to().bay(); - NodeDirection loadNodeDirection = findNodeForStore(loadRackId, unloadBay); - if (loadNodeDirection == null) { + var loadNodeDirectionList = findNodeForStore(loadRackId, unloadBay); + if (loadNodeDirectionList.isEmpty()) { throw new RuntimeException("Drop node not found for rackId=" + loadRackId + ", bay=" + unloadBay); } // 规划到取货点路径 - List toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction()); + var toLoadPathList = astar.findPathList(startNodeId, startDirection, loadNodeDirectionList); + var toLoadPath = toLoadPathList.path(); // 检查方向是否匹配,如果不匹配则插入旋转点 // State lastState = toLoadPath.get(toLoadPath.size() - 1); @@ -223,17 +230,18 @@ public class PtrPathPlanner { plan.addFinish(); } - private NodeDirection findNodeForStore(String storeId, int bay) { + private List findNodeForStore(String storeId, int bay) { List nodes = this.graph.getNodesForStore(storeId); + List results = Lists.newArrayList(); for (Node node : nodes) { for (StoreLink link : node.storeLinks()) { if (link.storeId().equals(storeId) && link.bay() == bay) { LCCDirection agvDirection = convertStoreDirection(link.direction()); - return new NodeDirection(node, agvDirection); + results.add(new NodeDirection(node, agvDirection)); } } } - return null; + return results; } /** @@ -273,6 +281,6 @@ public class PtrPathPlanner { // 辅助记录类 - private record NodeDirection(Node node, LCCDirection direction) { + public record NodeDirection(Node node, LCCDirection direction) { } }