Merge remote-tracking branch 'origin/master'

servo/src/main/java/com/galaxis/rcs/common/entity/StoreLocation.java

@@ -1,5 +1,7 @@
 package com.galaxis.rcs.common.entity;
 
+import org.clever.core.Conv;
+
 /**
  * 存储存储位信息
  * <pre>
@@ -21,4 +23,25 @@ public record StoreLocation(
     public String toString() {
         return rackId + "_" + bay + "_" + level + "_" + cell;
     }
+
+    public static StoreLocation of(String rackPosition, String separator) {
+        // 从 'rack1/0/0/0' 解析为 'rack1_0_0_0'
+        if (rackPosition == null || rackPosition.isEmpty()) {
+            throw new RuntimeException("rackPosition cannot be null or empty");
+        }
+
+        String[] parts = rackPosition.split(separator);
+        if (parts.length == 1) {
+            return new StoreLocation(parts[0], 0, 0, 0);
+        }
+        if (parts.length != 4) {
+            throw new RuntimeException("Invalid rack position format: " + rackPosition);
+        }
+
+        String rackId = parts[0];
+        int bay = Conv.asInteger(parts[1]);
+        int level = Conv.asInteger(parts[2]);
+        int cell = Conv.asInteger(parts[3]);
+        return new StoreLocation(rackId, bay, level, cell);
+    }
 }

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

@@ -35,4 +35,17 @@ public enum LCCDirection {
 
         throw new IllegalArgumentException("No constant with name: " + value);
     }
+
+    public static LCCDirection fromString(String value, LCCDirection defaultValue) {
+        if (value == null)
+            throw new IllegalArgumentException("Value cannot be null");
+
+        for (LCCDirection type : LCCDirection.values()) {
+            if (type.toString().equalsIgnoreCase(value.trim())) {
+                return type;
+            }
+        }
+
+        return defaultValue;
+    }
 }

servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java

@@ -1,20 +1,12 @@
 package com.galaxis.rcs.connector.cl2;
 
 import com.fasterxml.jackson.core.JsonProcessingException;
-import com.galaxis.rcs.plan.PlanTaskSequence;
-import com.galaxis.rcs.ptr.sendEntity.RcsConfigMessage;
 import com.galaxis.rcs.ptr.sendEntity.RcsTaskMessage;
 import com.galaxis.rcs.ptr.AmrMessageHandler;
-import com.google.common.base.Splitter;
 import com.yvan.logisticsModel.LogisticsRuntime;
-import com.yvan.logisticsModel.PtrAgvItem;
 import lombok.extern.slf4j.Slf4j;
-import org.clever.core.BannerUtils;
-import org.clever.core.json.JsonWrapper;
 import org.eclipse.paho.mqttv5.common.MqttException;
 
-import java.util.Map;
-
 /**
  * CL2 车型报文推送
  */

servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java

@@ -2,7 +2,7 @@ package com.galaxis.rcs.connector.cl2;
 
 import com.galaxis.rcs.ptr.sendEntity.RcsConfigMessage;
 import com.yvan.logisticsModel.LogisticsRuntime;
-import com.yvan.logisticsModel.PtrAgvItem;
+import com.galaxis.rcs.ptr.PtrAgvItem;
 
 import java.util.Map;
 

servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java

@@ -1,9 +1,5 @@
 package com.galaxis.rcs.connector.cl2;
 
-import com.galaxis.rcs.common.entity.RcsTaskPlan;
-import com.yvan.logisticsModel.ExecutorItem;
-import com.yvan.logisticsModel.LogisticsRuntime;
-import com.yvan.logisticsModel.PtrAgvItem;
 import lombok.extern.slf4j.Slf4j;
 
 @Slf4j

servo/src/main/java/com/galaxis/rcs/plan/path/AStarNodeState.java

@@ -1,29 +0,0 @@
-package com.galaxis.rcs.plan.path;
-
-/**
- * A*路径节点状态
- */
-public record AStarNodeState(
-    String nodeId,          // 当前节点ID
-    int direction,          // 当前方向 (0,90,180,270)
-    float gCost,           // 实际代价
-    float hCost,           // 启发式代价
-    AStarNodeState parent   // 父节点
-) implements Comparable<AStarNodeState> {
-
-    // 状态唯一标识
-    public String stateKey() {
-        return nodeId + ":" + direction;
-    }
-
-    // 总代价
-    public float fCost() {
-        return gCost + hCost;
-    }
-
-    @Override
-    public int compareTo(AStarNodeState other) {
-        return Float.compare(this.fCost(), other.fCost());
-    }
-}
-

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

@@ -1,122 +1,220 @@
 package com.galaxis.rcs.plan.path;
 
+import com.galaxis.rcs.common.enums.LCCDirection;
+import com.google.common.collect.Maps;
+
 import java.util.*;
 
