From 7aef1e7d84b6010cda397aadacaa0f23e2e44e8b Mon Sep 17 00:00:00 2001 From: luoyifan Date: Mon, 23 Jun 2025 22:56:17 +0800 Subject: [PATCH] =?UTF-8?q?PtrPathPlanner=20=E7=AE=97=E6=B3=95=E6=B5=8B?= =?UTF-8?q?=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 1 + examples/example1.json | 1145 ++++++++++++++++++++ servo/src/main/java/com/galaxis/rcs/RCS.java | 22 +- .../com/galaxis/rcs/plan/PlanTaskSequence.java | 41 + .../galaxis/rcs/plan/path2/AStarPathPlanner.java | 43 +- .../galaxis/rcs/plan/path2/NavigationGraph.java | 14 +- .../com/galaxis/rcs/plan/path2/PtrPathPlanner.java | 51 +- .../java/com/galaxis/rcs/plan/path2/State.java | 5 +- .../java/com/yvan/logisticsEnv/LogisticsEnv.java | 3 +- .../com/yvan/logisticsModel/LogisticsRuntime.java | 5 +- .../yvan/workbench/controller/EnvController.java | 4 + 11 files changed, 1282 insertions(+), 52 deletions(-) create mode 100644 examples/example1.json diff --git a/.gitignore b/.gitignore index f25b4e8..74eaa06 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ # Compiled class file *.class +.lck .gradle .idea diff --git a/examples/example1.json b/examples/example1.json new file mode 100644 index 0000000..ef1be85 --- /dev/null +++ b/examples/example1.json @@ -0,0 +1,1145 @@ +{ + "catalogCode": "f2", + "t": "floor", + "items": [ + { + "id": "rack1", + "t": "rack", + "v": true, + "tf": [ + [ + 4.196, + 0, + 5.882 + ], + [ + 0, + 90, + 0 + ], + [ + 0.1, + 0.1, + 1 + ] + ], + "dt": { + "rackDepth": 1, + "bottomBarHeight": 0.2, + "bottomLinkHeight": 0.2, + "topLinkDistance": 0.2, + "levelCount": 3, + "bayCount": 4, + "hideFloor": false, + "extendColumns": true, + "columnSpacing": 1, + "bays": [ + { + "bayWidth": 1.275, + "levelHeight": [ + 1.4, + 1.4, + 1.4 + ] + }, + { + "bayWidth": 1.275, + "levelHeight": [ + 1.4, + 1.4, + 1.4 + ] + }, + { + "bayWidth": 1.275, + "levelHeight": [ + 1.4, + 1.4, + 1.4 + ] + }, + { + "bayWidth": 1.275, + "levelHeight": [ + 1.4, + 1.4, + 1.4 + ] + } + ], + "center": [], + "in": [], + "out": [] + } + }, + { + "id": "1", + "t": "way", + "v": true, + "logicX": 11, + "logicY": 10, + "logicZ": 1, + "tf": [ + [ + 2.7, + 0, + 2.13 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "2", + "38", + "36" + ], + "out": [ + "2", + "38", + "36" + ], + "center": [], + "agvRotation": [ + "cl2", + "clx" + ] + } + }, + { + "id": "2", + "t": "way", + "v": true, + "logicX": 11, + "logicY": 11, + "logicZ": 1, + "tf": [ + [ + 2.7, + 0, + 2.832 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "1", + "3" + ], + "out": [ + "1", + "3" + ], + "center": [] + } + }, + { + "id": "3", + "t": "way", + "v": true, + "logicX": 11, + "logicY": 13, + "logicZ": 1, + "tf": [ + [ + 2.7, + 0, + 3.932 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "2", + "4" + ], + "out": [ + "2", + "4" + ], + "center": [], + "linkStore": [ + { + "item": "rack1", + "bay": 3, + "level": 2, + "cell": 0, + "direction": "right" + } + ] + } + }, + { + "id": "4", + "t": "way", + "v": true, + "logicX": 11, + "logicY": 15, + "logicZ": 1, + "tf": [ + [ + 2.7, + 0, + 4.582 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "3", + "5" + ], + "out": [ + "3", + "5" + ], + "center": [] + } + }, + { + "id": "5", + "t": "way", + "v": true, + "logicX": 11, + "logicY": 16, + "logicZ": 1, + "tf": [ + [ + 2.7, + 0, + 5.232 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "4", + "6" + ], + "out": [ + "4", + "6" + ], + "center": [], + "linkStore": [ + { + "item": "rack1", + "bay": 2, + "level": 2, + "cell": 0, + "direction": "right" + } + ] + } + }, + { + "id": "6", + "t": "way", + "v": true, + "logicX": 11, + "logicY": 17, + "logicZ": 1, + "tf": [ + [ + 2.7, + 0, + 5.882 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "5", + "7" + ], + "out": [ + "5", + "7" + ], + "center": [] + } + }, + { + "id": "7", + "t": "way", + "v": true, + "logicX": 11, + "logicY": 18, + "logicZ": 1, + "tf": [ + [ + 2.7, + 0, + 6.532 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "6", + "8" + ], + "out": [ + "6", + "8" + ], + "center": [], + "linkStore": [ + { + "item": "rack1", + "bay": 1, + "level": 1, + "cell": 0, + "direction": "right" + } + ] + } + }, + { + "id": "8", + "t": "way", + "v": true, + "logicX": 11, + "logicY": 20, + "logicZ": 1, + "tf": [ + [ + 2.7, + 0, + 7.75 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "7", + "charger1" + ], + "out": [ + "7", + "charger1" + ], + "center": [], + "linkStore": [ + { + "item": "rack1", + "bay": 0, + "level": 2, + "cell": 0, + "direction": "right" + } + ] + } + }, + { + "id": "17", + "t": "way", + "v": true, + "logicX": 13, + "logicY": 10, + "logicZ": 1, + "tf": [ + [ + 5.65, + 0, + 2.13 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "20" + ], + "out": [ + "20" + ], + "center": [] + } + }, + { + "id": "20", + "t": "way", + "v": true, + "logicX": 13, + "logicY": 12, + "logicZ": 1, + "tf": [ + [ + 5.65, + 0, + 2.865 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "17", + "21" + ], + "out": [ + "17", + "21" + ], + "center": [], + "linkStore": [ + { + "item": "54", + "bay": 0, + "level": 0, + "cell": 0, + "direction": "right" + } + ] + } + }, + { + "id": "21", + "t": "way", + "v": true, + "logicX": 13, + "logicY": 13, + "logicZ": 1, + "tf": [ + [ + 5.65, + 0, + 3.932 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "20", + "22" + ], + "out": [ + "20", + "22" + ], + "center": [], + "linkStore": [ + { + "item": "rack1", + "bay": 3, + "level": 1, + "cell": 0, + "direction": "left" + } + ] + } + }, + { + "id": "22", + "t": "way", + "v": true, + "logicX": 13, + "logicY": 14, + "logicZ": 1, + "tf": [ + [ + 5.65, + 0, + 4.348 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "21", + "23" + ], + "out": [ + "21", + "23" + ], + "center": [], + "linkStore": [ + { + "item": "56", + "bay": 0, + "level": 0, + "cell": 0, + "direction": "right" + } + ] + } + }, + { + "id": "23", + "t": "way", + "v": true, + "logicX": 13, + "logicY": 16, + "logicZ": 1, + "tf": [ + [ + 5.65, + 0, + 5.232 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "22", + "24" + ], + "out": [ + "22", + "24" + ], + "center": [], + "agvRotation": [ + "cl2", + "clx" + ], + "linkStore": [ + { + "item": "rack1", + "bay": 2, + "level": 1, + "cell": 0, + "direction": "left" + } + ] + } + }, + { + "id": "24", + "t": "way", + "v": true, + "logicX": 13, + "logicY": 17, + "logicZ": 1, + "tf": [ + [ + 5.65, + 0, + 5.882 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "23", + "25" + ], + "out": [ + "23", + "25" + ], + "center": [] + } + }, + { + "id": "25", + "t": "way", + "v": true, + "logicX": 13, + "logicY": 18, + "logicZ": 1, + "tf": [ + [ + 5.65, + 0, + 6.532 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "24", + "26" + ], + "out": [ + "24", + "26" + ], + "center": [], + "linkStore": [ + { + "item": "rack1", + "bay": 1, + "level": 1, + "cell": 0, + "direction": "left" + } + ] + } + }, + { + "id": "26", + "t": "way", + "v": true, + "logicX": 13, + "logicY": 19, + "logicZ": 1, + "tf": [ + [ + 5.65, + 0, + 6.744 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "25", + "27" + ], + "out": [ + "25", + "27" + ], + "center": [], + "linkStore": [ + { + "item": "58", + "bay": 0, + "level": 0, + "cell": 0, + "direction": "right" + } + ] + } + }, + { + "id": "27", + "t": "way", + "v": true, + "logicX": 13, + "logicY": 20, + "logicZ": 1, + "tf": [ + [ + 5.65, + 0, + 7.75 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "26", + "charger2" + ], + "out": [ + "26", + "charger2" + ], + "center": [], + "linkStore": [ + { + "item": "rack1", + "bay": 0, + "level": 1, + "cell": 0, + "direction": "left" + } + ] + } + }, + { + "id": "36", + "t": "way", + "v": true, + "logicX": 12, + "logicY": 10, + "logicZ": 1, + "tf": [ + [ + 3.9, + 0, + 2.13 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "1" + ], + "out": [ + "1" + ], + "center": [], + "linkStore": [ + { + "item": "47", + "bay": 0, + "level": 0, + "cell": 0, + "direction": "up" + } + ] + } + }, + { + "id": "38", + "t": "way", + "v": true, + "logicX": 10, + "logicY": 10, + "logicZ": 1, + "tf": [ + [ + 1.5, + 0, + 2.13 + ], + [ + 90, + 0, + 0 + ], + [ + 0.25, + 0.25, + 0.1 + ] + ], + "dt": { + "in": [ + "1" + ], + "out": [ + "1" + ], + "center": [], + "linkStore": [ + { + "item": "49", + "bay": 0, + "level": 0, + "cell": 0, + "direction": "up" + } + ] + } + }, + { + "id": "47", + "t": "gstore", + "v": true, + "tf": [ + [ + 3.9, + 0, + 0.63 + ], + [ + 0, + 90, + 0 + ], + [ + 1, + 0.01, + 1 + ] + ], + "dt": { + "in": [], + "out": [], + "center": [], + "strokeWidth": 0.1 + } + }, + { + "id": "49", + "t": "gstore", + "v": true, + "tf": [ + [ + 1.5, + 0, + 0.63 + ], + [ + 0, + 90, + 0 + ], + [ + 1, + 0.01, + 1 + ] + ], + "dt": { + "in": [], + "out": [], + "center": [], + "strokeWidth": 0.1 + } + }, + { + "id": "54", + "t": "gstore", + "v": true, + "tf": [ + [ + 7.1, + 0, + 2.865 + ], + [ + 0, + 0, + 0 + ], + [ + 1, + 0.01, + 1 + ] + ], + "dt": { + "in": [], + "out": [], + "center": [], + "strokeWidth": 0.1 + } + }, + { + "id": "56", + "t": "gstore", + "v": true, + "tf": [ + [ + 7.1, + 0, + 4.35 + ], + [ + 0, + 0, + 0 + ], + [ + 1, + 0.01, + 1 + ] + ], + "dt": { + "in": [], + "out": [], + "center": [], + "strokeWidth": 0.1 + } + }, + { + "id": "58", + "t": "gstore", + "v": true, + "tf": [ + [ + 7.1, + 0, + 6.75 + ], + [ + 0, + 90, + 0 + ], + [ + 1, + 0.01, + 1 + ] + ], + "dt": { + "in": [], + "out": [], + "center": [], + "strokeWidth": 0.1 + } + }, + { + "id": "10", + "t": "cl2", + "v": true, + "tf": [ + [ + 5.65, + 0, + 2.13 + ], + [ + 0, + -90, + 0 + ], + [ + 1, + 1, + 1 + ] + ], + "dt": { + "in": [], + "out": [], + "center": [], + "ptrWidth": 1.5, + "ptrDepth": 1.5, + "ptrHeight": 1.98 + } + }, + { + "id": "199", + "t": "clx", + "v": true, + "tf": [ + [ + 1.5, + 0, + 2.13 + ], + [ + 0, + 0, + 0 + ], + [ + 1, + 1, + 1 + ] + ], + "dt": { + "in": [], + "out": [], + "center": [], + "clxWidth": 1.65, + "clxDepth": 1.65, + "clxHeight": 3.393 + } + }, + { + "id": "charger1", + "t": "way", + "v": true, + "tf": [ + [ + 2.696, + 0, + 8.75 + ], + [ + 0, + 0, + 0 + ], + [ + 1, + 1, + 1 + ] + ], + "dt": { + "in": [ + "8" + ], + "out": [ + "8" + ], + "center": [], + "isCharger": true + } + }, + { + "id": "charger2", + "t": "way", + "v": true, + "tf": [ + [ + 5.655, + 0, + 8.75 + ], + [ + 0, + 0, + 0 + ], + [ + 1, + 1, + 1 + ] + ], + "dt": { + "in": [ + "27" + ], + "out": [ + "27" + ], + "center": [], + "isCharger": true + } + } + ] +} diff --git a/servo/src/main/java/com/galaxis/rcs/RCS.java b/servo/src/main/java/com/galaxis/rcs/RCS.java index a2c9b75..9897fcb 100644 --- a/servo/src/main/java/com/galaxis/rcs/RCS.java +++ b/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 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); - return Model.newSuccess(planSequence); + .planCarryTask(planSequence, "17", LCCDirection.DOWN, carryTask); + return planSequence.toPrettyMap(); } public static void runDemo() { diff --git a/servo/src/main/java/com/galaxis/rcs/plan/PlanTaskSequence.java b/servo/src/main/java/com/galaxis/rcs/plan/PlanTaskSequence.java index f7c2886..7dff083 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/PlanTaskSequence.java +++ b/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 taskList = Lists.newArrayList(); public final RcsTaskBiz bizTask; @@ -94,4 +99,40 @@ public class PlanTaskSequence { this.isFinished = true; return task; } + + /** + * 输出方便阅读的 Json 格式 + * + * @return + */ + public Map toPrettyMap() { + JsonWrapper jw = new JsonWrapper(); + jw.set("executorId", executorId); + jw.set("bizTask", bizTask); + List 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(); + } } diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java index 47882cf..94ce6d0 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java @@ -13,7 +13,7 @@ public class AStarPathPlanner { } // 路径规划状态 - public List findPath(String startId, float startDirectionAngle, String endId, float endDirectionAngle) { + public List 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 = { - current.directionAngle(), // 前进方向不变 - (current.directionAngle() + 180) % 360 // 后退方向反转 - }; + LCCDirection[] possibleHeadings; + if (current.directionAngle() == LCCDirection.UP || current.directionAngle() == LCCDirection.DOWN) { + possibleHeadings = new LCCDirection[]{LCCDirection.UP, LCCDirection.DOWN}; + } else { + possibleHeadings = new LCCDirection[]{LCCDirection.LEFT, LCCDirection.RIGHT}; + } - for (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()), - 360 - Math.abs(rotation - current.directionAngle()) - ); + // 计算旋转代价 + int angleDiff = 0; + if (current.directionAngle() == LCCDirection.UP && rotation == LCCDirection.RIGHT) { + angleDiff = 90; + } else if (current.directionAngle() == LCCDirection.RIGHT && rotation == LCCDirection.DOWN) { + angleDiff = 90; + } else if (current.directionAngle() == LCCDirection.DOWN && rotation == LCCDirection.LEFT) { + angleDiff = 90; + } else if (current.directionAngle() == LCCDirection.LEFT && rotation == LCCDirection.UP) { + angleDiff = 90; + } else if (current.directionAngle() == rotation) { + // 无需旋转 + continue; + } else { + angleDiff = 180; // 反向旋转 + } float rotationCost = angleDiff * ROTATION_COST_PER_DEGREE; - considerState(current, current.node(), rotation, - rotationCost, open, visited, goal); + considerState(current, current.node(), rotation, 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 open, Map 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; } diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java index 61b36be..11e58af 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java @@ -78,8 +78,18 @@ public class NavigationGraph { return nodeMap.get(id); } - public List getNodesForStore(String storeId) { - return storeToNodes.getOrDefault(storeId, Collections.emptyList()); + public List getNodesForStore(String storeId) { + List nodes = new ArrayList<>(); + List 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 getNeighbors(Node node) { diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java index 7fd6c29..5deb84e 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java +++ b/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 toPickupPath = astar.findPath(startId, initDirectionAngle, pickupNode.id(), pickupRotationAngle); + List toPickupPath = astar.findPath(startId, initDirectionAngle, pickupNode.id(), pickupDirectionAngle); // 规划到放货点路径 - List toDeliverPath = astar.findPath(pickupNode.id(), pickupRotationAngle, dropNode.id(), dropRotationAngle); + List toDeliverPath = astar.findPath(pickupNode.id(), pickupDirectionAngle, dropNode.id(), dropDirectionAngle); // 生成指令序列 - generateMoves(seq, toPickupPath); - seq.addLoad(task.lpn(), pickupRackId, pickupBay, task.from().level(), task.from().cell()); - generateMoves(seq, toDeliverPath); - seq.addUnload(dropRackId, task.to().level(), task.to().bay(), task.to().cell()); - seq.addFinish(); + generateMoves(plan, toPickupPath); + plan.addLoad(task.lpn(), pickupRackId, pickupBay, task.from().level(), task.from().cell()); + generateMoves(plan, toDeliverPath); + plan.addUnload(dropRackId, task.to().level(), task.to().bay(), task.to().cell()); + plan.addFinish(); } private Node findStoreNode(String storeId, int bay) { - return graph.getNodesForStore(storeId).stream() - .map(graph::getNode) - .filter(node -> node.storeLinks().stream() - .anyMatch(link -> link.storeId().equals(storeId) && link.bay() == bay)) - .findFirst() - .orElseThrow(); + List nodes = this.graph.getNodesForStore(storeId); + for (Node node : nodes) { + for (StoreLink link : node.storeLinks()) { + if (link.storeId().equals(storeId) && link.bay() == bay) { + return node; + } + } + } + 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())) - .orElse(0f); + .map(link -> link.direction()) + .orElseThrow(() -> new RuntimeException("Not found storeLink in id=" + node.id() + ", bay=" + bay)); } - private void generateMoves(PlanTaskSequence seq, List path) { + private void generateMoves(PlanTaskSequence plan, List path) { // 简化的指令生成（实际需处理方向变化） for (int i = 1; i < path.size(); i++) { Node node = path.get(i); - seq.addMoveTo(node.id()); + plan.addMoveTo(node.id()); } } + + } diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/State.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/State.java index 5848382..339ea1a 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/State.java +++ b/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 { + @Override public int compareTo(State other) { return Float.compare(g + h, other.g + other.h); diff --git a/servo/src/main/java/com/yvan/logisticsEnv/LogisticsEnv.java b/servo/src/main/java/com/yvan/logisticsEnv/LogisticsEnv.java index 4e40165..b8e8ec5 100644 --- a/servo/src/main/java/com/yvan/logisticsEnv/LogisticsEnv.java +++ b/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; diff --git a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java index c137a0c..4c97685 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java +++ b/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 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(); } diff --git a/servo/src/main/java/com/yvan/workbench/controller/EnvController.java b/servo/src/main/java/com/yvan/workbench/controller/EnvController.java index c7f09df..3f80f4b 100644 --- a/servo/src/main/java/com/yvan/workbench/controller/EnvController.java +++ b/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()); }