A* 寻路

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

@@ -0,0 +1,38 @@
+package com.galaxis.rcs.common.enums;
+
+/**
+ * 路径点与货位 / 充电器关联记录
+ */
+public enum LCCDirection {
+    /**
+     * 上方 / z负方向 / up / 北
+     */
+    UP,
+    /**
+     * 下方 / z正方向 / down / 南
+     */
+    DOWN,
+
+    /**
+     * 左方 / x负方向 / left / 西
+     */
+    LEFT,
+
+    /**
+     * 右方 / x正方向 / right / 东
+     */
+    RIGHT;
+
+    public static LCCDirection fromString(String value) {
+        if (value == null)
+            throw new IllegalArgumentException("Value cannot be null");
+
+        for (LCCDirection type : LCCDirection.values()) {
+            if (type.toString().equalsIgnoreCase(value.trim())) {
+                return type;
+            }
+        }
+
+        throw new IllegalArgumentException("No constant with name: " + value);
+    }
+}

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

@@ -0,0 +1,8 @@
+package com.galaxis.rcs.plan.path2;
+
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+
+public class AGVPathPlanner {
+}

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

@@ -0,0 +1,95 @@
+package com.galaxis.rcs.plan.path2;
+
+import com.galaxis.rcs.common.enums.LCCDirection;
+import com.yvan.logisticsModel.LogisticsRuntime;
+
+import java.util.*;
+
+/**
+ * A* 导航图
+ */
+public class NavigationGraph {
+    private final Map<String, Node> nodeMap = new HashMap<>();
+    private final Map<String, List<String>> storeToNodes = new HashMap<>();
+
+    public NavigationGraph() {
+    }
+
+//    private void initialize(LogisticsRuntime runtime) {
+//        var items = runtime.getStaticItems();
+//        for (Map<String, Object> item : items) {
+//            String type = (String) item.get("t");
+//            if ("way".equals(type)) {
+//                float[][] tf = (float[][]) item.get("tf");
+//                String id = (String) item.get("id");
+//                Map<String, Object> dt = (Map<String, Object>) item.get("dt");
+//
+//                // 提取坐标
+//                float x = tf[0][0];
+//                float z = tf[0][2]; // Z向下增长
+//
+//                // 检查可旋转性
+//                boolean rotatable = dt.containsKey("agvRotation");
+//
+//                // 提取邻居节点
+//                List<String> in = (List<String>) dt.get("in");
+//                List<String> out = (List<String>) dt.get("out");
+//                Set<String> neighbors = new HashSet<>();
+//                if (in != null) neighbors.addAll(in);
+//                if (out != null) neighbors.addAll(out);
+//
+//                // 提取货位链接
+//                List<StoreLink> storeLinks = new ArrayList<>();
+//                List<Map<String, Object>> links = (List<Map<String, Object>>) dt.get("linkStore");
+//                if (links != null) {
+//                    for (Map<String, Object> link : links) {
+//                        String storeId = (String) link.get("item");
+//                        int bay = (Integer) link.get("bay");
+//                        int level = (Integer) link.get("level");
+//                        int cell = (Integer) link.get("cell");
+//                        String direction = (String) link.get("direction");
+//                        storeLinks.add(new StoreLink(storeId, bay, level, cell, LCCDirection.fromString(direction)));
+//
+//                        // 建立货位到节点的反向映射
+//                        storeToNodes.computeIfAbsent(storeId, k -> new ArrayList<>()).add(id);
+//                    }
+//                }
+//
+//                nodeMap.put(id, new Node(id, x, z, rotatable,
+//                    new ArrayList<>(neighbors), storeLinks));
+//            }
+//        }
+//
+//        // 2. 验证邻居双向连接
+//        for (Node node : nodeMap.values()) {
+//            Iterator<String> it = node.neighbors().iterator();
+//            while (it.hasNext()) {
+//                String neighborId = it.next();
+//                if (!nodeMap.containsKey(neighborId)) {
+//                    it.remove(); // 移除无效邻居
+//                }
+//            }
+//        }
+//    }
+
+    public Node getNode(String id) {
+        return nodeMap.get(id);
+    }
+
+    public List<String> getNodesForStore(String storeId) {
+        return storeToNodes.getOrDefault(storeId, Collections.emptyList());
+    }
+
+    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/Node.java

@@ -0,0 +1,20 @@
+package com.galaxis.rcs.plan.path2;
+
+import java.util.List;
+
+/**
+ * 路径节点定义
+ *
+ * @param id        节点ID
+ * @param x         节点X坐标
+ * @param z         节点Z坐标
+ * @param rotatable 节点是否可旋转
+ * @param neighbors 邻居节点ID列表
+ */
+public record Node(String id,
+                   float x,
+                   float z,
+                   boolean rotatable,
+                   List<String> neighbors,
+                   List<StoreLink> storeLinks) {
+}

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

@@ -0,0 +1,8 @@
+package com.galaxis.rcs.plan.path2;
+
+import com.galaxis.rcs.common.enums.LCCDirection;
+
+public record StoreLink(
+    String storeId, int bay, int level, int cell, LCCDirection direction
+) {
+}

servo/src/main/java/com/yvan/logisticsModel/BaseItem.java

@@ -17,7 +17,7 @@ public abstract class BaseItem {
     final String t;
 
     /**
-     * 变换矩阵, 3x3矩阵, 采用Y轴向上为正, X轴向右, Z轴向前的右手坐标系
+     * 变换矩阵, 3x3矩阵, tf[0]=position,采用 X向右增长，Y轴向屏幕外增长，Z向下增长, tf[1]=rotation, tf[2]=scale
      */
     final float[][] tf;
 

servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java

@@ -13,6 +13,7 @@ import lombok.extern.slf4j.Slf4j;
 import org.clever.core.Conv;
 import org.clever.core.json.JsonWrapper;
 
+import java.util.Iterator;
 import java.util.List;
 import java.util.Map;
 
@@ -164,4 +165,12 @@ public class LogisticsRuntime {
 
         this.taskDispatchFactory.startPolling();
     }
+
+    public Iterable<StaticItem> getStaticItems() {
+        for (Floor floor : this.floorMap.values()) {
+            // return floor.itemMap.values();
+            return floor.itemMap.values();
+        }
+        return null;
+    }
 }