-/**
+import static com.galaxis.rcs.plan.path.PathUtils.*;
- * A*路径规划器
+
- */
 public class AStarPathPlanner {
+    private static final float ROTATION_COST_PER_DEGREE = 0.01f;
+    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<State> findPath(String startId, LCCDirection startDirection, String endId, LCCDirection endDirection) {
-     *
+        Node start = graph.getNode(startId);
-     * @param startNodeId    起始节点ID
+        Node end = graph.getNode(endId);
-     * @param startDirection 起始方向
+        if (start == null) {
-     * @param endNodeId      目标节点ID
+            throw new RuntimeException("Start node not found: " + startId);
-     * @param endDirection   目标方向
+        }
-     * @return 路径节点序列 (包含方向信息)
+        if (end == null) {
-     */
+            throw new RuntimeException("End node not found: " + endId);
-    public List<AStarNodeState> planPath(String startNodeId, int startDirection, String endNodeId, int endDirection) {
+        }
-        // 开放集 (优先队列)
+
-        PriorityQueue<AStarNodeState> openSet = new PriorityQueue<>();
+        // 使用复合键避免状态重复
-
+        Map<String, State> visited = new HashMap<>();
-        // 状态管理
+        PriorityQueue<State> open = new PriorityQueue<>();
-        Map<String, Float> gScoreMap = new HashMap<>();
-        Map<String, AStarNodeState> cameFrom = new HashMap<>();
-
-        // 初始化起点
-        AStarNodeState start = new AStarNodeState(
-            startNodeId,
-            startDirection,
-            0,
-            graph.heuristicCost(startNodeId, endNodeId),
-            null
-        );
 
-        openSet.add(start);
+        // 初始状态
-        gScoreMap.put(start.stateKey(), 0.0f);
+        State initialState = new State(start, startDirection, 0, heuristic(start, end), null);
+        open.add(initialState);
+        visited.put(stateKey(start.id(), startDirection), initialState);
 
-        while (!openSet.isEmpty()) {
+        while (!open.isEmpty()) {
-            AStarNodeState current = openSet.poll();
+            State current = open.poll();
 
-            // 到达目标状态
+            // 到达目标节点且方向匹配
-            if (current.nodeId().equals(endNodeId) &&
+            if (current.node().id().equals(endId) && current.direction() == endDirection) {
-                current.direction() == endDirection) {
+                return buildPath(current);
-                return reconstructPath(cameFrom, current);
             }
 
-            // 处理移动操作 (到相邻节点)
+            // 处理邻边移动
-            for (NavigationNode neighbor : graph.getAdjacentNodes(current.nodeId())) {
+            for (Node neighbor : graph.getNeighbors(current.node())) {
-                PathSegment segment = graph.getPathSegment(current.nodeId(), neighbor.id());
+                // 检查节点是否被阻塞
-                if (segment == null) continue;
+                if (isBlocked(neighbor.id())) continue;
 
-                float moveCost = segment.distance();
+                // 计算移动方向
-                float tentativeGCost = current.gCost() + moveCost;
+                LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor);
-                String neighborKey = neighbor.id() + ":" + current.direction();
 
-                // 发现更好路径
+                // 尝试前进
-                if (tentativeGCost < gScoreMap.getOrDefault(neighborKey, Float.MAX_VALUE)) {
+                if (canMoveForward(current.direction(), moveDirection)) {
-                    AStarNodeState neighborState = new AStarNodeState(
+                    float moveCost = calculateMoveCost(current.node(), neighbor, false);
-                        neighbor.id(),
+                    considerState(current, neighbor, current.direction(),
-                        current.direction(),
+                        moveCost, open, visited, end);
-                        tentativeGCost,
+                }
-                        graph.heuristicCost(neighbor.id(), endNodeId),
+                // 尝试后退
+                else if (canMoveBackward(current.direction(), moveDirection)) {
+                    float moveCost = calculateMoveCost(current.node(), neighbor, true);
+                    considerState(current, neighbor, current.direction(),
+                        moveCost, open, visited, end);
+                }
+                // 需要旋转
+                else if (current.node().rotatable()) {
+                    // 计算需要旋转到的方向
+                    LCCDirection requiredDirection = calculateRequiredDirection(moveDirection);
+
+                    // 考虑旋转后移动
+                    float rotationCost = calculateRotationCost(
+                        current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE
+                    );
+                    float moveCost = calculateMoveCost(current.node(), neighbor, false);
+                    float totalCost = rotationCost + moveCost;
+
+                    // 创建旋转状态
+                    State rotatedState = new State(
+                        current.node(), requiredDirection,
+                        current.g() + rotationCost,
+                        heuristic(current.node(), end),
                         current
                     );
 
-                    cameFrom.put(neighborKey, current);
+                    // 考虑旋转后的移动
-                    gScoreMap.put(neighborKey, tentativeGCost);
+                    considerState(rotatedState, neighbor, requiredDirection,
-                    openSet.add(neighborState);
+                        moveCost, open, visited, end);
                 }
             }
 
-            // 处理旋转操作 (当前节点可旋转时)
+            // 处理原地旋转 - 只考虑目标方向
-            NavigationNode currentNode = graph.nodes.get(current.nodeId());
+            if (current.node().rotatable()) {
-            if (currentNode != null && currentNode.rotatable()) {
+                // 只考虑旋转到目标方向（如果可能）
-                for (int rotation : new int[]{90, -90}) {
+                if (current.direction() != endDirection) {
-                    int newDirection = (current.direction() + rotation + 360) % 360;
+                    float rotationCost = calculateRotationCost(
-                    float rotateCost = 1.0f; // 旋转代价
+                        current.direction(), endDirection, ROTATION_COST_PER_DEGREE
-                    float tentativeGCost = current.gCost() + rotateCost;
+                    );
-                    String rotatedKey = current.nodeId() + ":" + newDirection;
+                    considerState(current, current.node(), endDirection,
+                        rotationCost, open, visited, end);
+                }
 
-                    // 发现更好路径
+                // 另外考虑旋转到移动所需的方向
-                    if (tentativeGCost < gScoreMap.getOrDefault(rotatedKey, Float.MAX_VALUE)) {
+                for (Node neighbor : graph.getNeighbors(current.node())) {
-                        AStarNodeState rotatedState = new AStarNodeState(
+                    LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor);
-                            current.nodeId(),
+                    LCCDirection requiredDirection = calculateRequiredDirection(moveDirection);
-                            newDirection,
+
-                            tentativeGCost,
+                    if (requiredDirection != current.direction()) {
-                            current.hCost(), // 旋转不改变位置，启发值不变
+                        float rotationCost = calculateRotationCost(
-                            current
+                            current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE
                         );
+                        considerState(current, current.node(), requiredDirection,
+                            rotationCost, open, visited, end);
+                    }
+                }
 
-                        cameFrom.put(rotatedKey, current);
+                // for (LCCDirection rotation : LCCDirection.values()) {
-                        gScoreMap.put(rotatedKey, tentativeGCost);
+                //     if (rotation == current.direction()) continue;
-                        openSet.add(rotatedState);
+                //
+                //     float rotationCost = calculateRotationCost(
+                //         current.direction(), rotation, ROTATION_COST_PER_DEGREE
+                //     );
+                //     considerState(current, current.node(), rotation,
+                //         rotationCost, open, visited, end);
+                // }
             }
         }
+        return Collections.emptyList();
     }
+
+    /**
+     * 考虑新的状态并更新开放列表和访问记录
+     *
+     * @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 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, end);
+            State newState = new State(nextNode, nextDirection, newG, h, current);
+            open.add(newState);
+            visited.put(key, newState);
+        }
+    }
+
+    private List<State> buildPath(State state) {
+        LinkedList<State> path = new LinkedList<>();
+        while (state != null) {
+            path.addFirst(state);
+            state = state.parent();
+        }
+        return path;
     }
 
-        return Collections.emptyList(); // 未找到路径
+    /**
+     * 启发式函数，计算两个节点之间的距离
+     */
+    private float heuristic(Node a, Node b) {
+        return graph.distance(a, b);
+        // 使用曼哈顿距离？？
+        // return Math.abs(a.x() - b.x()) + Math.abs(a.z() - b.z());
     }
 
-    private List<AStarNodeState> reconstructPath(
+    /**
-        Map<String, AStarNodeState> cameFrom,
+     * 生成状态的唯一键
-        AStarNodeState endState
+     */
-    ) {
+    private String stateKey(String nodeId, LCCDirection direction) {
-        LinkedList<AStarNodeState> path = new LinkedList<>();
+        return nodeId + "|" + direction;
-        AStarNodeState current = endState;
+    }
 
-        while (current != null) {
+    public void setNodeWeight(String nodeId, float weight) {
-            path.addFirst(current);
+        nodeWeights.put(nodeId, weight);
-            current = cameFrom.get(current.stateKey());
     }
 
-        return path;
+    public void setBlocked(String nodeId, float blockedFactor) {
+        blockedNodes.put(nodeId, blockedFactor);
+    }
+
+    private float calculateMoveCost(Node from, Node to, boolean isBackward) {
+        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) * (isBackward ? 1.2f : 1.0f);
+        return baseCost * weight * (1 + blockedFactor);
+    }
+
+    private boolean isBlocked(String nodeId) {
+        return blockedNodes.containsKey(nodeId) && blockedNodes.get(nodeId) > 0.8f;
+    }
+
+    private boolean canMoveForward(LCCDirection currentDir, LCCDirection moveDir) {
+        return currentDir == moveDir;
+    }
+
+    private boolean canMoveBackward(LCCDirection currentDir, LCCDirection moveDir) {
+        return currentDir == getOppositeDirection(moveDir);
+    }
+
+    private LCCDirection calculateRequiredDirection(LCCDirection moveDirection) {
+        // 侧插式AGV只能前进或后退，不能横移
+        return moveDirection;
     }
 }

servo/src/main/java/com/galaxis/rcs/plan/path/GraphInitializer.java

@@ -1,95 +0,0 @@
-package com.galaxis.rcs.plan.path;
-
-import com.galaxis.rcs.common.enums.OperationSide;
-import com.yvan.logisticsModel.LogisticsRuntime;
-import org.clever.core.Conv;
-
-import java.util.List;
-import java.util.Map;
-
-public class GraphInitializer {
-
-//    public NavigationGraph initializeGraph(LogisticsRuntime runtime, String agvType, List<Map<String, Object>> jsonData) {
-//        if (runtime.pathPlannerMap.containsKey(agvType)) {
-//            return runtime.pathPlannerMap.get(agvType);
-//        }
-//        NavigationGraph graph = new NavigationGraph();
-//        runtime.pathPlannerMap.put(agvType, graph);
-//
-//        // 第一步：创建所有节点
-//        for (Map<String, Object> nodeData : jsonData) {
-//            String id = (String) nodeData.get("id");
-//
-//            // 判断是否是 way 类型才创建 NavigationNode
-//            if (!"way".equals(nodeData.get("t"))) {
-//                continue;
-//            }
-//
-//            List<List<Float>> tf = (List<List<Float>>) nodeData.get("tf");
-//            float x = tf.get(0).get(0);
-//            float z = tf.get(0).get(2);
-//
-//            // 检查是否为可旋转点
-//            Map<String, Object> dt = (Map<String, Object>) nodeData.get("dt");
-//            boolean rotatable = false;
-//            if (dt.containsKey("agvRotation")) {
-//                rotatable = true;
-//            }
-//
-//            // 添加节点
-//            graph.addNode(new NavigationNode(id, x, z, rotatable));
-//        }
-//
-//        // 第二步：添加路径连接
-//        for (Map<String, Object> nodeData : jsonData) {
-//            if (!"way".equals(nodeData.get("t"))) continue;
-//
-//            String id = (String) nodeData.get("id");
-//            Map<String, Object> dt = (Map<String, Object>) nodeData.get("dt");
-//
-//            List<String> outEdges = (List<String>) dt.get("out");
-//            if (outEdges != null) {
-//                for (String neighborId : outEdges) {
-//                    if (graph.nodes.containsKey(id) && graph.nodes.containsKey(neighborId)) {
-//                        NavigationNode from = graph.nodes.get(id);
-//                        NavigationNode to = graph.nodes.get(neighborId);
-//                        graph.addBidirectionalPath(from, to);
-//                    }
-//                }
-//            }
-//        }
-//
-//        // 第三步：添加操作点 OperationPoint
-//        for (Map<String, Object> nodeData : jsonData) {
-//            if (!"way".equals(nodeData.get("t"))) continue;
-//
-//            String nodeId = (String) nodeData.get("id");
-//            Map<String, Object> dt = (Map<String, Object>) nodeData.get("dt");
-//
-//            if (dt.containsKey("linkStore")) {
-//                List<Map<String, Object>> linkStores = (List<Map<String, Object>>) dt.get("linkStore");
-//                for (Map<String, Object> store : linkStores) {
-//                    String targetId = (String) store.get("item");
-//                    Integer bay = Conv.asInteger(store.get("bay"));
-//                    Integer level = Conv.asInteger(store.get("level"));
-//                    Integer cell = Conv.asInteger(store.get("cell"));
-//
-//                    // 根据位置确定方位（这里假设固定为 TOP，可根据 tf 中的方向判断更精确）
-//                    OperationSide side = OperationSide.TOP;
-//
-//                    OperationPoint point = new OperationPoint(
-//                        graph.nodes.get(nodeId),
-//                        targetId,
-//                        side,
-//                        bay,
-//                        level,
-//                        cell
-//                    );
-//                    graph.addOperationPoint(point);
-//                }
-//            }
-//        }
-//
-//        return graph;
-//    }
-}

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

@@ -1,69 +1,147 @@
 package com.galaxis.rcs.plan.path;
 
+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.*;
-import java.util.concurrent.ConcurrentHashMap;
 
 /**
- * 导航图管理器
+ * A* 导航图
  */
+@Slf4j
 public class NavigationGraph {
-    final Map<String, NavigationNode> nodes = new ConcurrentHashMap<>();
+    private final LogisticsRuntime runtime;
-    final Map<String, OperationPoint> operationPoints = new ConcurrentHashMap<>();
+
-    final Map<String, PathSegment> pathSegments = new ConcurrentHashMap<>();
+    /**
-    final Map<String, List<NavigationNode>> adjacencyList = new ConcurrentHashMap<>();
+     * 缓存距离计算结果，避免重复计算
+     */
+    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<>();
+
+    public NavigationGraph(LogisticsRuntime runtime) {
+        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,
-    public void addNode(NavigationNode node) {
+                          String endId, LCCDirection endDirection,
-        nodes.put(node.id(), node);
+                          List<State> path) {
-        adjacencyList.put(node.id(), new ArrayList<>());
+        String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection;
+        pathCache.put(cacheKey, path);
     }
 
-    // 添加双向路径
+    public Node getNodeById(String nodeId){
-    public void addBidirectionalPath(NavigationNode a, NavigationNode b) {
+        return nodeMap.get(nodeId);
-        float distance = a.distanceTo(b);
+    }
-        pathSegments.put(a.id() + "->" + b.id(), new PathSegment(a, b, distance));
-        pathSegments.put(b.id() + "->" + a.id(), new PathSegment(b, a, distance));
 
-        adjacencyList.get(a.id()).add(b);
+    public List<Node> getNodesForStore(String storeId) {
-        adjacencyList.get(b.id()).add(a);
+        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) {
-    public void addOperationPoint(OperationPoint point) {
+        return node.neighbors().stream()
-        operationPoints.put(point.locationKey(), point);
+            .map(nodeMap::get)
+            .filter(Objects::nonNull)
+            .toList();
     }
 
-    // 查找操作点
+    public float distance(Node a, Node b) {
-    public OperationPoint findOperationPoint(String locationKey) {
+        float dx = a.x() - b.x();
-        return operationPoints.get(locationKey);
+        float dz = a.z() - b.z();
+        return (float) Math.sqrt(dx * dx + dz * dz);
     }
 
-    // 获取路径段
+
-    public PathSegment getPathSegment(String startId, String endId) {
+    public void init() {
-        return pathSegments.get(startId + "->" + endId);
+        var items = runtime.getStaticItems();
+        for (StaticItem item : items) {
+            if ("way".equals(item.getT())) {
+                float[][] tf = item.getTf();
+                String id = item.getId();
+                Map<String, Object> dt = item.getDt();
+
+                // 提取坐标
+                float x = tf[0][0];
+                float z = tf[0][2]; // Z向下增长
+
+                // 检查可旋转性
+                boolean rotatable = dt.containsKey("agvRotation");
+                if (rotatable) {
+                    log.info("Node {} is rotatable", id);
                 }
 
-    // 获取相邻节点
+                // 提取邻居节点
-    public List<NavigationNode> getAdjacentNodes(String nodeId) {
+                List<String> in = (List<String>) dt.get("in");
-        return adjacencyList.getOrDefault(nodeId, Collections.emptyList());
+                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,
-    public NavigationNode findNearestRotationPoint(String startId) {
+                    new ArrayList<>(neighbors), storeLinks));
-        NavigationNode start = nodes.get(startId);
+            }
-        if (start == null) return null;
+        }
 
-        return nodes.values().parallelStream()
+        // 2. 验证邻居双向连接
-            .filter(NavigationNode::rotatable)
+        for (Node node : nodeMap.values()) {
-            .min(Comparator.comparing(start::distanceTo))
+            Iterator<String> it = node.neighbors().iterator();
-            .orElse(null);
+            while (it.hasNext()) {
+                String neighborId = it.next();
+                if (!nodeMap.containsKey(neighborId)) {
+                    it.remove(); // 移除无效邻居
+                }
+            }
+        }
     }
 
-    // 计算启发式代价 (使用欧几里得距离)
+    public Node getNode(String id) {
-    public float heuristicCost(String fromId, String toId) {
+        return nodeMap.get(id);
-        NavigationNode from = nodes.get(fromId);
-        NavigationNode to = nodes.get(toId);
-        if (from == null || to == null) return Float.MAX_VALUE;
-        return from.distanceTo(to);
     }
 }

servo/src/main/java/com/galaxis/rcs/plan/path/NavigationNode.java

@@ -1,25 +0,0 @@
-package com.galaxis.rcs.plan.path;
-
-import org.clever.core.Conv;
-
-/**
- * 路标节点（地图中的顶点）
- */
-public record NavigationNode(
-    String id,          // 节点ID (如 "20", "charger1")
-    float x,           // 向右增长
-    float z,           // 向下增长 (y无效)
-    boolean rotatable   // 是否为可旋转位
-) implements Comparable<NavigationNode> {
-    // 计算到另一个节点的欧几里得距离
-    public float distanceTo(NavigationNode other) {
-        float dx = this.x - other.x;
-        float dz = this.z - other.z;
-        return Conv.asFloat(Math.sqrt(dx * dx + dz * dz));
-    }
-
-    @Override
-    public int compareTo(NavigationNode other) {
-        return this.id.compareTo(other.id);
-    }
-}

servo/src/main/java/com/galaxis/rcs/plan/path2/Node.java → servo/src/main/java/com/galaxis/rcs/plan/path/Node.java

@@ -1,4 +1,4 @@
-package com.galaxis.rcs.plan.path2;
+package com.galaxis.rcs.plan.path;
 
 import java.util.List;
 

servo/src/main/java/com/galaxis/rcs/plan/path/OperationPoint.java

@@ -1,33 +0,0 @@
-package com.galaxis.rcs.plan.path;
-
-import com.galaxis.rcs.common.enums.OperationSide;
-
-/**
- * 操作位置（取货/放货/充电点）
- */
-public record OperationPoint(
-    NavigationNode node,        // 关联的路标节点
-    String targetId,            // 货位ID (如 "54") 或货架ID (如 "rack1")
-    OperationSide side,         // 方位
-    Integer bay,                // 列 (货架时非空)
-    Integer level,              // 层 (货架时非空)
-    Integer cell                // 格 (货架时非空)
-) {
-    // 计算操作所需方向
-    public int requiredDirection() {
-        return switch (side) {
-            case LEFT -> 90;
-            case RIGHT -> 270;
-            case TOP -> 0;
-            case BOTTOM -> 180;
-        };
-    }
-
-    // 位置唯一标识
-    public String locationKey() {
-        if (bay == null || level == null || cell == null) {
-            return targetId; // 地堆货位
-        }
-        return targetId + "-" + bay + "-" + level + "-" + cell;
-    }
-}

servo/src/main/java/com/galaxis/rcs/plan/path/PathSegment.java

@@ -1,15 +0,0 @@
-package com.galaxis.rcs.plan.path;
-
-/**
- * 路径段（节点之间的连接）
- */
-public record PathSegment(
-    NavigationNode start,   // 起点节点
-    NavigationNode end,     // 终点节点
-    float distance         // 路径距离
-) {
-    // 路径唯一标识
-    public String key() {
-        return start.id() + "->" + end.id();
-    }
-}

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

@@ -1,4 +1,4 @@
-package com.galaxis.rcs.plan.path2;
+package com.galaxis.rcs.plan.path;
 
 import com.galaxis.rcs.common.enums.LCCDirection;
 

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

@@ -0,0 +1,236 @@
+package com.galaxis.rcs.plan.path;
+
+import com.galaxis.rcs.common.enums.LCCDirection;
+import com.galaxis.rcs.plan.PlanTaskSequence;
+import com.galaxis.rcs.plan.task.CarryTask;
+import com.galaxis.rcs.plan.task.LoadTask;
+import com.galaxis.rcs.plan.task.MoveTask;
+import com.galaxis.rcs.plan.task.UnloadTask;
+
+import java.util.List;
+
+import static com.galaxis.rcs.plan.path.PathUtils.convertStoreDirection;
+
+public class PtrPathPlanner {
+    private final NavigationGraph graph;
+    private final AStarPathPlanner astar;
+
+    public PtrPathPlanner(NavigationGraph graph) {
+        this.graph = graph;
+        this.astar = new AStarPathPlanner(graph);
+    }
+
+
+    public void planMoveTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, MoveTask moveTask) {
+
+        Node node = this.graph.getNodeById(moveTask.targetWayPointId());
+
+        List<State> toPath = astar.findPath(startNodeId, startDirection, node.id(), moveTask.targetDirection());
+        generateMoves(plan, toPath);
+        plan.addFinish();
+    }
+
+    public void planCarryTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, CarryTask task) {
+        // 取货点
+        String loadRackId = task.from().rackId();
+        int pickupBay = task.from().bay();
+        NodeDirection loadNodeDirection = findNodeForStore(loadRackId, pickupBay);
+        if (loadNodeDirection == null) {
+            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) {
+            throw new RuntimeException("Drop node not found for rackId=" + unloadRackId + ", bay=" + unloadBay);
+        }
+
+        // 规划到取货点路径
+        List<State> toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction());
+
+        // 检查方向是否匹配，如果不匹配则插入旋转点
+         if (!toLoadPath.isEmpty()) {
+             State lastState = toLoadPath.get(toLoadPath.size() - 1);
+             if (lastState.direction() != loadNodeDirection.direction()) {
+                 Node rotationNode = PathUtils.findNearestRotationNode(
+                     graph, lastState.node(), lastState.direction(), loadNodeDirection.direction()
+                 );
+
+                 if (rotationNode != null) {
+                     // 插入旋转路径
+                     List<State> toRotation = astar.findPath(
+                         lastState.node().id(), lastState.direction(),
+                         rotationNode.id(), loadNodeDirection.direction()
+                     );
+                     toLoadPath.addAll(toRotation);
+
+                     // 从旋转点到目标点
+                     List<State> fromRotation = astar.findPath(
+                         rotationNode.id(), loadNodeDirection.direction(),
+                         loadNodeDirection.node().id(), loadNodeDirection.direction()
+                     );
+                     toLoadPath.addAll(fromRotation);
+                 }
+             }
+         }
+
+        // 规划到放货点路径
+        List<State> toUnloadPath = astar.findPath(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirection.node().id(), unloadNodeDirection.direction());
+
+        // 生成指令序列
+        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();
+    }
+
+
+    public void planUnloadTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, UnloadTask task) {
+        Node fromNode = this.graph.getNodeById(startNodeId);
+
+        // 放货点
+        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<State> toUnloadPath = astar.findPath(startNodeId, startDirection, unloadNodeDirection.node().id(), unloadNodeDirection.direction());
+
+        // 检查方向是否匹配，如果不匹配则插入旋转点
+        // State lastState = toUnloadPath.get(toUnloadPath.size() - 1);
+        // if (startDirection != lastState.direction()) {
+        //     Node rotationNode = PathUtils.findNearestRotationNode(
+        //         graph, fromNode, startDirection, lastState.direction()
+        //     );
+        //
+        //     if (rotationNode != null) {
+        //         // 插入旋转路径
+        //         List<State> toRotation = astar.findPath(
+        //             startNodeId, startDirection,
+        //             rotationNode.id(), unloadNodeDirection.direction()
+        //         );
+        //         toUnloadPath.addAll(toRotation);
+        //
+        //         // 从旋转点到目标点
+        //         List<State> fromRotation = astar.findPath(
+        //             rotationNode.id(), unloadNodeDirection.direction(),
+        //             unloadNodeDirection.node().id(), unloadNodeDirection.direction()
+        //         );
+        //         toUnloadPath.addAll(fromRotation);
+        //     }
+        // }
+
+        // 生成指令序列
+        generateMoves(plan, toUnloadPath);
+        plan.addUnload(unloadRackId, task.to().level(), task.to().bay(), task.to().cell());
+
+        plan.addFinish();
+    }
+
+
+    public void planLoadTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, LoadTask task) {
+        Node fromNode = this.graph.getNodeById(startNodeId);
+
+        // 放货点
+        String loadRackId = task.to().rackId();
+        int unloadBay = task.to().bay();
+        NodeDirection loadNodeDirection = findNodeForStore(loadRackId, unloadBay);
+        if (loadNodeDirection == null) {
+            throw new RuntimeException("Drop node not found for rackId=" + loadRackId + ", bay=" + unloadBay);
+        }
+
+        // 规划到取货点路径
+        List<State> toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction());
+
+        // 检查方向是否匹配，如果不匹配则插入旋转点
+        // State lastState = toLoadPath.get(toLoadPath.size() - 1);
+        // if (startDirection != lastState.direction()) {
+        //     Node rotationNode = PathUtils.findNearestRotationNode(
+        //         graph, fromNode, startDirection, lastState.direction()
+        //     );
+        //
+        //     if (rotationNode != null) {
+        //         // 插入旋转路径
+        //         List<State> toRotation = astar.findPath(
+        //             startNodeId, startDirection,
+        //             rotationNode.id(), loadNodeDirection.direction()
+        //         );
+        //         toLoadPath.addAll(toRotation);
+        //
+        //         // 从旋转点到目标点
+        //         List<State> fromRotation = astar.findPath(
+        //             rotationNode.id(), loadNodeDirection.direction(),
+        //             loadNodeDirection.node().id(), loadNodeDirection.direction()
+        //         );
+        //         toLoadPath.addAll(fromRotation);
+        //     }
+        // }
+
+        // 生成指令序列
+        generateMoves(plan, toLoadPath);
+        plan.addLoad("N/A", loadRackId, task.to().level(), task.to().bay(), task.to().cell());
+
+        plan.addFinish();
+    }
+
+    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) {
+                    LCCDirection agvDirection = convertStoreDirection(link.direction());
+                    return new NodeDirection(node, agvDirection);
+                }
+            }
+        }
+        return null;
+    }
+
+    /**
+     * 根据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++) {
+            State current = path.get(i);
+
+            // 如果是旋转动作
+            if (current.node().equals(prevState.node())) {
+                float angle = PathUtils.getRequiredDirection(current.direction());
+                sequence.addRotationTo(angle);
+            }
+            // 移动动作 - 检查是否需要后退
+            else {
+                // 检查移动方向
+                LCCDirection moveDir = PathUtils.calculateMoveDirection(prevState.node(), current.node());
+
+                // 如果移动方向与车头方向相反，需要后退
+                boolean isBackward = (current.direction() == PathUtils.getOppositeDirection(moveDir));
+
+                if (isBackward) {
+                    sequence.addMoveBackward(current.node().id());
+                } 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 → servo/src/main/java/com/galaxis/rcs/plan/path/State.java

@@ -1,4 +1,4 @@
-package com.galaxis.rcs.plan.path2;
+package com.galaxis.rcs.plan.path;
 
 import com.galaxis.rcs.common.enums.LCCDirection;
 

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

@@ -1,4 +1,4 @@
-package com.galaxis.rcs.plan.path2;
+package com.galaxis.rcs.plan.path;
 
 import com.galaxis.rcs.common.enums.LCCDirection;
 

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

@@ -1,220 +0,0 @@
-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.01f;
-    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<State> findPath(String startId, LCCDirection startDirection, String endId, LCCDirection endDirection) {
-        Node start = graph.getNode(startId);
-        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, startDirection, 0, heuristic(start, end), null);
-        open.add(initialState);
-        visited.put(stateKey(start.id(), startDirection), initialState);
-
-        while (!open.isEmpty()) {
-            State current = open.poll();
-
-            // 到达目标节点且方向匹配
-            if (current.node().id().equals(endId) && current.direction() == endDirection) {
-                return buildPath(current);
-            }
-
-            // 处理邻边移动
-            for (Node neighbor : graph.getNeighbors(current.node())) {
-                // 检查节点是否被阻塞
-                if (isBlocked(neighbor.id())) continue;
-
-                // 计算移动方向
-                LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor);
-
-                // 尝试前进
-                if (canMoveForward(current.direction(), moveDirection)) {
-                    float moveCost = calculateMoveCost(current.node(), neighbor, false);
-                    considerState(current, neighbor, current.direction(),
-                        moveCost, open, visited, end);
-                }
-                // 尝试后退
-                else if (canMoveBackward(current.direction(), moveDirection)) {
-                    float moveCost = calculateMoveCost(current.node(), neighbor, true);
-                    considerState(current, neighbor, current.direction(),
-                        moveCost, open, visited, end);
-                }
-                // 需要旋转
-                else if (current.node().rotatable()) {
-                    // 计算需要旋转到的方向
-                    LCCDirection requiredDirection = calculateRequiredDirection(moveDirection);
-
-                    // 考虑旋转后移动
-                    float rotationCost = calculateRotationCost(
-                        current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE
-                    );
-                    float moveCost = calculateMoveCost(current.node(), neighbor, false);
-                    float totalCost = rotationCost + moveCost;
-
-                    // 创建旋转状态
-                    State rotatedState = new State(
-                        current.node(), requiredDirection,
-                        current.g() + rotationCost,
-                        heuristic(current.node(), end),
-                        current
-                    );
-
-                    // 考虑旋转后的移动
-                    considerState(rotatedState, neighbor, requiredDirection,
-                        moveCost, open, visited, end);
-                }
-            }
-
-            // 处理原地旋转 - 只考虑目标方向
-            if (current.node().rotatable()) {
-                // 只考虑旋转到目标方向（如果可能）
-                if (current.direction() != endDirection) {
-                    float rotationCost = calculateRotationCost(
-                        current.direction(), endDirection, ROTATION_COST_PER_DEGREE
-                    );
-                    considerState(current, current.node(), endDirection,
-                        rotationCost, open, visited, end);
-                }
-
-                // 另外考虑旋转到移动所需的方向
-                for (Node neighbor : graph.getNeighbors(current.node())) {
-                    LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor);
-                    LCCDirection requiredDirection = calculateRequiredDirection(moveDirection);
-
-                    if (requiredDirection != current.direction()) {
-                        float rotationCost = calculateRotationCost(
-                            current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE
-                        );
-                        considerState(current, current.node(), requiredDirection,
-                            rotationCost, open, visited, end);
-                    }
-                }
-
-                // for (LCCDirection rotation : LCCDirection.values()) {
-                //     if (rotation == current.direction()) continue;
-                //
-                //     float rotationCost = calculateRotationCost(
-                //         current.direction(), rotation, ROTATION_COST_PER_DEGREE
-                //     );
-                //     considerState(current, current.node(), rotation,
-                //         rotationCost, open, visited, end);
-                // }
-            }
-        }
-        return Collections.emptyList();
-    }
-
-    /**
-     * 考虑新的状态并更新开放列表和访问记录
-     *
-     * @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 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, end);
-            State newState = new State(nextNode, nextDirection, newG, h, current);
-            open.add(newState);
-            visited.put(key, newState);
-        }
-    }
-
-    private List<State> buildPath(State state) {
-        LinkedList<State> path = new LinkedList<>();
-        while (state != null) {
-            path.addFirst(state);
-            state = state.parent();
-        }
-        return path;
-    }
-
-    /**
-     * 启发式函数，计算两个节点之间的距离
-     */
-    private float heuristic(Node a, Node b) {
-        return graph.distance(a, b);
-        // 使用曼哈顿距离？？
-        // return Math.abs(a.x() - b.x()) + Math.abs(a.z() - b.z());
-    }
-
-    /**
-     * 生成状态的唯一键
-     */
-    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, boolean isBackward) {
-        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) * (isBackward ? 1.2f : 1.0f);
-        return baseCost * weight * (1 + blockedFactor);
-    }
-
-    private boolean isBlocked(String nodeId) {
-        return blockedNodes.containsKey(nodeId) && blockedNodes.get(nodeId) > 0.8f;
-    }
-
-    private boolean canMoveForward(LCCDirection currentDir, LCCDirection moveDir) {
-        return currentDir == moveDir;
-    }
-
-    private boolean canMoveBackward(LCCDirection currentDir, LCCDirection moveDir) {
-        return currentDir == getOppositeDirection(moveDir);
-    }
-
-    private LCCDirection calculateRequiredDirection(LCCDirection moveDirection) {
-        // 侧插式AGV只能前进或后退，不能横移
-        return moveDirection;
-    }
-}

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

@@ -1,147 +0,0 @@
-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<>();
-
-    public NavigationGraph(LogisticsRuntime runtime) {
-        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 Node getNodeById(String nodeId){
-        return nodeMap.get(nodeId);
-    }
-
-    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) {
-            if ("way".equals(item.getT())) {
-                float[][] tf = item.getTf();
-                String id = item.getId();
-                Map<String, Object> dt = item.getDt();
-
-                // 提取坐标
-                float x = tf[0][0];
-                float z = tf[0][2]; // Z向下增长
-
-                // 检查可旋转性
-                boolean rotatable = dt.containsKey("agvRotation");
-                if (rotatable) {
-                    log.info("Node {} is rotatable", id);
-                }
-
-                // 提取邻居节点
-                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);
-    }
-}

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

@@ -1,143 +0,0 @@
-package com.galaxis.rcs.plan.path2;
-
-import com.galaxis.rcs.common.enums.LCCDirection;
-import com.galaxis.rcs.plan.PlanTaskSequence;
-import com.galaxis.rcs.plan.task.CarryTask;
-import com.galaxis.rcs.plan.task.MoveTask;
-
-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;
-
-    public PtrPathPlanner(NavigationGraph graph) {
-        this.graph = graph;
-        this.astar = new AStarPathPlanner(graph);
-    }
-
-
-    public void planMoveTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, MoveTask moveTask) {
-
-        Node node = this.graph.getNodeById(moveTask.targetWayPointId());
-
-        List<State> toPath = astar.findPath(startNodeId, startDirection, node.id(), startDirection);
-        generateMoves(plan, toPath);
-        plan.addFinish();
-    }
-
-    public void planCarryTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, CarryTask task) {
-        // 取货点
-        String loadRackId = task.from().rackId();
-        int pickupBay = task.from().bay();
-        NodeDirection loadNodeDirection = findNodeForStore(loadRackId, pickupBay);
-        if (loadNodeDirection == null) {
-            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) {
-            throw new RuntimeException("Drop node not found for rackId=" + unloadRackId + ", bay=" + unloadBay);
-        }
-
-        // 规划到取货点路径
-        List<State> toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction());
-
-        // 检查方向是否匹配，如果不匹配则插入旋转点
-        if (!toLoadPath.isEmpty()) {
-            State lastState = toLoadPath.get(toLoadPath.size() - 1);
-            if (lastState.direction() != loadNodeDirection.direction()) {
-                Node rotationNode = PathUtils.findNearestRotationNode(
-                    graph, lastState.node(), lastState.direction(), loadNodeDirection.direction()
-                );
-
-                if (rotationNode != null) {
-                    // 插入旋转路径
-                    List<State> toRotation = astar.findPath(
-                        lastState.node().id(), lastState.direction(),
-                        rotationNode.id(), loadNodeDirection.direction()
-                    );
-                    toLoadPath.addAll(toRotation);
-
-                    // 从旋转点到目标点
-                    List<State> fromRotation = astar.findPath(
-                        rotationNode.id(), loadNodeDirection.direction(),
-                        loadNodeDirection.node().id(), loadNodeDirection.direction()
-                    );
-                    toLoadPath.addAll(fromRotation);
-                }
-            }
-        }
-
-        // 规划到放货点路径
-        List<State> toUnloadPath = astar.findPath(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirection.node().id(), unloadNodeDirection.direction());
-
-        // 生成指令序列
-        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 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) {
-                    LCCDirection agvDirection = convertStoreDirection(link.direction());
-                    return new NodeDirection(node, agvDirection);
-                }
-            }
-        }
-        return null;
-    }
-
-    /**
-     * 根据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++) {
-            State current = path.get(i);
-
-            // 如果是旋转动作
-            if (current.node().equals(prevState.node())) {
-                float angle = PathUtils.getRequiredDirection(current.direction());
-                sequence.addRotationTo(angle);
-            }
-            // 移动动作 - 检查是否需要后退
-            else {
-                // 检查移动方向
-                LCCDirection moveDir = PathUtils.calculateMoveDirection(prevState.node(), current.node());
-
-                // 如果移动方向与车头方向相反，需要后退
-                boolean isBackward = (current.direction() == PathUtils.getOppositeDirection(moveDir));
-
-                if (isBackward) {
-                    sequence.addMoveBackward(current.node().id());
-                } else {
-                    sequence.addMoveTo(current.node().id());
-                }
-            }
-            prevState = current;
-        }
-    }
-
-
-    // 辅助记录类
-    private record NodeDirection(Node node, LCCDirection direction) {
-    }
-}

servo/src/main/java/com/galaxis/rcs/plan/task/LoadTask.java

@@ -0,0 +1,15 @@
+package com.galaxis.rcs.plan.task;
+
+import com.galaxis.rcs.common.entity.StoreLocation;
+
+/**
+ * 装入任务
+ */
+public record LoadTask(
+    String agv,
+    int priority,
+    StoreLocation to
+) {
+}
+
+

