PtrPathPlanner 算法性能优化

servo/src/main/java/com/galaxis/rcs/common/enums/LCCDirection.java

@@ -1,7 +1,7 @@
 package com.galaxis.rcs.common.enums;
 
 /**
- * 路径点与货位 / 充电器关联记录
+ * 方向枚举
  */
 public enum LCCDirection {
     /**

servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java

@@ -1,128 +1,180 @@
 package com.galaxis.rcs.plan.path2;
 
 import com.galaxis.rcs.common.enums.LCCDirection;
+import com.google.common.collect.Maps;
 
 import java.util.*;
 
+import static com.galaxis.rcs.plan.path2.PathUtils.*;
+
 public class AStarPathPlanner {
     private static final float ROTATION_COST_PER_DEGREE = 0.1f;
+    private static final float BLOCKED_COST = 10000f; // 阻塞系数成本
+    private static final float WEIGHT_FACTOR = 1.5f; // 权重因子
+
     private final NavigationGraph graph;
+    private final Map<String, Float> nodeWeights = Maps.newConcurrentMap();
+    private final Map<String, Float> blockedNodes = Maps.newConcurrentMap();
 
     public AStarPathPlanner(NavigationGraph graph) {
         this.graph = graph;
     }
 
     // 路径规划状态
-    public List<Node> findPath(String startId, LCCDirection startDirectionAngle, String endId, LCCDirection endDirectionAngle) {
+    public List<State> findPath(String startId, LCCDirection startDirection, String endId, LCCDirection endDirection) {
         Node start = graph.getNode(startId);
-        Node goal = graph.getNode(endId);
-        if (start == null || goal == null) return Collections.emptyList();
+        Node end = graph.getNode(endId);
+        if (start == null) {
+            throw new RuntimeException("Start node not found: " + startId);
+        }
+        if (end == null) {
+            throw new RuntimeException("End node not found: " + endId);
+        }
 
         // 使用复合键避免状态重复
         Map<String, State> visited = new HashMap<>();
         PriorityQueue<State> open = new PriorityQueue<>();
 
         // 初始状态
-        State initialState = new State(start, startDirectionAngle, 0, heuristic(start, goal), null);
+        State initialState = new State(start, startDirection, 0, heuristic(start, end), null);
         open.add(initialState);
-        visited.put(stateKey(start.id(), startDirectionAngle), initialState);
+        visited.put(stateKey(start.id(), startDirection), initialState);
 
         while (!open.isEmpty()) {
             State current = open.poll();
 
             // 到达目标节点且方向匹配
             if (current.node().id().equals(endId) &&
-                current.directionAngle() == endDirectionAngle) {
+                current.direction() == endDirection) {
                 return buildPath(current);
             }
 
-            // 处理邻居移动
+            // 处理邻边移动
             for (Node neighbor : graph.getNeighbors(current.node())) {
-                // 计算可能的两种方向（前进/后退）
-                LCCDirection[] possibleHeadings;
-                if (current.directionAngle() == LCCDirection.UP || current.directionAngle() == LCCDirection.DOWN) {
-                    possibleHeadings = new LCCDirection[]{LCCDirection.UP, LCCDirection.DOWN};
-                } else {
-                    possibleHeadings = new LCCDirection[]{LCCDirection.LEFT, LCCDirection.RIGHT};
-                }
-
-                for (LCCDirection nextHeading : possibleHeadings) {
-                    float moveCost = graph.distance(current.node(), neighbor);
-                    considerState(current, neighbor, nextHeading, moveCost, open, visited, goal);
-                }
-            }
-
-            // 处理旋转（仅在可旋转节点）
+                // 检查节点是否被阻塞
+                if (isBlocked(neighbor.id())) continue;
+
+                LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor);
+
+                // 前进所需方向
+                LCCDirection forwardHeading = moveDirection;
+                // 后退所需方向
+                LCCDirection backwardHeading = getOppositeDirection(moveDirection);
+
+                // 尝试前进
+                if (current.direction() == forwardHeading) {
+                    float moveCost = calculateMoveCost(current.node(), neighbor);
+                    considerState(current, neighbor, forwardHeading,
+                        moveCost, open, visited, end);
+                } else if (current.node().rotatable()) {
+                    float rotationCost = calculateRotationCost(
+                        current.direction(), forwardHeading, ROTATION_COST_PER_DEGREE
+                    );
+                    float moveCost = calculateMoveCost(current.node(), neighbor);
+                    float totalCost = rotationCost + moveCost;
+                    considerState(current, neighbor, forwardHeading,
+                        totalCost, open, visited, end);
+                }
+
+                // 尝试后退
+                if (current.direction() == backwardHeading) {
+                    float moveCost = calculateMoveCost(current.node(), neighbor);
+                    considerState(current, neighbor, backwardHeading,
+                        moveCost, open, visited, end);
+                } else if (current.node().rotatable()) {
+                    float rotationCost = calculateRotationCost(
+                        current.direction(), backwardHeading, ROTATION_COST_PER_DEGREE
+                    );
+                    float moveCost = calculateMoveCost(current.node(), neighbor);
+                    float totalCost = rotationCost + moveCost;
+                    considerState(current, neighbor, backwardHeading,
+                        totalCost, open, visited, end);
+                }
+            }
+
+            // 处理原地旋转
             if (current.node().rotatable()) {
-                for (LCCDirection rotation : new LCCDirection[]{LCCDirection.UP, LCCDirection.DOWN, LCCDirection.LEFT, LCCDirection.RIGHT}) {
-                    if (rotation == current.directionAngle()) continue;
-
-                    // 计算旋转代价
-                    int angleDiff = 0;
-                    if (current.directionAngle() == LCCDirection.UP && rotation == LCCDirection.RIGHT) {
-                        angleDiff = 90;
-                    } else if (current.directionAngle() == LCCDirection.RIGHT && rotation == LCCDirection.DOWN) {
-                        angleDiff = 90;
-                    } else if (current.directionAngle() == LCCDirection.DOWN && rotation == LCCDirection.LEFT) {
-                        angleDiff = 90;
-                    } else if (current.directionAngle() == LCCDirection.LEFT && rotation == LCCDirection.UP) {
-                        angleDiff = 90;
-                    } else if (current.directionAngle() == rotation) {
-                        // 无需旋转
-                        continue;
-                    } else {
-                        angleDiff = 180; // 反向旋转
-                    }
-                    float rotationCost = angleDiff * ROTATION_COST_PER_DEGREE;
+                for (LCCDirection rotation : LCCDirection.values()) {
+                    if (rotation == current.direction()) continue;
 
-                    considerState(current, current.node(), rotation, rotationCost, open, visited, goal);
+                    float rotationCost = calculateRotationCost(
+                        current.direction(), rotation, ROTATION_COST_PER_DEGREE
+                    );
+                    considerState(current, current.node(), rotation,
+                        rotationCost, open, visited, end);
                 }
             }
         }
         return Collections.emptyList();
     }
 
-    private void considerState(State current, Node nextNode, LCCDirection nextHeading,
+    /**
+     * 考虑新的状态并更新开放列表和访问记录
+     *
+     * @param current       当前状态
+     * @param nextNode      下一个节点
+     * @param nextDirection 下一个方向
+     * @param cost          移动成本
+     * @param open          开放列表
+     * @param visited       访问记录
+     * @param end           目标节点
+     */
+    private void considerState(State current, Node nextNode, LCCDirection nextDirection,
                                float cost, PriorityQueue<State> open,
-                               Map<String, State> visited, Node goal) {
-        String key = stateKey(nextNode.id(), nextHeading);
+                               Map<String, State> visited, Node end) {
+        String key = stateKey(nextNode.id(), nextDirection);
         float newG = current.g() + cost;
 
         if (!visited.containsKey(key) || visited.get(key).g() > newG) {
-            float h = heuristic(nextNode, goal);
-            State newState = new State(nextNode, nextHeading, newG, h, current);
+            float h = heuristic(nextNode, end);
+            State newState = new State(nextNode, nextDirection, newG, h, current);
             open.add(newState);
             visited.put(key, newState);
         }
     }
 
-    private List<Node> buildPath(State state) {
-        LinkedList<Node> path = new LinkedList<>();
+    private List<State> buildPath(State state) {
+        LinkedList<State> path = new LinkedList<>();
         while (state != null) {
-            path.addFirst(state.node());
+            path.addFirst(state);
             state = state.parent();
         }
         return path;
     }
 
+    /**
+     * 启发式函数，计算两个节点之间的距离
+     */
     private float heuristic(Node a, Node b) {
         return graph.distance(a, b);
-    }
-
-    private String stateKey(String nodeId, LCCDirection directionAngle) {
-        return nodeId + "|" + directionAngle;
+        // 使用曼哈顿距离？？
+        // return Math.abs(a.x() - b.x()) + Math.abs(a.z() - b.z());
     }
 
     /**
-     * 根据方向要求计算目标方向
+     * 生成状态的唯一键
      */
-    public static float getRequiredDirection(LCCDirection direction) {
-        return switch (direction) {
-            case UP -> 90f;     // 车头朝上
-            case DOWN -> 270f;  // 车头朝下
-            case LEFT -> 180f;  // 车头朝左
-            case RIGHT -> 0f;   // 车头朝右
-            default -> 0;        // 默认为右
-        };
+    private String stateKey(String nodeId, LCCDirection direction) {
+        return nodeId + "|" + direction;
+    }
+
+    public void setNodeWeight(String nodeId, float weight) {
+        nodeWeights.put(nodeId, weight);
+    }
+
+    public void setBlocked(String nodeId, float blockedFactor) {
+        blockedNodes.put(nodeId, blockedFactor);
+    }
+
+    private float calculateMoveCost(Node from, Node to) {
+        float baseCost = graph.distance(from, to);
+        float weight = nodeWeights.getOrDefault(to.id(), 1.0f);
+        float blockedFactor = blockedNodes.getOrDefault(to.id(), 0f);
+
+        return baseCost * weight * (1 + blockedFactor);
+    }
+
+    private boolean isBlocked(String nodeId) {
+        return blockedNodes.containsKey(nodeId) && blockedNodes.get(nodeId) > 0.8f;
     }
 }

servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java

@@ -1,16 +1,35 @@
 package com.galaxis.rcs.plan.path2;
 
 import com.galaxis.rcs.common.enums.LCCDirection;
+import com.google.common.collect.Maps;
 import com.yvan.logisticsModel.LogisticsRuntime;
 import com.yvan.logisticsModel.StaticItem;
+import lombok.extern.slf4j.Slf4j;
 
 import java.util.*;
 
 /**
  * A* 导航图
  */
+@Slf4j
 public class NavigationGraph {
     private final LogisticsRuntime runtime;
+
+    /**
+     * 缓存距离计算结果，避免重复计算
+     */
+    private final Map<String, Float> distanceCache = Maps.newConcurrentMap();
+
+    /**
+     * 缓存邻居节点列表，避免重复查询
+     */
+    private final Map<String, List<Node>> neighborCache = Maps.newConcurrentMap();
+
+    /**
+     * 添加路径缓存
+     */
+    private final Map<String, List<State>> pathCache = Maps.newConcurrentMap();
+
     private final Map<String, Node> nodeMap = new HashMap<>();
     private final Map<String, List<String>> storeToNodes = new HashMap<>();
 
@@ -18,6 +37,47 @@ public class NavigationGraph {
         this.runtime = runtime;
     }
 
+    public List<State> getCachedPath(String startId, LCCDirection startDirection,
+                                     String endId, LCCDirection endDirection) {
+        String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection;
+        return pathCache.get(cacheKey);
+    }
+
+    public void cachePath(String startId, LCCDirection startDirection,
+                          String endId, LCCDirection endDirection,
+                          List<State> path) {
+        String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection;
+        pathCache.put(cacheKey, path);
+    }
+
+    public List<Node> getNodesForStore(String storeId) {
+        List<Node> nodes = new ArrayList<>();
+        List<String> nodeIds = storeToNodes.get(storeId);
+        if (nodeIds != null) {
+            for (String id : nodeIds) {
+                Node node = nodeMap.get(id);
+                if (node != null) {
+                    nodes.add(node);
+                }
+            }
+        }
+        return nodes;
+    }
+
+    public List<Node> getNeighbors(Node node) {
+        return node.neighbors().stream()
+            .map(nodeMap::get)
+            .filter(Objects::nonNull)
+            .toList();
+    }
+
+    public float distance(Node a, Node b) {
+        float dx = a.x() - b.x();
+        float dz = a.z() - b.z();
+        return (float) Math.sqrt(dx * dx + dz * dz);
+    }
+
+
     public void init() {
         var items = runtime.getStaticItems();
         for (StaticItem item : items) {
@@ -32,6 +92,9 @@ public class NavigationGraph {
 
                 // 检查可旋转性
                 boolean rotatable = dt.containsKey("agvRotation");
+                if (rotatable) {
+                    log.info("Node {} is rotatable", id);
+                }
 
                 // 提取邻居节点
                 List<String> in = (List<String>) dt.get("in");
@@ -77,31 +140,4 @@ public class NavigationGraph {
     public Node getNode(String id) {
         return nodeMap.get(id);
     }
-
-    public List<Node> getNodesForStore(String storeId) {
-        List<Node> nodes = new ArrayList<>();
-        List<String> nodeIds = storeToNodes.get(storeId);
-        if (nodeIds != null) {
-            for (String id : nodeIds) {
-                Node node = nodeMap.get(id);
-                if (node != null) {
-                    nodes.add(node);
-                }
-            }
-        }
-        return nodes;
-    }
-
-    public List<Node> getNeighbors(Node node) {
-        return node.neighbors().stream()
-            .map(nodeMap::get)
-            .filter(Objects::nonNull)
-            .toList();
-    }
-
-    public float distance(Node a, Node b) {
-        float dx = a.x() - b.x();
-        float dz = a.z() - b.z();
-        return (float) Math.sqrt(dx * dx + dz * dz);
-    }
 }

servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java

@@ -0,0 +1,149 @@
+package com.galaxis.rcs.plan.path2;
+
+import com.galaxis.rcs.common.enums.LCCDirection;
+
+import java.util.*;
+
+public class PathUtils {
+    /**
+     * 计算移动方向
+     */
+    public static LCCDirection calculateMoveDirection(Node from, Node to) {
+        float dx = to.x() - from.x();
+        float dz = to.z() - from.z();
+
+        if (Math.abs(dx) > Math.abs(dz)) {
+            return dx > 1 ? LCCDirection.RIGHT : LCCDirection.LEFT;
+        } else {
+            return dz > 1 ? LCCDirection.DOWN : LCCDirection.UP;
+        }
+    }
+
+    /**
+     * 检查方向是否有效（侧插式专用）
+     */
+    public static boolean isValidForSideLoader(Node node, int bay, LCCDirection direction) {
+        Optional<StoreLink> link = node.storeLinks().stream()
+            .filter(l -> l.bay() == bay)
+            .findFirst();
+
+        if (link.isEmpty()) return false;
+
+        LCCDirection requiredDirection = convertForSideLoader(link.get().direction());
+        return direction == requiredDirection;
+    }
+
+    /**
+     * 转换货位方向到AGV所需方向（侧插式专用）
+     */
+    public static LCCDirection convertForSideLoader(LCCDirection storeDirection) {
+        /*
+         * 侧插式AGV方向规则：
+         * - 货位在上方 → 车头向右（货叉朝左）
+         * - 货位在下方 → 车头向左（货叉朝左）
+         * - 货位在左方 → 车头向上（货叉朝左）
+         * - 货位在右方 → 车头向下（货叉朝左）
+         */
+        return switch (storeDirection) {
+            case UP -> LCCDirection.RIGHT;
+            case DOWN -> LCCDirection.LEFT;
+            case LEFT -> LCCDirection.UP;
+            case RIGHT -> LCCDirection.DOWN;
+        };
+    }
+
+    /**
+     * 获取最近的旋转点
+     */
+    public static Node findNearestRotationNode(NavigationGraph graph, Node from) {
+        // 使用BFS查找最近的旋转点
+        Queue<Node> queue = new LinkedList<>();
+        Set<String> visited = new HashSet<>();
+        queue.add(from);
+        visited.add(from.id());
+
+        while (!queue.isEmpty()) {
+            Node current = queue.poll();
+            if (current.rotatable()) {
+                return current;
+            }
+
+            for (Node neighbor : graph.getNeighbors(current)) {
+                if (!visited.contains(neighbor.id())) {
+                    visited.add(neighbor.id());
+                    queue.add(neighbor);
+                }
+            }
+        }
+        return null;
+    }
+
+    /**
+     * 获取相反方向
+     */
+    public static LCCDirection getOppositeDirection(LCCDirection dir) {
+        return switch (dir) {
+            case UP -> LCCDirection.DOWN;
+            case DOWN -> LCCDirection.UP;
+            case LEFT -> LCCDirection.RIGHT;
+            case RIGHT -> LCCDirection.LEFT;
+        };
+    }
+
+    /**
+     * 计算旋转代价
+     */
+    public static float calculateRotationCost(LCCDirection from, LCCDirection to, float ROTATION_COST_PER_DEGREE) {
+        float angle1 = getRequiredDirection(from);
+        float angle2 = getRequiredDirection(to);
+        float diff = Math.abs(angle1 - angle2);
+        diff = Math.min(diff, 360 - diff);
+        return diff * ROTATION_COST_PER_DEGREE;
+    }
+
+    /**
+     * 获取所需方向的角度
+     */
+    public static float getRequiredDirection(LCCDirection direction) {
+        return switch (direction) {
+            case UP -> 90f;
+            case DOWN -> 270f;
+            case LEFT -> 180f;
+            case RIGHT -> 0f;
+        };
+    }
+
+    /**
+     * 转换货位方向到AGV所需方向
+     */
+    public static LCCDirection convertStoreDirection(LCCDirection storeDirection) {
+        // 转换规则: 货位在路径点的方位 -> AGV所需方向
+        return switch (storeDirection) {
+            case UP -> LCCDirection.RIGHT;    // 货位在上方 → 车头向右
+            case DOWN -> LCCDirection.LEFT;    // 货位在下方 → 车头向左
+            case LEFT -> LCCDirection.UP;      // 货位在左方 → 车头向上
+            case RIGHT -> LCCDirection.DOWN;   // 货位在右方 → 车头向下
+        };
+    }
+
+    /**
+     * 将角度转换为 LCCDirection
+     */
+    public static LCCDirection convertAngleToDirection(float angle) {
+        // 标准化角度
+        angle = ((angle % 360f) + 360f) % 360f;
+
+        if (angle >= 315f || angle < 45f) return LCCDirection.RIGHT;
+        if (angle >= 45f && angle < 135f) return LCCDirection.UP;
+        if (angle >= 135f && angle < 225f) return LCCDirection.LEFT;
+        return LCCDirection.DOWN;
+    }
+
+    /**
+     * 计算方向角度差
+     */
+    public static float angleDifference(float angle1, float angle2) {
+        float diff = Math.abs(angle1 - angle2) % 360;
+        return diff > 180 ? 360 - diff : diff;
+    }
+}

servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java

@@ -6,6 +6,9 @@ import com.galaxis.rcs.plan.task.CarryTask;
 
 import java.util.List;
 
+import static com.galaxis.rcs.plan.path2.PathUtils.convertStoreDirection;
+import static com.galaxis.rcs.plan.path2.PathUtils.getRequiredDirection;
+
 public class PtrPathPlanner {
     private final NavigationGraph graph;
     private final AStarPathPlanner astar;
@@ -15,43 +18,50 @@ public class PtrPathPlanner {
         this.astar = new AStarPathPlanner(graph);
     }
 
-    public void planCarryTask(PlanTaskSequence plan, String startId, LCCDirection initDirectionAngle, CarryTask task) {
+    public void planCarryTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, CarryTask task) {
         // 取货点
-        String pickupRackId = task.from().rackId();
+        String loadRackId = task.from().rackId();
         int pickupBay = task.from().bay();
-        Node pickupNode = findStoreNode(pickupRackId, pickupBay);
-        LCCDirection pickupDirectionAngle = getRequiredHeading(pickupNode, pickupBay);
+        NodeDirection loadNodeDirection = findNodeForStore(loadRackId, pickupBay);
+        if (loadNodeDirection == null) {
+            throw new RuntimeException("Pickup node not found for rackId=" + loadRackId + ", bay=" + pickupBay);
+        }
 
         // 放货点
-        String dropRackId = task.to().rackId();
-        int dropBay = task.to().bay();
-        Node dropNode = findStoreNode(dropRackId, dropBay);
-        LCCDirection dropDirectionAngle = getRequiredHeading(dropNode, 0);
+        String unloadRackId = task.to().rackId();
+        int unloadBay = task.to().bay();
+        NodeDirection unloadNodeDirection = findNodeForStore(unloadRackId, unloadBay);
+        if (unloadNodeDirection == null) {
+            throw new RuntimeException("Drop node not found for rackId=" + unloadRackId + ", bay=" + unloadBay);
+        }
 
         // 规划到取货点路径
-        List<Node> toPickupPath = astar.findPath(startId, initDirectionAngle, pickupNode.id(), pickupDirectionAngle);
+        List<State> toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction());
 
         // 规划到放货点路径
-        List<Node> toDeliverPath = astar.findPath(pickupNode.id(), pickupDirectionAngle, dropNode.id(), dropDirectionAngle);
+        List<State> toUnloadPath = astar.findPath(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirection.node().id(), unloadNodeDirection.direction());
 
         // 生成指令序列
-        generateMoves(plan, toPickupPath);
-        plan.addLoad(task.lpn(), pickupRackId, pickupBay, task.from().level(), task.from().cell());
-        generateMoves(plan, toDeliverPath);
-        plan.addUnload(dropRackId, task.to().level(), task.to().bay(), task.to().cell());
+        generateMoves(plan, toLoadPath);
+        plan.addLoad(task.lpn(), loadRackId, pickupBay, task.from().level(), task.from().cell());
+
+        generateMoves(plan, toUnloadPath);
+        plan.addUnload(unloadRackId, task.to().level(), task.to().bay(), task.to().cell());
+
         plan.addFinish();
     }
 
-    private Node findStoreNode(String storeId, int bay) {
+    private NodeDirection findNodeForStore(String storeId, int bay) {
         List<Node> nodes = this.graph.getNodesForStore(storeId);
         for (Node node : nodes) {
             for (StoreLink link : node.storeLinks()) {
                 if (link.storeId().equals(storeId) && link.bay() == bay) {
-                    return node;
+                    LCCDirection agvDirection = convertStoreDirection(link.direction());
+                    return new NodeDirection(node, agvDirection);
                 }
             }
         }
-        throw new RuntimeException("Not found WayPoint link Store, rackId=" + storeId + ", bay=" + bay);
+        return null;
     }
 
     /**
@@ -65,13 +75,32 @@ public class PtrPathPlanner {
             .orElseThrow(() -> new RuntimeException("Not found storeLink in id=" + node.id() + ", bay=" + bay));
     }
 
-    private void generateMoves(PlanTaskSequence plan, List<Node> path) {
-        // 简化的指令生成（实际需处理方向变化）
+    /**
+     * 根据A*状态，生成移动指令序列
+     */
+    private void generateMoves(PlanTaskSequence sequence, List<State> path) {
+        if (path.isEmpty()) return;
+
+        // 第一个状态是起点，跳过
+        State prevState = path.get(0);
+
         for (int i = 1; i < path.size(); i++) {
-            Node node = path.get(i);
-            plan.addMoveTo(node.id());
+            State current = path.get(i);
+
+            // 如果是旋转动作
+            if (current.node().equals(prevState.node())) {
+                float angle = getRequiredDirection(current.direction());
+                sequence.addRotationTo(angle);
+            }
+            // 移动动作
+            else {
+                sequence.addMoveTo(current.node().id());
+            }
+            prevState = current;
         }
     }
 
-
+    // 辅助记录类
+    private record NodeDirection(Node node, LCCDirection direction) {
+    }
 }

servo/src/main/java/com/galaxis/rcs/plan/path2/State.java

@@ -2,8 +2,11 @@ package com.galaxis.rcs.plan.path2;
 
 import com.galaxis.rcs.common.enums.LCCDirection;
 
+/**
+ * A* 路径规划状态
+ */
 public record State(Node node,
-                    LCCDirection directionAngle,
+                    LCCDirection direction,
                     float g,
                     float h,
                     State parent)

servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java

@@ -2,6 +2,15 @@ package com.galaxis.rcs.plan.path2;
 
 import com.galaxis.rcs.common.enums.LCCDirection;
 
+/**
+ * A* 路径与库位的连接定义
+ *
+ * @param storeId   库位ID
+ * @param bay       列
+ * @param level     层
+ * @param cell      格
+ * @param direction 方位（路径相对于库位的方位，比如路标的"左边"是库位）
+ */
 public record StoreLink(
     String storeId, int bay, int level, int cell, LCCDirection direction
 ) {