PtrPathPlanner 算法测试

.gitignore

@@ -1,5 +1,6 @@
 # Compiled class file
 *.class
+.lck
 .gradle
 .idea
 

servo/src/main/java/com/galaxis/rcs/RCS.java

@@ -6,16 +6,15 @@ import com.galaxis.rcs.common.entity.RcsTaskBiz;
 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.planner.Planner;
 import com.galaxis.rcs.plan.TaskPlannerFactory;
 import com.galaxis.rcs.plan.PlanTaskSequence;
 import com.galaxis.rcs.plan.task.CarryTask;
 import com.google.common.base.Joiner;
-import com.yvan.logisticsEnv.LogisticsEnv;
+import com.yvan.logisticsEnv.EnvStartParam;
 import com.yvan.logisticsModel.LogisticsRuntime;
 import com.yvan.logisticsModel.LogisticsRuntimeService;
-import com.yvan.logisticsMonitor.task.BizTask;
-import com.yvan.workbench.model.entity.Model;
 import lombok.SneakyThrows;
 import org.apache.commons.io.FileUtils;
 import org.clever.core.json.JsonWrapper;
@@ -34,14 +33,17 @@ public class RCS {
     @SneakyThrows
     public static void init() {
         if (LogisticsRuntimeService.INSTANCE.findByEnvCode(1L) == null) {
-            String fs = Joiner.on("\n").join(FileUtils.readLines(new File("./yvan-rcs-web/src/example/example1.json"), StandardCharsets.UTF_8));
+            String fs = Joiner.on("\n").join(FileUtils.readLines(new File("./examples/example1.json"), StandardCharsets.UTF_8));
             JsonWrapper jw = new JsonWrapper(fs);
 
             LogisticsRuntimeService.INSTANCE.createEnv(1L);
             LogisticsRuntime runtime = LogisticsRuntimeService.INSTANCE.findByEnvCode(1L);
             runtime.loadMap(jw);
 
-            runtime.start();
+            EnvStartParam param = new EnvStartParam();
+            param.setTimeRate(1);
+            param.setVirtual(false);
+            runtime.start(param);
         }
 
     }
@@ -77,7 +79,7 @@ public class RCS {
         return result;
     }
 
-    public static Model<PlanTaskSequence> runPath() {
+    public static Object runPath() {
         String executorId = "10"; // 执行器ID
         String lpn = "pallet1124";
         long envId = 1;
@@ -90,7 +92,7 @@ public class RCS {
         bizTask.setLpn(lpn);
         bizTask.setPriority(1);
         bizTask.setTaskFrom("rack1_0_1_0");
-        bizTask.setTaskTo("20_0_0_0");
+        bizTask.setTaskTo("54_0_0_0");
         bizTask.setAllocatedExecutorId(executorId);
         bizTask.setBizTaskPayload("N/A");
         bizTask.setBizTaskErrorInfo("N/A");
@@ -102,12 +104,12 @@ public class RCS {
         CarryTask carryTask = new CarryTask(
             executorId, lpn, 1,
             new StoreLocation("rack1", 0, 1, 0),
-            new StoreLocation("20", 0, 0, 0)
+            new StoreLocation("54", 0, 0, 0)
         );
 
         logisticsRuntime.pathPlannerMap.get("cl2")
-            .planCarryTask(planSequence, "17", 270f, carryTask);
+            .planCarryTask(planSequence, "17", LCCDirection.DOWN, carryTask);
-        return Model.newSuccess(planSequence);
+        return planSequence.toPrettyMap();
     }
 
     public static void runDemo() {

servo/src/main/java/com/galaxis/rcs/plan/PlanTaskSequence.java

@@ -1,5 +1,6 @@
 package com.galaxis.rcs.plan;
 
+import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.galaxis.rcs.common.entity.RcsTaskBiz;
 import com.galaxis.rcs.common.entity.RcsTaskPlan;
 import com.galaxis.rcs.common.enums.PlanTaskStatus;
@@ -7,14 +8,18 @@ import com.galaxis.rcs.common.enums.PlanTaskType;
 import com.google.common.collect.Lists;
 import com.yvan.logisticsModel.LogisticsRuntime;
 import org.clever.core.id.SnowFlake;
+import org.clever.core.json.JsonWrapper;
 
 import java.math.BigDecimal;
 import java.util.Date;
 import java.util.List;
+import java.util.Map;
 
 public class PlanTaskSequence {
+    @JsonIgnore
     public static final SnowFlake snowFlake = new SnowFlake();
     public final String executorId;
+    @JsonIgnore
     public final LogisticsRuntime logisticsRuntime;
     public final List<RcsTaskPlan> taskList = Lists.newArrayList();
     public final RcsTaskBiz bizTask;
@@ -94,4 +99,40 @@ public class PlanTaskSequence {
         this.isFinished = true;
         return task;
     }
+
+    /**
+     * 输出方便阅读的 Json 格式
+     *
+     * @return
+     */
+    public Map<String, ?> toPrettyMap() {
+        JsonWrapper jw = new JsonWrapper();
+        jw.set("executorId", executorId);
+        jw.set("bizTask", bizTask);
+        List<String> list = Lists.newArrayList();
+        for (RcsTaskPlan task : taskList) {
+            String taskStr = "UNKNOWN:" + task.getPlanType();
+            switch (PlanTaskType.valueOf(task.getPlanType())) {
+                case MOVE:
+                    taskStr = "MOVE " + task.getTargetId();
+                    break;
+                case LOAD:
+                    taskStr = "LOAD " + task.getTargetId() + "_" + task.getTargetBay() + "_" + task.getTargetLevel() + "_" + task.getTargetCell();
+                    break;
+                case UNLOAD:
+                    taskStr = "UNLOAD " + task.getTargetId() + "_" + task.getTargetBay() + "_" + task.getTargetLevel() + "_" + task.getTargetCell();
+                    break;
+                case ROTATION:
+                    taskStr = "Rotation " + task.getTargetRotation();
+                    break;
+                case FINISH:
+                    taskStr = "FINISH";
+                    break;
+            }
+
+            list.add(taskStr);
+        }
+        jw.set("items", list);
+        return jw.getInnerMap();
+    }
 }

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

@@ -13,7 +13,7 @@ public class AStarPathPlanner {
     }
 
     // 路径规划状态
-    public List<Node> findPath(String startId, float startDirectionAngle, String endId, float endDirectionAngle) {
+    public List<Node> findPath(String startId, LCCDirection startDirectionAngle, String endId, LCCDirection endDirectionAngle) {
         Node start = graph.getNode(startId);
         Node goal = graph.getNode(endId);
         if (start == null || goal == null) return Collections.emptyList();
@@ -39,12 +39,14 @@ public class AStarPathPlanner {
             // 处理邻居移动
             for (Node neighbor : graph.getNeighbors(current.node())) {
                 // 计算可能的两种方向（前进/后退）
-                float[] possibleHeadings = {
+                LCCDirection[] possibleHeadings;
-                    current.directionAngle(), // 前进方向不变
+                if (current.directionAngle() == LCCDirection.UP || current.directionAngle() == LCCDirection.DOWN) {
-                    (current.directionAngle() + 180) % 360 // 后退方向反转
+                    possibleHeadings = new LCCDirection[]{LCCDirection.UP, LCCDirection.DOWN};
-                };
+                } else {
+                    possibleHeadings = new LCCDirection[]{LCCDirection.LEFT, LCCDirection.RIGHT};
+                }
 
-                for (float nextHeading : possibleHeadings) {
+                for (LCCDirection nextHeading : possibleHeadings) {
                     float moveCost = graph.distance(current.node(), neighbor);
                     considerState(current, neighbor, nextHeading, moveCost, open, visited, goal);
                 }
@@ -52,24 +54,35 @@ public class AStarPathPlanner {
 
             // 处理旋转（仅在可旋转节点）
             if (current.node().rotatable()) {
-                for (float rotation : new float[]{0, 90, 180, 270}) {
+                for (LCCDirection rotation : new LCCDirection[]{LCCDirection.UP, LCCDirection.DOWN, LCCDirection.LEFT, LCCDirection.RIGHT}) {
                     if (rotation == current.directionAngle()) continue;
 
-                    float angleDiff = Math.min(
+                    // 计算旋转代价
-                        Math.abs(rotation - current.directionAngle()),
+                    int angleDiff = 0;
-                        360 - Math.abs(rotation - current.directionAngle())
+                    if (current.directionAngle() == LCCDirection.UP && rotation == LCCDirection.RIGHT) {
-                    );
+                        angleDiff = 90;
+                    } else if (current.directionAngle() == LCCDirection.RIGHT && rotation == LCCDirection.DOWN) {
+                        angleDiff = 90;
+                    } else if (current.directionAngle() == LCCDirection.DOWN && rotation == LCCDirection.LEFT) {
+                        angleDiff = 90;
+                    } else if (current.directionAngle() == LCCDirection.LEFT && rotation == LCCDirection.UP) {
+                        angleDiff = 90;
+                    } else if (current.directionAngle() == rotation) {
+                        // 无需旋转
+                        continue;
+                    } else {
+                        angleDiff = 180; // 反向旋转
+                    }
                     float rotationCost = angleDiff * ROTATION_COST_PER_DEGREE;
 
-                    considerState(current, current.node(), rotation,
+                    considerState(current, current.node(), rotation, rotationCost, open, visited, goal);
-                        rotationCost, open, visited, goal);
                 }
             }
         }
         return Collections.emptyList();
     }
 
-    private void considerState(State current, Node nextNode, float nextHeading,
+    private void considerState(State current, Node nextNode, LCCDirection nextHeading,
                                float cost, PriorityQueue<State> open,
                                Map<String, State> visited, Node goal) {
         String key = stateKey(nextNode.id(), nextHeading);
@@ -96,7 +109,7 @@ public class AStarPathPlanner {
         return graph.distance(a, b);
     }
 
-    private String stateKey(String nodeId, float directionAngle) {
+    private String stateKey(String nodeId, LCCDirection directionAngle) {
         return nodeId + "|" + directionAngle;
     }
 

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

@@ -78,8 +78,18 @@ public class NavigationGraph {
         return nodeMap.get(id);
     }
 
-    public List<String> getNodesForStore(String storeId) {
+    public List<Node> getNodesForStore(String storeId) {
-        return storeToNodes.getOrDefault(storeId, Collections.emptyList());
+        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) {

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

@@ -1,5 +1,6 @@
 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;
 
@@ -14,55 +15,63 @@ public class PtrPathPlanner {
         this.astar = new AStarPathPlanner(graph);
     }
 
-    public void planCarryTask(PlanTaskSequence seq, String startId, float initDirectionAngle, CarryTask task) {
+    public void planCarryTask(PlanTaskSequence plan, String startId, LCCDirection initDirectionAngle, CarryTask task) {
         // 取货点
         String pickupRackId = task.from().rackId();
         int pickupBay = task.from().bay();
         Node pickupNode = findStoreNode(pickupRackId, pickupBay);
-        float pickupRotationAngle = getRequiredHeading(pickupNode, pickupBay);
+        LCCDirection pickupDirectionAngle = getRequiredHeading(pickupNode, pickupBay);
 
         // 放货点
         String dropRackId = task.to().rackId();
         int dropBay = task.to().bay();
         Node dropNode = findStoreNode(dropRackId, dropBay);
-        float dropRotationAngle = getRequiredHeading(dropNode, 0);
+        LCCDirection dropDirectionAngle = getRequiredHeading(dropNode, 0);
 
         // 规划到取货点路径
-        List<Node> toPickupPath = astar.findPath(startId, initDirectionAngle, pickupNode.id(), pickupRotationAngle);
+        List<Node> toPickupPath = astar.findPath(startId, initDirectionAngle, pickupNode.id(), pickupDirectionAngle);
 
         // 规划到放货点路径
-        List<Node> toDeliverPath = astar.findPath(pickupNode.id(), pickupRotationAngle, dropNode.id(), dropRotationAngle);
+        List<Node> toDeliverPath = astar.findPath(pickupNode.id(), pickupDirectionAngle, dropNode.id(), dropDirectionAngle);
 
         // 生成指令序列
-        generateMoves(seq, toPickupPath);
+        generateMoves(plan, toPickupPath);
-        seq.addLoad(task.lpn(), pickupRackId, pickupBay, task.from().level(), task.from().cell());
+        plan.addLoad(task.lpn(), pickupRackId, pickupBay, task.from().level(), task.from().cell());
-        generateMoves(seq, toDeliverPath);
+        generateMoves(plan, toDeliverPath);
-        seq.addUnload(dropRackId, task.to().level(), task.to().bay(), task.to().cell());
+        plan.addUnload(dropRackId, task.to().level(), task.to().bay(), task.to().cell());
-        seq.addFinish();
+        plan.addFinish();
     }
 
     private Node findStoreNode(String storeId, int bay) {
-        return graph.getNodesForStore(storeId).stream()
+        List<Node> nodes = this.graph.getNodesForStore(storeId);
-            .map(graph::getNode)
+        for (Node node : nodes) {
-            .filter(node -> node.storeLinks().stream()
+            for (StoreLink link : node.storeLinks()) {
-                .anyMatch(link -> link.storeId().equals(storeId) && link.bay() == bay))
+                if (link.storeId().equals(storeId) && link.bay() == bay) {
-            .findFirst()
+                    return node;
-            .orElseThrow();
+                }
+            }
+        }
+        throw new RuntimeException("Not found WayPoint link Store, rackId=" + storeId + ", bay=" + bay);
     }
 
-    private float getRequiredHeading(Node node, int bay) {
+    /**
+     * 获取指定节点和货架的所需朝向
+     */
+    private LCCDirection getRequiredHeading(Node node, int bay) {
         return node.storeLinks().stream()
             .filter(link -> link.bay() == bay)
             .findFirst()
-            .map(link -> AStarPathPlanner.getRequiredDirection(link.direction()))
+            .map(link -> link.direction())
-            .orElse(0f);
+            .orElseThrow(() -> new RuntimeException("Not found storeLink in id=" + node.id() + ", bay=" + bay));
     }
 
-    private void generateMoves(PlanTaskSequence seq, List<Node> path) {
+    private void generateMoves(PlanTaskSequence plan, List<Node> path) {
         // 简化的指令生成（实际需处理方向变化）
         for (int i = 1; i < path.size(); i++) {
             Node node = path.get(i);
-            seq.addMoveTo(node.id());
+            plan.addMoveTo(node.id());
         }
     }
+
+
 }

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

@@ -1,11 +1,14 @@
 package com.galaxis.rcs.plan.path2;
 
+import com.galaxis.rcs.common.enums.LCCDirection;
+
 public record State(Node node,
-                    float directionAngle,
+                    LCCDirection directionAngle,
                     float g,
                     float h,
                     State parent)
     implements Comparable<State> {
+
     @Override
     public int compareTo(State other) {
         return Float.compare(g + h, other.g + other.h);

servo/src/main/java/com/yvan/logisticsEnv/LogisticsEnv.java

@@ -48,10 +48,11 @@ public class LogisticsEnv {
      */
     private int timeRate;
 
-    public void start() {
+    public void start(EnvStartParam param) {
         if (this.isRunning) {
             throw new IllegalStateException("环境已经在运行中");
         }
+        this.startParam = param;
         this.isRunning = true;
         this.startTime = System.currentTimeMillis();
         this.stopTime = 0L;

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

@@ -8,6 +8,7 @@ import com.galaxis.rcs.task.TaskService;
 import com.google.common.collect.Lists;
 import com.google.common.collect.Maps;
 import com.google.common.collect.Sets;
+import com.yvan.logisticsEnv.EnvStartParam;
 import com.yvan.logisticsEnv.LogisticsEnv;
 import lombok.extern.slf4j.Slf4j;
 import org.clever.core.Conv;
@@ -156,7 +157,7 @@ public class LogisticsRuntime {
         }
     }
 
-    public void start() {
+    public void start(EnvStartParam param) {
         // 开启所有机器人的任务调度
         Set<String> executorTypes = Sets.newHashSet();
         for (ExecutorItem executorItem : executorItemMap.values()) {
@@ -172,7 +173,7 @@ public class LogisticsRuntime {
             this.pathPlannerMap.put(type, new PtrPathPlanner(graph));
         }
 
-        this.logisticsEnv.start();
+        this.logisticsEnv.start(param);
         this.taskDispatchFactory.startPolling();
     }
 

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

@@ -32,6 +32,10 @@ public class EnvController {
         return Model.newSuccess(true);
     }
 
+    public static Model<?> runPath() {
+        return Model.newSuccess(RCS.runPath());
+    }
+
     public static Model<?> getDemo() {
         return Model.newSuccess(RCS.getDemo());
     }