servo/src/main/java/com/galaxis/rcs/plan/task/MoveTask.java

@@ -1,8 +1,11 @@
 package com.galaxis.rcs.plan.task;
 
+import com.galaxis.rcs.common.enums.LCCDirection;
+
 public record MoveTask(
     String agv,
     String targetWayPointId,
+    LCCDirection targetDirection,
     int priority
 ) {
 }

servo/src/main/java/com/galaxis/rcs/plan/task/UnloadTask.java

@@ -0,0 +1,15 @@
+package com.galaxis.rcs.plan.task;
+
+import com.galaxis.rcs.common.entity.StoreLocation;
+
+/**
+ * 卸载任务
+ */
+public record UnloadTask(
+    String agv,
+    int priority,
+    StoreLocation to
+) {
+}
+
+

servo/src/main/java/com/galaxis/rcs/ptr/AgvEventManager.java

@@ -2,7 +2,6 @@ package com.galaxis.rcs.ptr;
 
 import com.galaxis.rcs.common.enums.AgvEventType;
 import com.yvan.logisticsModel.LogisticsRuntime;
-import com.yvan.logisticsModel.PtrAgvItem;
 import lombok.extern.slf4j.Slf4j;
 
 import java.util.List;

servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java

@@ -11,7 +11,6 @@ import com.google.common.collect.Maps;
 import com.google.common.collect.Sets;
 import com.yvan.logisticsEnv.EnvPayload;
 import com.yvan.logisticsModel.LogisticsRuntime;
