A*算法，适应多终点类型

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<State> path) {
+    }
+
+    public TryFindPathResult findPathList(String startNodeId, LCCDirection startDirection, List<PtrPathPlanner.NodeDirection> endList) {
+        // 尝试所有起点和终点组合
+        for (PtrPathPlanner.NodeDirection end : endList) {
+            try {
+                List<State> 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<State> findPath(String startId, LCCDirection startDirection, String endId, LCCDirection endDirection) {
         Node start = graph.getNode(startId);

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<NodeDirection> 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<NodeDirection> unloadNodeDirectionList = findNodeForStore(unloadRackId, unloadBay);
+        if (unloadNodeDirectionList.isEmpty()) {
             throw new RuntimeException("Drop node not found for rackId=" + unloadRackId + ", bay=" + unloadBay);
         }
 
         // 规划到取货点路径
-        List<State> 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<State> 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<State> 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<State> 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<NodeDirection> findNodeForStore(String storeId, int bay) {
         List<Node> nodes = this.graph.getNodesForStore(storeId);
+        List<NodeDirection> 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) {
     }
 }