-import com.yvan.logisticsModel.PtrAgvItem;
 import lombok.SneakyThrows;
 import lombok.extern.slf4j.Slf4j;
 import org.clever.core.BannerUtils;
@@ -140,6 +139,7 @@ public class AmrMessageHandler {
         switch (ArmMessageType.fromValue(id)) {
             case AMR_TASK_COMPLETED:
                 AmrMessage<AmrTaskCompletedMessage> taskCompletedMessage = JacksonUtils.parse(json, typeRef20010Message);
+                handleTaskCompletedMessage(agvItem, taskCompletedMessage.content);
                 break;
             case AMR_TASK_STATUS:
                 handleTaskStatusMessage(agvItem, jw, json);
@@ -419,6 +419,11 @@ public class AmrMessageHandler {
         updateRedisNetDelay(agvItem.getId(), netDelay);
     }
 
+
+    private void handleTaskCompletedMessage(PtrAgvItem agvItem, AmrTaskCompletedMessage message) {
+        agvItem.taskCompleted(message.CurX, message.CurY, message.CurDirection, 4);
+    }
+
     private void handleLandmarkMessage(PtrAgvItem agvItem, AmrLandmarkMessage message) {
         // 这是源逻辑，CurLogicX / CurLogicY / CurDirection 需要到 PtrAgvItem 中更新, 因为要触发事件
         agvItem.x = message.X;
@@ -488,6 +493,7 @@ public class AmrMessageHandler {
                 // agvItem.logicY = completedMessage.Info.CurLogicY;
                 // // agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation;
                 // agvItem.direction = taskCompleted.content.Info.CurDirection;
+//                agvItem.updateTask(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection, 4);
                 agvItem.updatePosition(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection);
                 break;
             case 8:

servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnector.java

@@ -0,0 +1,5 @@
+package com.galaxis.rcs.ptr;
+
+public abstract class PtrAgvConnector {
+
+}

servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java → servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnectorThread.java

@@ -1,15 +1,16 @@
-package com.yvan.logisticsModel;
+package com.galaxis.rcs.ptr;
 
 import com.fasterxml.jackson.core.JsonProcessingException;
 import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector;
 import com.galaxis.rcs.ptr.sendEntity.RcsTaskMessage;
-import com.yvan.logisticsMonitor.task.DeviceTask;
+import com.yvan.logisticsModel.LogisticsRuntime;
 import lombok.extern.slf4j.Slf4j;
 import org.eclipse.paho.mqttv5.common.MqttException;
 
 import java.util.ArrayList;
 import java.util.List;
 import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.locks.LockSupport;
 
 @Slf4j
 public class PtrAgvConnectorThread extends Thread {
@@ -40,8 +41,6 @@ public class PtrAgvConnectorThread extends Thread {
             int taskCount = 0;
 
             PtrAgvDeviceTask startTask = null;
-            PtrAgvDeviceTask currentTask = null;
-            PtrAgvDeviceTask nextTask = null;
             RcsTaskMessage taskMessage = null;
 
             // 计算中的任务
@@ -55,26 +54,10 @@ public class PtrAgvConnectorThread extends Thread {
                     }
                 }
 
-                // for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
+                PtrAgvDeviceTask currentTask = this.ptrAgvItem.deviceTaskQueue.take();
-                //     if (task.taskGroupStatus < 3) {
+                if (startTask == null) {
-                //         LockSupport.park(); // 阻塞当前线程
+                    startTask = currentTask;
-                //         break;
-                //     }
-                // }
-
-                // 从队列中获取任务，如果队列为空则阻塞等待
-                if (nextTask == null) {
-                    nextTask = this.ptrAgvItem.deviceTaskQueue.take();
-
-                } else {
-                    currentTask = nextTask;
-                }
-
-                if (startTask == null && !nextTask.isLastTask) {
                     taskMessage = new RcsTaskMessage(this.logisticsRuntime);
-                    startTask = nextTask;
-                    currentTask = nextTask;
-                    startTask.seqNo = taskMessage.SeqNo;
                     taskMessage.OperationType = startTask.operationType;
                     taskMessage.PickMode = startTask.pickMode;
                     taskMessage.GoNow = true;
@@ -82,33 +65,28 @@ public class PtrAgvConnectorThread extends Thread {
                     taskMessage.StartY = startTask.startPoint.logicY;
                     taskMessage.Link = new ArrayList<>();
                 }
-
-                if (currentTask == nextTask && taskMessage != null) {
                 currentTask.seqNo = taskMessage.SeqNo;
                 RcsTaskMessage.LinkData link = new RcsTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed);
                 taskMessage.Link.add(link);
                 taskCount++;
                 distance += euclideanDistance(currentTask.startPoint.tf[0], currentTask.endPoint.tf[0]);
                 computingTaskList.add(currentTask);
-                }
 
-                // 组织设备任务并发送
-                else if (taskMessage != null &&
-                    (currentTask.operationType > 0 // 当前任务不是移动任务
-                        || currentTask.pickMode > 0 // 当前作业不是默认作业（无）
-                        || nextTask.isLastTask  // 下一个任务是结束任务
-                        || (((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction) && taskMessage.Link.size() > 0) // 下一个任务和开始任务方向不一致
-                        // 单向移动距离大于2m时并且点位数量大于1
-                        || (distance > 2 && taskCount > 1))) {
 
+                PtrAgvDeviceTask nextTask = this.ptrAgvItem.deviceTaskQueue.peek();
+                if (currentTask.isGroupEnd
+                    || (taskMessage.Link.size() > 0 && nextTask != null && ((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction)) // 下一个任务和开始任务方向不一致
+                    // 单向移动距离大于2m时并且点位数量大于1
+                    || (distance > 2 && taskCount > 1)) {
                     taskMessage.OperationType = currentTask.operationType;
                     taskMessage.PickMode = currentTask.pickMode;
-                    taskMessage.EndX = currentTask.endPoint.logicX;
+                    taskMessage.EndX = currentTask.groupEndPoint.logicX;
-                    taskMessage.EndY = currentTask.endPoint.logicY;
+                    taskMessage.EndY = currentTask.groupEndPoint.logicY;
                     taskMessage.GoodsSlotDirection = currentTask.goodsSlotDirection;
                     taskMessage.GoodsSlotHeight = currentTask.goodsSlotHeight;
                     taskMessage.LinkCounts = (short) taskMessage.Link.size();
-
+                    taskMessage.ChargeDirection = currentTask.chargeDirection;
+                    taskMessage.ChargeLocation = currentTask.chargeLocation;
                     try {
                         // 发送任务
                         this.__currentTaskSeqNo = taskMessage.SeqNo;
@@ -121,18 +99,27 @@ public class PtrAgvConnectorThread extends Thread {
                         }
                         computingTaskList.clear();
                     } catch (MqttException | JsonProcessingException e) {
-                        log.error("Cl2DeviceConnector robotMove: id=" + ptrAgvItem.getId() + ", task=" + currentTask.planTaskId, e);
+                        log.error("Cl2DeviceConnector robotMove: id=" + ptrAgvItem.getId() + ", task=" + currentTask.movePlanTaskId, e);
                     }
                     distance = 0;
                     taskCount = 0;
                     taskMessage = null;
                     startTask = null;
-                }
 
-                if (nextTask == currentTask || nextTask.isLastTask) {
+                    if (currentTask.isGroupEnd) {
-                    nextTask = null;
+                        // 当一组任务结束时，阻塞当前线程，等当前任务执组行完毕后在外部线程中唤醒
+                        LockSupport.park(); // 阻塞当前线程
+                    } else {
+                        for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
+                            if (task.taskGroupStatus < 4) {
+                                LockSupport.park(); // 阻塞当前线程
+                                break;
                             }
                         }
+                    }
+                }
+
+            }
         } catch (InterruptedException e) {
             System.out.println("Connector thread interrupted for executor: " + this.ptrAgvItem.getId());
         } finally {

servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java → servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java

@@ -1,4 +1,9 @@
-package com.yvan.logisticsModel;
+package com.galaxis.rcs.ptr;
+
+import com.yvan.logisticsModel.StaticItem;
+
+import java.util.HashSet;
+import java.util.Set;
 
 public class PtrAgvDeviceTask {
     //该段目标点X坐标	UInt16 逻辑单位，乘以一定系数才是物理距离
@@ -13,6 +18,10 @@ public class PtrAgvDeviceTask {
     public StaticItem startPoint;
     // 该段目标点
     public StaticItem endPoint;
+    // 分组起始点
+    public StaticItem groupStartPoint;
+    // 分组目标点
+    public StaticItem groupEndPoint;
     // 作业类型 UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业）
     public short operationType;
     // 提升移栽货物拣货模式	UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
@@ -24,20 +33,30 @@ public class PtrAgvDeviceTask {
     // 目标货位相对于地面的绝对高度
     public int goodsSlotHeight;
 
+    //充电桩朝向UseBriefLocation	UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+    public Short chargeDirection;
+    // 充电工位坐标和充电桩之间距离	UInt16 单位：mm
+    public Integer chargeLocation;
+    // 目标货架编号 String 仅做校验使用（仅接货用）
+    public String storageRacksNo;
+    // 结束朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+    public Short endDirection;
 
-    /** 规划ID */
+    /** 移动规划ID */
-    public Long planTaskId;
+    public Long movePlanTaskId;
+    /** 转动取货放货充电等规划ID */
+    public Set<Long> planTaskIdSet = new HashSet<>();
     /** 业务任务ID */
     public Long bizTaskId;
     // 作业序号 发送给小车的作业序号
     public int seqNo;
 
-    // 任务状态 0创建 1发送成功 2接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更
+    // 任务状态 0创建 1任务模式改变 2任务接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更
     public int taskStatus = 0;
     public int taskGroupStatus = 0;
 
-    // 是否最后任务
+    // 任务分组结束标记 生成报文时作为判断依据
-    public boolean isLastTask = false;
+    public boolean isGroupEnd = false;
     public int checkLogicX;
     public int checkLogicY;
 }

servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java → servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java

@@ -1,4 +1,4 @@
-package com.yvan.logisticsModel;
+package com.galaxis.rcs.ptr;
 
 import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.galaxis.rcs.common.entity.RcsTaskPlan;
@@ -8,7 +8,6 @@ import com.galaxis.rcs.common.enums.PlanTaskStatus;
 import com.galaxis.rcs.common.enums.PlanTaskType;
 import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector;
 import com.galaxis.rcs.plan.PlanTaskSequence;
-import com.galaxis.rcs.ptr.*;
 import com.galaxis.rcs.ptr.receiveEntity.AmrHeartbeatMessage;
 import com.galaxis.rcs.ptr.receiveEntity.base.CurBatteryData;
 import com.galaxis.rcs.ptr.sendEntity.RcsConfigMessage;
@@ -16,6 +15,9 @@ import com.galaxis.rcs.ptr.sendEntity.RcsSRMessage;
 import com.galaxis.rcs.ptr.sendEntity.RcsSetLocationMessage;
 import com.google.common.base.Joiner;
 import com.google.common.collect.Queues;
+import com.yvan.logisticsModel.ExecutorItem;
+import com.yvan.logisticsModel.LogisticsRuntime;
+import com.yvan.logisticsModel.StaticItem;
 import lombok.SneakyThrows;
 import lombok.extern.slf4j.Slf4j;
 import org.clever.core.BannerUtils;
@@ -81,7 +83,7 @@ public abstract class PtrAgvItem extends ExecutorItem {
      * 当前执行的任务规划列表
      */
     @JsonIgnore
-    private volatile PlanTaskSequence planTaskSequence;
+    public volatile PlanTaskSequence planTaskSequence;
 
     /**
      * 当前执行的设备任务列表
@@ -183,15 +185,16 @@ public abstract class PtrAgvItem extends ExecutorItem {
 
     @SneakyThrows
     public synchronized void cancelTask() {
-        if (planTaskSequence == null) {
-            throw new IllegalStateException("No active task to cancel");
-        }
-
         // 发送取消指令
         amrMessageHandler.sendCmdCancelTask(this.getId(), this.connectorThread.getCurrentTaskSeqNo());
 
+        if (planTaskSequence != null) {
             planTaskSequence = null;
+        }
+        if (!deviceTaskQueue.isEmpty()) {
             deviceTaskQueue.clear();
+        }
+
         fireEvent(AgvEventType.PLAN_CANCEL, this);
     }
 
@@ -239,6 +242,30 @@ public abstract class PtrAgvItem extends ExecutorItem {
         return true;
     }
 
+    public void taskCompleted(int logicX, int logicY, short direction, int taskStatus) {
+
+        updatePosition(logicX, logicY, direction);
+        // 查找当前分组任务
+        for (PtrAgvDeviceTask task : runningDeviceTaskList) {
+            task.taskGroupStatus = taskStatus;
+            if (taskStatus == 4) {
+                // 更新计划任务
+                List<RcsTaskPlan> planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList();
+                for (RcsTaskPlan planTask : planTaskList) {
+                    planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
+                    planTaskSequence.savePlanTask(planTask);
+                }
+            }
+        }
+
+        if (planTaskSequence != null && planTaskSequence.isAllCompleted()) {
+            fireEvent(AgvEventType.PLAN_COMPLETE, this);
+            this.runningDeviceTaskList.clear();
+            planTaskSequence = null;
+        }
+        LockSupport.unpark(connectorThread);
+    }
+
     public void updatePosition(int logicX, int logicY, short direction) {
         int oldX = this.logicX;
         int oldY = this.logicY;
@@ -273,7 +300,7 @@ public abstract class PtrAgvItem extends ExecutorItem {
 
             for (int i = 0; i < runningDeviceTaskList.size(); i++) {
                 PtrAgvDeviceTask task = runningDeviceTaskList.get(i);
-                if (task.checkLogicX == logicX && task.checkLogicY == logicY) {
+                if (task.checkLogicX == logicX && task.checkLogicY == logicY && task.taskStatus < 4) {
                     finishTargetIndex = i;
                     break;
                 }
@@ -284,13 +311,11 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 // 标记前面的任务都完成了
                 for (int i = 0; i <= finishTargetIndex; i++) {
                     PtrAgvDeviceTask task = runningDeviceTaskList.get(i);
-
                     task.taskStatus = 4; // 标记为完成
-                    task.taskGroupStatus = 4; // 标记为任务组完成
                     fireEvent(AgvEventType.DEVICE_TASK_COMPLETE, this, task);
 
                     // 更新计划任务
-                    RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.planTaskId);
+                    RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId);
                     if (planTask != null) {
                         planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
                         planTaskSequence.savePlanTask(planTask);
@@ -299,6 +324,7 @@ public abstract class PtrAgvItem extends ExecutorItem {
 
                 if (planTaskSequence.isAllCompleted()) {
                     fireEvent(AgvEventType.PLAN_COMPLETE, this);
+                    this.runningDeviceTaskList.clear();
                     planTaskSequence = null;
                 }
             }
@@ -315,10 +341,14 @@ public abstract class PtrAgvItem extends ExecutorItem {
             )
         });
 
-        if (needCompute) {
+        if (needCompute && this.runningDeviceTaskList.size() > 0) {
+            int index = this.runningDeviceTaskList.size() - 1;
+            PtrAgvDeviceTask task = this.runningDeviceTaskList.get(index);
+            if (task.groupEndPoint != task.endPoint) {
                 LockSupport.unpark(connectorThread);
             }
         }
+    }
 
     /**
      * 更新设备任务状态 暂时没有处理任务取消相关的状态
@@ -447,13 +477,16 @@ public abstract class PtrAgvItem extends ExecutorItem {
         if (startPoint == null) {
             log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, agv当前点位为空 地图上没有标记", this.getId(), sequence.bizTask.getBizTaskId());
         }
+        StaticItem groupStartPoint = startPoint;
 
+        Set<Long> rotationPlanTaskIdSet = new HashSet<>();
 
         // 生成移动报文
         List<PtrAgvDeviceTask> deviceTaskList = new ArrayList<>();
         List<Map<String, Object>> linkStore = null;
         // 检查 planList 是不是全都是我的任务
-        for (RcsTaskPlan plan : sequence.taskList) {
+        for (int i = 0; i < sequence.taskList.size(); i++) {
+            RcsTaskPlan plan = sequence.taskList.get(i);
             String endPointId = plan.getTargetId();
 
             if (plan.getPlanType().equals(PlanTaskType.MOVE.toString()) || plan.getPlanType().equals(PlanTaskType.MOVE_BACKWARD.toString())) {
@@ -462,18 +495,22 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 linkStore = (List<Map<String, Object>>) pointItem.dt.get("linkStore");
                 int d = -1;
                 if (startPoint.logicX == pointItem.logicX && startPoint.logicY != pointItem.logicY) {
-                    d = pointItem.logicY > startPoint.logicY ? CDirection.db : CDirection.dt;
+                    d = pointItem.logicY >= startPoint.logicY ? CDirection.db : CDirection.dt;
                     if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) {
                         throw new RuntimeException("方向错误");
                     }
 
                 } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX != pointItem.logicX) {
-                    d = pointItem.logicX > startPoint.logicX ? CDirection.dr : CDirection.dl;
+                    d = pointItem.logicX >= startPoint.logicX ? CDirection.dr : CDirection.dl;
                     if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) {
                         throw new RuntimeException("方向错误");
                     }
                     // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX());
 
+                } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX == pointItem.logicX) {
+                    d = direction;
+                    // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX());
+
                 } else {
                     throw new RuntimeException("无法识别的点位关系");
                 }
@@ -486,7 +523,9 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 deviceTask.startPoint = startPoint;
                 deviceTask.endPoint = pointItem;
                 deviceTask.bizTaskId = plan.getBizTaskId();
-                deviceTask.planTaskId = plan.getPlanTaskId();
+                deviceTask.movePlanTaskId = plan.getPlanTaskId();
+                deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+                rotationPlanTaskIdSet.clear();
                 // 行走任务完成后，检查用的字段
                 deviceTask.checkLogicX = pointItem.logicX;
                 deviceTask.checkLogicY = pointItem.logicY;
@@ -516,12 +555,33 @@ public abstract class PtrAgvItem extends ExecutorItem {
                 } else if (r >= 225 && r < 315) {
                     direction = CDirection.db;
                 }
+                rotationPlanTaskIdSet.add(plan.getPlanTaskId());
 
             } else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) {
 
+                if (deviceTaskList.isEmpty()) {
+                    PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
+                    deviceTask.x = startPoint.logicX;
+                    deviceTask.y = startPoint.logicY;
+                    deviceTask.speed = speed;
+                    deviceTask.direction = direction;
+                    deviceTask.pickMode = 0;
+                    deviceTask.startPoint = startPoint;
+                    deviceTask.endPoint = startPoint;
+                    deviceTask.bizTaskId = plan.getBizTaskId();
+                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
+                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+                    rotationPlanTaskIdSet.clear();
+                    // 行走任务完成后，检查用的字段
+                    deviceTask.checkLogicX = startPoint.logicX;
+                    deviceTask.checkLogicY = startPoint.logicY;
+                    deviceTaskList.add(deviceTask);
+                    linkStore = (List<Map<String, Object>>) startPoint.dt.get("linkStore");
+                }
                 PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                 deviceTask.operationType = COperationType.transplantLoadAndUnload;
                 deviceTask.pickMode = CPickMode.load;
+                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
                 //处理取货高度
                 StaticItem storeItem = runtime.getStaticItemById(endPointId);
                 Map<String, Object> storeItemRaw = storeItem.dt;
@@ -556,17 +616,43 @@ public abstract class PtrAgvItem extends ExecutorItem {
                     }
                 }
 
+                // 标记任务分组结束
+                deviceTask.isGroupEnd = true;
+                deviceTask.groupEndPoint = deviceTask.endPoint;
+                deviceTask.groupStartPoint = groupStartPoint;
+                groupStartPoint = deviceTask.endPoint;
+
             } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) {
+                if (deviceTaskList.isEmpty()) {
+                    PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
+                    deviceTask.x = startPoint.logicX;
+                    deviceTask.y = startPoint.logicY;
+                    deviceTask.speed = speed;
+                    deviceTask.direction = direction;
+                    deviceTask.pickMode = 0;
+                    deviceTask.startPoint = startPoint;
+                    deviceTask.endPoint = startPoint;
+                    deviceTask.bizTaskId = plan.getBizTaskId();
+                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
+                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+                    rotationPlanTaskIdSet.clear();
+                    // 行走任务完成后，检查用的字段
+                    deviceTask.checkLogicX = startPoint.logicX;
+                    deviceTask.checkLogicY = startPoint.logicY;
+                    deviceTaskList.add(deviceTask);
+                    linkStore = (List<Map<String, Object>>) startPoint.dt.get("linkStore");
+                }
                 PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                 deviceTask.operationType = COperationType.transplantLoadAndUnload;
                 deviceTask.pickMode = CPickMode.unload;
+                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
                 // 处理卸货高度
                 StaticItem storeItem = runtime.getStaticItemById(endPointId);
                 Map<String, Object> storeItemRaw = storeItem.dt;
-                if (storeItemRaw.containsKey("bays") && storeItemRaw.containsKey("level")) {
+                if (storeItemRaw.containsKey("bays")) {
                     List<Map<String, Object>> bays = (List<Map<String, Object>>) storeItemRaw.get("bays");
                     Map<String, Object> bay = bays.get(plan.getTargetBay());
-                    List<Double> levelHeight = (List<Double>) bay.get("levels");
+                    List<Double> levelHeight = (List<Double>) bay.get("levelHeight");
                     deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
                 } else {
                     deviceTask.goodsSlotHeight = 1;
@@ -593,11 +679,42 @@ public abstract class PtrAgvItem extends ExecutorItem {
                         }
                     }
                 }
+                // 标记任务分组结束
+                deviceTask.isGroupEnd = true;
+                deviceTask.groupEndPoint = deviceTask.endPoint;
+                deviceTask.groupStartPoint = groupStartPoint;
+                groupStartPoint = deviceTask.endPoint;
 
             } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) {
+                if (deviceTaskList.isEmpty()) {
+                    PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
+                    deviceTask.x = startPoint.logicX;
+                    deviceTask.y = startPoint.logicY;
+                    deviceTask.speed = speed;
+                    deviceTask.direction = direction;
+                    deviceTask.pickMode = 0;
+                    deviceTask.startPoint = startPoint;
+                    deviceTask.endPoint = startPoint;
+                    deviceTask.bizTaskId = plan.getBizTaskId();
+                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
+                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+                    rotationPlanTaskIdSet.clear();
+                    // 行走任务完成后，检查用的字段
+                    deviceTask.checkLogicX = startPoint.logicX;
+                    deviceTask.checkLogicY = startPoint.logicY;
+                    deviceTaskList.add(deviceTask);
+                }
                 PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                 deviceTask.operationType = COperationType.charge;
+                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
                 // 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm)
+                deviceTask.chargeDirection = 0;
+                deviceTask.chargeLocation = 200;
+                // 标记任务分组结束
+                deviceTask.isGroupEnd = true;
+                deviceTask.groupEndPoint = deviceTask.endPoint;
+                deviceTask.groupStartPoint = groupStartPoint;
+                groupStartPoint = deviceTask.endPoint;
             }
 
             if (!plan.getExecutorId().equals(this.getId())) {
@@ -605,10 +722,41 @@ public abstract class PtrAgvItem extends ExecutorItem {
             }
 
         }
-        // 添加结束任务
+
-        PtrAgvDeviceTask deviceTaskEnd = new PtrAgvDeviceTask();
+        if (deviceTaskList.size() <= 0) {
-        deviceTaskEnd.isLastTask = true;
+            return;
-        deviceTaskList.add(deviceTaskEnd);
+        }
+
+        // 标记任务分组结束
+        PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
+        deviceTask.groupEndPoint = deviceTask.endPoint;
+        deviceTask.groupStartPoint = groupStartPoint;
+        deviceTask.isGroupEnd = true;
+        // 最后一个规划任务为旋转时需要添加一个endDirection
+        if (rotationPlanTaskIdSet.size() > 0) {
+            deviceTask.endDirection = direction;
+            deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
+            rotationPlanTaskIdSet.clear();
+        }
+
+        // 反向标记任务组
+        int lastIndex = deviceTaskList.size() - 1;
+        for (int i = deviceTaskList.size() - 1; i >= 0; i--) {
+            PtrAgvDeviceTask d = deviceTaskList.get(i);
+            if (d.isGroupEnd) {
+                lastIndex = i;
+            } else {
+                d.operationType = deviceTaskList.get(lastIndex).operationType;
+                d.pickMode = deviceTaskList.get(lastIndex).pickMode;
+                d.groupStartPoint = deviceTaskList.get(lastIndex).groupStartPoint;
+                d.groupEndPoint = deviceTaskList.get(lastIndex).groupEndPoint;
+                d.goodsSlotHeight = deviceTaskList.get(lastIndex).goodsSlotHeight;
+                d.goodsSlotDirection = deviceTaskList.get(lastIndex).goodsSlotDirection;
+                d.chargeDirection = deviceTaskList.get(lastIndex).chargeDirection;
+                d.chargeLocation = deviceTaskList.get(lastIndex).chargeLocation;
+            }
+        }
+
 
 //        planQueue.addAll(sequence.taskList);
         deviceTaskQueue.addAll(deviceTaskList);

servo/src/main/java/com/galaxis/rcs/ptr/receiveEntity/AmrTaskCompletedMessage.java

@@ -11,10 +11,10 @@ import com.galaxis.rcs.ptr.receiveEntity.base.SummaryData;
 @JsonIgnoreProperties(ignoreUnknown = true)
 public class AmrTaskCompletedMessage extends AmrCommonMessage {
 
-    // 当前X坐标 Double 当前实际位置在地图坐标系中的X坐标
+    // 当前X坐标 UInt16
-    public double CurX;
+    public int CurX;
     // 当前Y坐标 Double 当前实际位置在地图坐标系中的Y坐标
-    public double CurY;
+    public int CurY;
     // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
     public short CurDirection;
     // 当前方向 Double 角度

servo/src/main/java/com/galaxis/rcs/ptr/sendEntity/RcsTaskMessage.java

@@ -40,6 +40,9 @@ public class RcsTaskMessage {
     public Integer GoodsSlotHeight;
     // 目标货位朝向 UInt8	朝向定义与充电桩朝向相同。 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
     public Short GoodsSlotDirection;
+    // 结束朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
+    public Short EndDirection;
+
     // 多机构^[1]^的拣货模式 UInt8[3] 数组形式，意义同"PickMode"
     public List<Short> MPickMode;
     // 多机构^[1]^的目标货位高度 UInt16[3] 单位：mm

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

@@ -12,22 +12,22 @@ import java.util.Map;
  */
 @Data
 public abstract class BaseItem {
-    final String id;
+    public final String id;
-    final String t;
+    public final String t;
 
     /**
      * 变换矩阵, 3x3矩阵, tf[0]=position,采用 X向右增长，Y轴向屏幕外增长，Z向下增长, tf[1]=rotation, tf[2]=scale
      */
-    final float[][] tf;
+    public final float[][] tf;
 
     /**
      * 物品的自定义数据
      */
     @JsonIgnore
-    final Map<String, Object> dt;
+    public final Map<String, Object> dt;
 
     @JsonIgnore
-    final Map<String, Object> raw;
+    public final Map<String, Object> raw;
 
     @JsonIgnore
     public final LogisticsRuntime runtime;
@@ -60,16 +60,4 @@ public abstract class BaseItem {
 
         this.dt = (Map<String, Object>) map.get("dt");
     }
-
-    public float getTransformationX() {
-        return tf[0][0];
-    }
-
-    public float getTransformationY() {
-        return tf[0][2];
-    }
-
-    public float getTransformationZ() {
-        return tf[0][1];
-    }
 }

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

@@ -2,8 +2,8 @@ package com.yvan.logisticsModel;
 
 import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.galaxis.rcs.connector.cl2.Cl2Item;
-import com.galaxis.rcs.plan.path2.NavigationGraph;
+import com.galaxis.rcs.plan.path.NavigationGraph;
-import com.galaxis.rcs.plan.path2.PtrPathPlanner;
+import com.galaxis.rcs.plan.path.PtrPathPlanner;
 import com.galaxis.rcs.ptr.AgvEventManager;
 import com.galaxis.rcs.ptr.AmrMessageHandler;
 import com.galaxis.rcs.task.TaskDispatchFactory;

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

@@ -1,7 +0,0 @@
-package com.yvan.logisticsModel;
-
-import com.galaxis.rcs.common.entity.RcsTaskPlan;
-
-public abstract class PtrAgvConnector {
-
-}

servo/src/main/java/com/yvan/workbench/controller/RcsController.java

@@ -1,15 +1,12 @@
 package com.yvan.workbench.controller;
 
-import com.galaxis.rcs.RCSService;
+import com.galaxis.rcs.common.entity.*;
-import com.galaxis.rcs.common.entity.RcsTaskBiz;
+import com.galaxis.rcs.common.enums.*;
-import com.galaxis.rcs.common.entity.StoreLocation;
-import com.galaxis.rcs.common.enums.BizTaskStatus;
-import com.galaxis.rcs.common.enums.BizTaskType;
-import com.galaxis.rcs.common.enums.LCCDirection;
 import com.galaxis.rcs.plan.PlanTaskSequence;
-import com.galaxis.rcs.plan.task.CarryTask;
+import com.galaxis.rcs.plan.task.*;
-import com.galaxis.rcs.plan.task.MoveTask;
+import com.galaxis.rcs.ptr.PtrAgvItem;
 import com.google.common.base.Strings;
+import com.google.common.collect.Maps;
 import com.yvan.logisticsModel.*;
 import com.yvan.workbench.model.entity.Model;
 import org.apache.commons.lang3.NotImplementedException;
@@ -20,118 +17,48 @@ import org.clever.web.mvc.annotation.RequestBody;
 import java.util.Map;
 
 public class RcsController {
-//    /**
-//     * 后台机器人移动
-//     * @param agvId 机器人ID
-//     * @param targetWayPointId 目标路径点ID
-//     * @param option 选项
-//     */
-//    agvMove(agvId: string, targetWayPointId: string, option: any = {}): Promise<ServerResponse<boolean>>
-//
-//    /**
-//     * 后台机器人搬运（库存点 -> 库存点）
-//     * @param agvId 机器人ID
-//     * @param fromStoreLoc 库存点ID
-//     * @param targetStoreLoc 目标库存点ID
-//     * @param option 其他选项
-//     */
-//    agvCarry(agvId: string, fromStoreLoc: string, targetStoreLoc: string, option: any = {}): Promise<ServerResponse<boolean>>
-//
-//    /**
-//     * 机器人充电
-//     * @param agvId 机器人ID
-//     * @param chargerId 充电桩ID
-//     * @param option 其他选项
-//     */
-//    agvToCharger(agvId: string, chargerId: string, option: any = {}): Promise<ServerResponse<boolean>>
-
     static final SnowFlake snowFlake = new SnowFlake();
 
     public static Model<Object> agvMove(@RequestBody Map<String, Object> params) {
-        String projectUUID = Conv.asString(params.get("projectUUID"));
+        Object ret = getCommonParamAndCreateBizTask(params);
-        String catalogCode = Conv.asString(params.get("catalogCode"));
+        if (ret instanceof Model) {
-        Long envId = Conv.asLong(params.get("envId"));
+            // 异常
-        String agvId = Conv.asString(params.get("agvId"));
+            return (Model<Object>) ret;
-        String targetWayPointId = Conv.asString(params.get("targetWayPointId"));
-        Map<String, Object> option = (Map<String, Object>) params.get("option");
-
-        if (Strings.isNullOrEmpty(agvId)) {
-            return Model.newFail("agvId Must not be empty");
         }
+        RcsCommonParam ps = (RcsCommonParam) ret;
+
+        String targetWayPointId = Conv.asString(params.get("targetWayPointId"));
         if (Strings.isNullOrEmpty(targetWayPointId)) {
             return Model.newFail("targetWayPointId Must not be empty");
         }
+        String targetDirection = Conv.asString(params.get("targetDirection"));
+        if (Strings.isNullOrEmpty(targetDirection)) {
+            targetDirection = ps.fromDirection.toString();
+        }
 
-        LogisticsRuntime runtime = LogisticsRuntimeService.INSTANCE.getByProjectEnv(projectUUID, envId);
+        StaticItem toItem = ps.runtime.getStaticItemById(targetWayPointId);
-        StaticItem toItem = runtime.getStaticItemById(targetWayPointId);
         if (toItem == null) {
             return Model.newFail("target wayPoint not found!");
         }
 
-        ExecutorItem executorItem = runtime.executorItemMap.get(agvId);
+        ps.bizTask.setTaskTo(targetWayPointId);
-        if (executorItem == null) {
-            return Model.newFail("executor not found: " + agvId);
-        }
-        if (!(executorItem instanceof PtrAgvItem)) {
-            return Model.newFail("executor is not a PtrAgvItem id=" + agvId);
-        }
-
-        // 获取机器人当前所在位置, 也可以前端强制指定
-        // forceStartWayPointId: '6_2', forceStartDirectior: 'right'
-        StaticItem fromItem = null;
-        LCCDirection fromDirection = null;
-        if (option.get("forceStartWayPointId") != null) {
-            fromItem = runtime.getStaticItemById(Conv.asString(option.get("forceStartWayPointId")));
-        } else {
-            fromItem = runtime.getStaticItemByLogicXY(((PtrAgvItem) executorItem).logicX, ((PtrAgvItem) executorItem).logicY);
-        }
-        if (option.get("forceStartDirection") != null) {
-            fromDirection = LCCDirection.fromString(Conv.asString(option.get("forceStartDirection")));
-        } else {
-            fromDirection = ((PtrAgvItem) executorItem).getLCCDirection();
-        }
-
-        if (fromItem == null) {
-            return Model.newFail("PtrAgvItem not found at current location: " + ((PtrAgvItem) executorItem).logicX + "_" + ((PtrAgvItem) executorItem).logicY + ", id=" + agvId);
-        }
-        if (fromDirection == null) {
-            return Model.newFail("PtrAgvItem unkown direction id=" + agvId);
-        }
-
-        RcsTaskBiz bizTask = new RcsTaskBiz();
-        bizTask.setBizTaskId(snowFlake.nextId());
-        bizTask.setEnvId(envId);
-        bizTask.setBizType(BizTaskType.MOVE.toString());
-        bizTask.setLpn("N/A");
-        bizTask.setPriority(Conv.asInteger(option.get("priority"), 1));
-        bizTask.setTaskFrom(fromItem.getId());
-        bizTask.setTaskTo(targetWayPointId);
-        bizTask.setAllocatedExecutorId(agvId);
-        bizTask.setBizTaskPayload("N/A");
-        bizTask.setBizTaskErrorInfo("N/A");
-        bizTask.setBizTaskDescription("N/A");
-        bizTask.setBizTaskStatus(BizTaskStatus.WAITING_FOR_DISPATCH.toString());
-
-        PlanTaskSequence planSequence = new PlanTaskSequence(agvId, runtime, bizTask, "demo");
 
         MoveTask moveTask = new MoveTask(
-            agvId, bizTask.getTaskTo(), bizTask.getPriority()
+            ps.agvId, ps.bizTask.getTaskTo(), LCCDirection.fromString(targetDirection, ps.fromDirection), ps.bizTask.getPriority()
         );
 
-        runtime.pathPlannerMap.get(executorItem.getT())
+        ps.runtime.pathPlannerMap.get(ps.agv.getT())
-            .planMoveTask(planSequence, fromItem.getId(), fromDirection, moveTask);
+            .planMoveTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, moveTask);
 
-        PtrAgvItem agvItem = (PtrAgvItem) executorItem;
+        ps.agv.logicX = ps.fromItem.logicX;
-        agvItem.logicX = fromItem.logicX;
+        ps.agv.logicY = ps.fromItem.logicY;
-        agvItem.logicY = fromItem.logicY;
+        ps.agv.dispatchTask(ps.planSequence);
-        agvItem.dispatchTask(planSequence);
 
-        return Model.newSuccess(planSequence.toPrettyMap());
+        return Model.newSuccess(ps.planSequence.toPrettyMap());
     }
 
     public static Model<Object> agvInfo(@RequestBody Map<String, Object> params) {
         String projectUUID = Conv.asString(params.get("projectUUID"));
-        String catalogCode = Conv.asString(params.get("catalogCode"));
         Long envId = Conv.asLong(params.get("envId"));
         String agvId = Conv.asString(params.get("agvId"));
 
@@ -152,18 +79,111 @@ public class RcsController {
         return Model.newSuccess(executorItem);
     }
 
+    public static Model<Object> agvUnload(@RequestBody Map<String, Object> params) {
+        Object ret = getCommonParamAndCreateBizTask(params);
+        if (ret instanceof Model) {
+            // 异常
+            return (Model<Object>) ret;
+        }
+        RcsCommonParam ps = (RcsCommonParam) ret;
+
+        String targetStoreLoc = Conv.asString(params.get("targetStoreLoc"));
+        if (Strings.isNullOrEmpty(targetStoreLoc)) {
+            return Model.newFail("targetStoreLoc Must not be empty");
+        }
+        StoreLocation targetLocation = StoreLocation.of(targetStoreLoc, "/");
+
+        UnloadTask unloadTask = new UnloadTask(
+            ps.agvId, ps.bizTask.getPriority(),
+            targetLocation
+        );
+
+        ps.runtime.pathPlannerMap.get(ps.agv.getT())
+            .planUnloadTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, unloadTask);
+
+        ps.agv.logicX = ps.fromItem.logicX;
+        ps.agv.logicY = ps.fromItem.logicY;
+        ps.agv.dispatchTask(ps.planSequence);
+
+        return Model.newSuccess(ps.planSequence.toPrettyMap());
+    }
+
+    public static Model<Object> agvLoad(@RequestBody Map<String, Object> params) {
+        Object ret = getCommonParamAndCreateBizTask(params);
+        if (ret instanceof Model) {
+            // 异常
+            return (Model<Object>) ret;
+        }
+        RcsCommonParam ps = (RcsCommonParam) ret;
+
+        String targetStoreLoc = Conv.asString(params.get("targetStoreLoc"));
+        if (Strings.isNullOrEmpty(targetStoreLoc)) {
+            return Model.newFail("targetStoreLoc Must not be empty");
+        }
+        StoreLocation targetLocation = StoreLocation.of(targetStoreLoc, "/");
+
+        LoadTask loadTask = new LoadTask(
+            ps.agvId, ps.bizTask.getPriority(),
+            targetLocation
+        );
+
+        ps.runtime.pathPlannerMap.get(ps.agv.getT())
+            .planLoadTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, loadTask);
+
+        ps.agv.logicX = ps.fromItem.logicX;
+        ps.agv.logicY = ps.fromItem.logicY;
+        ps.agv.dispatchTask(ps.planSequence);
+
+        return Model.newSuccess(ps.planSequence.toPrettyMap());
+    }
+
+    public static Model<Object> cancelTasks(@RequestBody Map<String, Object> params) {
+        Object ret = getCommonParamAndCreateBizTask(params);
+        if (ret instanceof Model) {
+            // 异常
+            return (Model<Object>) ret;
+        }
+        RcsCommonParam ps = (RcsCommonParam) ret;
+
+        ps.agv.cancelTask();
+        return Model.newSuccess("AGV tasks cancelled successfully");
+    }
+
+    public static Model<Object> waitTaskFinish(@RequestBody Map<String, Object> params) {
+        Object ret = getCommonParamAndCreateBizTask(params);
+        if (ret instanceof Model) {
+            // 异常
+            return (Model<Object>) ret;
+        }
+        RcsCommonParam ps = (RcsCommonParam) ret;
+
+        var now = System.currentTimeMillis();
+
+        // 循环等待 ps.agv.planTaskSequence 清空或没值
+        while (ps.agv.planTaskSequence != null && !ps.agv.planTaskSequence.isEmpty()) {
+            try {
+                Thread.sleep(100); // 等待100毫秒
+            } catch (InterruptedException e) {
+                Thread.currentThread().interrupt(); // 恢复中断状态
+                return Model.newFail("Interrupted while waiting for task to finish");
+            }
+        }
+
+        var cost = System.currentTimeMillis() - now;
+        return Model.newSuccess(cost);
+    }
+
     public static Model<Object> agvCarry(@RequestBody Map<String, Object> params) {
-        String projectUUID = Conv.asString(params.get("projectUUID"));
+        Object ret = getCommonParamAndCreateBizTask(params);
-        String catalogCode = Conv.asString(params.get("catalogCode"));
+        if (ret instanceof Model) {
-        Long envId = Conv.asLong(params.get("envId"));
+            // 异常
-        String agvId = Conv.asString(params.get("agvId"));
+            return (Model<Object>) ret;
+        }
+        RcsCommonParam ps = (RcsCommonParam) ret;
+
         String fromStoreLoc = Conv.asString(params.get("fromStoreLoc"));
         String targetStoreLoc = Conv.asString(params.get("targetStoreLoc"));
-        Map<String, Object> option = (Map<String, Object>) params.get("option");
 
-        if (Strings.isNullOrEmpty(agvId)) {
-            return Model.newFail("agvId Must not be empty");
-        }
         if (Strings.isNullOrEmpty(fromStoreLoc)) {
             return Model.newFail("fromStoreLoc Must not be empty");
         }
@@ -171,21 +191,67 @@ public class RcsController {
             return Model.newFail("targetStoreLoc Must not be empty");
         }
 
-        LogisticsRuntime runtime = LogisticsRuntimeService.INSTANCE.getByProjectEnv(projectUUID, envId);
+        StoreLocation sourceLocation = StoreLocation.of(fromStoreLoc, "/");
-        StaticItem sourceItem = runtime.getStaticItemById(fromStoreLoc);
+        StoreLocation targetLocation = StoreLocation.of(targetStoreLoc, "/");
+
+        StaticItem sourceItem = ps.runtime.getStaticItemById(sourceLocation.rackId());
         if (sourceItem == null) {
             return Model.newFail("fromStoreLoc storePoint not found!");
         }
-        StaticItem targetItem = runtime.getStaticItemById(targetStoreLoc);
+        StaticItem targetItem = ps.runtime.getStaticItemById(targetLocation.rackId());
         if (targetItem == null) {
             return Model.newFail("targetStoreLoc storePoint not found!");
         }
 
+        ps.bizTask.setTaskFrom(fromStoreLoc);
+        ps.bizTask.setTaskTo(targetStoreLoc);
+
+        CarryTask carryTask = new CarryTask(
+            ps.agvId, "N/A", ps.bizTask.getPriority(),
+            sourceLocation,
+            targetLocation
+        );
+
+        ps.runtime.pathPlannerMap.get(ps.agv.getT())
+            .planCarryTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, carryTask);
+
+        ps.agv.logicX = ps.fromItem.logicX;
+        ps.agv.logicY = ps.fromItem.logicY;
+        ps.agv.dispatchTask(ps.planSequence);
+
+        return Model.newSuccess(ps.planSequence.toPrettyMap());
+    }
+
+    public static Model<Boolean> agvToCharger(@RequestBody Map<String, Object> params) {
+        throw new NotImplementedException("agvToCharger not implemented yet");
+    }
+
+    public static Object getCommonParamAndCreateBizTask(@RequestBody Map<String, Object> params) {
+        String projectUUID = Conv.asString(params.get("projectUUID"));
+        Long envId = Conv.asLong(params.get("envId"));
+        String agvId = Conv.asString(params.get("agvId"));
+        Map<String, Object> option = (Map<String, Object>) params.get("option");
+
+        if (Strings.isNullOrEmpty(projectUUID)) {
+            return Model.newFail("projectUUID Must not be empty");
+        }
+        if (envId == null || envId < 0) {
+            return Model.newFail("envId Must not be empty");
+        }
+        if (Strings.isNullOrEmpty(agvId)) {
+            return Model.newFail("agvId Must not be empty");
+        }
+        if (option == null) {
+            option = Maps.newHashMap();
+        }
+
+        LogisticsRuntime runtime = LogisticsRuntimeService.INSTANCE.getByProjectEnv(projectUUID, envId);
+
         ExecutorItem executorItem = runtime.executorItemMap.get(agvId);
         if (executorItem == null) {
             return Model.newFail("executor not found: " + agvId);
         }
-        if (!(executorItem instanceof PtrAgvItem)) {
+        if (!(executorItem instanceof PtrAgvItem agv)) {
             return Model.newFail("executor is not a PtrAgvItem id=" + agvId);
         }
 
@@ -196,16 +262,16 @@ public class RcsController {
         if (option.get("forceStartWayPointId") != null) {
             fromItem = runtime.getStaticItemById(Conv.asString(option.get("forceStartWayPointId")));
         } else {
-            fromItem = runtime.getStaticItemByLogicXY(((PtrAgvItem) executorItem).logicX, ((PtrAgvItem) executorItem).logicY);
+            fromItem = runtime.getStaticItemByLogicXY(agv.logicX, agv.logicY);
         }
         if (option.get("forceStartDirection") != null) {
             fromDirection = LCCDirection.fromString(Conv.asString(option.get("forceStartDirection")));
         } else {
-            fromDirection = ((PtrAgvItem) executorItem).getLCCDirection();
+            fromDirection = agv.getLCCDirection();
         }
 
         if (fromItem == null) {
-            return Model.newFail("PtrAgvItem not found at current location: " + ((PtrAgvItem) executorItem).logicX + "_" + ((PtrAgvItem) executorItem).logicY + ", id=" + agvId);
+            return Model.newFail("PtrAgvItem not found at current location: " + agv.logicX + "_" + agv.logicY + ", id=" + agvId);
         }
         if (fromDirection == null) {
             return Model.newFail("PtrAgvItem unkown direction id=" + agvId);
@@ -213,32 +279,31 @@ public class RcsController {
 
         RcsTaskBiz bizTask = new RcsTaskBiz();
         bizTask.setBizTaskId(snowFlake.nextId());
-        bizTask.setEnvId(1L);
+        bizTask.setEnvId(envId);
         bizTask.setBizType(BizTaskType.MOVE.toString());
         bizTask.setLpn("N/A");
         bizTask.setPriority(Conv.asInteger(option.get("priority"), 1));
-        bizTask.setTaskFrom(sourceItem.getId());
+        bizTask.setTaskFrom(fromItem.getId());
-        bizTask.setTaskTo(targetItem.getId());
         bizTask.setAllocatedExecutorId(agvId);
         bizTask.setBizTaskPayload("N/A");
         bizTask.setBizTaskErrorInfo("N/A");
         bizTask.setBizTaskDescription("N/A");
         bizTask.setBizTaskStatus(BizTaskStatus.WAITING_FOR_DISPATCH.toString());
 
-        PlanTaskSequence planSequence = new PlanTaskSequence(agvId, runtime, bizTask, "demo");
+        PlanTaskSequence planSequence = new PlanTaskSequence(agvId, runtime, bizTask, "RcsScript");
-
+        return new RcsCommonParam(projectUUID, envId, agvId, option, bizTask, runtime, agv, planSequence, fromItem, fromDirection);
-        CarryTask carryTask = new CarryTask(
-            agvId, "dummy", 1,
-            new StoreLocation("rack1", 0, 1, 0),
-            new StoreLocation("54", 0, 0, 0)
-        );
-
-        runtime.pathPlannerMap.get(executorItem.getT())
-            .planCarryTask(planSequence, fromItem.getId(), fromDirection, carryTask);
-        return Model.newSuccess(planSequence.toPrettyMap());
     }
 
-    public static Model<Boolean> agvToCharger(@RequestBody Map<String, Object> params) {
+    public record RcsCommonParam(
-        throw new NotImplementedException("agvToCharger not implemented yet");
+        String projectUUID,
+        Long envId,
+        String agvId,
+        Map<String, Object> option,
+        RcsTaskBiz bizTask,
+        LogisticsRuntime runtime,
+        PtrAgvItem agv,
+        PlanTaskSequence planSequence,
+        StaticItem fromItem,
+        LCCDirection fromDirection) {
     }
 }

servo/src/main/java/com/yvan/workbench/model/entity/Model.java

@@ -1,5 +1,7 @@
 package com.yvan.workbench.model.entity;
 
+import com.fasterxml.jackson.annotation.JsonInclude;
+
 public class Model<T> implements java.io.Serializable {
     private boolean success = false;
     private T data;
@@ -43,4 +45,15 @@ public class Model<T> implements java.io.Serializable {
         this.success = success;
         return this;
     }
+
+    @JsonInclude(JsonInclude.Include.NON_NULL)
+    private Throwable exception;
+
+    public Throwable getException() {
+        return exception;
+    }
+
+    public void setException(Throwable exception) {
+        this.exception = exception;
+    }
 }