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 9e67211..26a2d73 100644 --- a/servo/src/main/java/com/galaxis/rcs/RCS.java +++ b/servo/src/main/java/com/galaxis/rcs/RCS.java @@ -4,16 +4,17 @@ import com.galaxis.rcs.common.entity.AddTaskRequest; import com.galaxis.rcs.common.entity.AddTaskResult; 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; @@ -32,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); } } @@ -75,6 +79,72 @@ public class RCS { return result; } + public static Object runPath() { + String executorId = "10"; // 执行器ID + String lpn = "pallet1124"; + long envId = 1; + + LogisticsRuntime logisticsRuntime = LogisticsRuntimeService.INSTANCE.findByEnvCode(envId); + RcsTaskBiz bizTask = new RcsTaskBiz(); + bizTask.setBizTaskId(100L); + bizTask.setEnvId(envId); + bizTask.setBizType(BizTaskType.CARRY.toString()); + bizTask.setLpn(lpn); + bizTask.setPriority(1); + bizTask.setTaskFrom("rack1_0_1_0"); + bizTask.setTaskTo("54_0_0_0"); + bizTask.setAllocatedExecutorId(executorId); + bizTask.setBizTaskPayload("N/A"); + bizTask.setBizTaskErrorInfo("N/A"); + bizTask.setBizTaskDescription("N/A"); + bizTask.setBizTaskStatus(BizTaskStatus.WAITING_FOR_DISPATCH.toString()); + + PlanTaskSequence planSequence = new PlanTaskSequence(executorId, logisticsRuntime, bizTask, "demo"); + + CarryTask carryTask = new CarryTask( + executorId, lpn, 1, + new StoreLocation("rack1", 0, 1, 0), + new StoreLocation("54", 0, 0, 0) + ); + + logisticsRuntime.pathPlannerMap.get("cl2") + .planCarryTask(planSequence, "17", LCCDirection.DOWN, carryTask); + return planSequence.toPrettyMap(); + } + + public static Object runPath2() { + String executorId = "10"; // 执行器ID + String lpn = "pallet1124"; + long envId = 1; + + LogisticsRuntime logisticsRuntime = LogisticsRuntimeService.INSTANCE.findByEnvCode(envId); + RcsTaskBiz bizTask = new RcsTaskBiz(); + bizTask.setBizTaskId(100L); + bizTask.setEnvId(envId); + bizTask.setBizType(BizTaskType.CARRY.toString()); + bizTask.setLpn(lpn); + bizTask.setPriority(1); + bizTask.setTaskFrom("rack1_0_1_0"); + bizTask.setTaskTo("54_0_0_0"); + bizTask.setAllocatedExecutorId(executorId); + bizTask.setBizTaskPayload("N/A"); + bizTask.setBizTaskErrorInfo("N/A"); + bizTask.setBizTaskDescription("N/A"); + bizTask.setBizTaskStatus(BizTaskStatus.WAITING_FOR_DISPATCH.toString()); + + PlanTaskSequence planSequence = new PlanTaskSequence(executorId, logisticsRuntime, bizTask, "demo"); + + CarryTask carryTask = new CarryTask( + executorId, lpn, 1, + new StoreLocation("rack1", 0, 1, 0), + new StoreLocation("54", 0, 0, 0) + ); + + logisticsRuntime.pathPlannerMap.get("cl2") + .planCarryTask(planSequence, "27", LCCDirection.UP, carryTask); + return planSequence.toPrettyMap(); + } + public static void runDemo() { String executorId = "10"; // 执行器ID String lpn = "pallet1124"; diff --git a/servo/src/main/java/com/galaxis/rcs/common/enums/LCCDirection.java b/servo/src/main/java/com/galaxis/rcs/common/enums/LCCDirection.java index 074d0fd..7b073ef 100644 --- a/servo/src/main/java/com/galaxis/rcs/common/enums/LCCDirection.java +++ b/servo/src/main/java/com/galaxis/rcs/common/enums/LCCDirection.java @@ -1,7 +1,7 @@ package com.galaxis.rcs.common.enums; /** - * 路径点与货位 / 充电器关联记录 + * 方向枚举 */ public enum LCCDirection { /** diff --git a/servo/src/main/java/com/galaxis/rcs/common/enums/PlanTaskType.java b/servo/src/main/java/com/galaxis/rcs/common/enums/PlanTaskType.java index 55c2b29..8af44bb 100644 --- a/servo/src/main/java/com/galaxis/rcs/common/enums/PlanTaskType.java +++ b/servo/src/main/java/com/galaxis/rcs/common/enums/PlanTaskType.java @@ -5,6 +5,7 @@ package com.galaxis.rcs.common.enums; */ public enum PlanTaskType { MOVE, // 移动任务 + MOVE_BACKWARD, // 移动任务 ROTATION, // 旋转任务 LOAD, // 取货任务 UNLOAD, // 装载任务 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..5b8beb3 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; @@ -58,6 +63,13 @@ public class PlanTaskSequence { return task; } + public RcsTaskPlan addMoveBackward(String wayPointId) { + RcsTaskPlan task = this.createTaskPlanEntity(PlanTaskType.MOVE_BACKWARD.toString()); + task.setTargetId(wayPointId); + this.lastWayPointId = wayPointId; + return task; + } + // 添加旋转动作 public RcsTaskPlan addRotationTo(float rotationAngle) { RcsTaskPlan task = this.createTaskPlanEntity(PlanTaskType.ROTATION.toString()); @@ -94,4 +106,43 @@ 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 MOVE_BACKWARD: + taskStr = "MOVE_BACKWARD " + 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/path/GraphInitializer.java b/servo/src/main/java/com/galaxis/rcs/plan/path/GraphInitializer.java index 698c7a5..e3dd8cf 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path/GraphInitializer.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/GraphInitializer.java @@ -9,87 +9,87 @@ import java.util.Map; public class GraphInitializer { - public NavigationGraph initializeGraph(LogisticsRuntime runtime, String agvType, List> jsonData) { - if (runtime.executorGraphMap.containsKey(agvType)) { - return runtime.executorGraphMap.get(agvType); - } - NavigationGraph graph = new NavigationGraph(); - runtime.executorGraphMap.put(agvType, graph); - - // 第一步：创建所有节点 - for (Map nodeData : jsonData) { - String id = (String) nodeData.get("id"); - - // 判断是否是 way 类型才创建 NavigationNode - if (!"way".equals(nodeData.get("t"))) { - continue; - } - - List> tf = (List>) nodeData.get("tf"); - float x = tf.get(0).get(0); - float z = tf.get(0).get(2); - - // 检查是否为可旋转点 - Map dt = (Map) nodeData.get("dt"); - boolean rotatable = false; - if (dt.containsKey("agvRotation")) { - rotatable = true; - } - - // 添加节点 - graph.addNode(new NavigationNode(id, x, z, rotatable)); - } - - // 第二步：添加路径连接 - for (Map nodeData : jsonData) { - if (!"way".equals(nodeData.get("t"))) continue; - - String id = (String) nodeData.get("id"); - Map dt = (Map) nodeData.get("dt"); - - List outEdges = (List) 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 nodeData : jsonData) { - if (!"way".equals(nodeData.get("t"))) continue; - - String nodeId = (String) nodeData.get("id"); - Map dt = (Map) nodeData.get("dt"); - - if (dt.containsKey("linkStore")) { - List> linkStores = (List>) dt.get("linkStore"); - for (Map 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; - } +// public NavigationGraph initializeGraph(LogisticsRuntime runtime, String agvType, List> jsonData) { +// if (runtime.pathPlannerMap.containsKey(agvType)) { +// return runtime.pathPlannerMap.get(agvType); +// } +// NavigationGraph graph = new NavigationGraph(); +// runtime.pathPlannerMap.put(agvType, graph); +// +// // 第一步：创建所有节点 +// for (Map nodeData : jsonData) { +// String id = (String) nodeData.get("id"); +// +// // 判断是否是 way 类型才创建 NavigationNode +// if (!"way".equals(nodeData.get("t"))) { +// continue; +// } +// +// List> tf = (List>) nodeData.get("tf"); +// float x = tf.get(0).get(0); +// float z = tf.get(0).get(2); +// +// // 检查是否为可旋转点 +// Map dt = (Map) nodeData.get("dt"); +// boolean rotatable = false; +// if (dt.containsKey("agvRotation")) { +// rotatable = true; +// } +// +// // 添加节点 +// graph.addNode(new NavigationNode(id, x, z, rotatable)); +// } +// +// // 第二步：添加路径连接 +// for (Map nodeData : jsonData) { +// if (!"way".equals(nodeData.get("t"))) continue; +// +// String id = (String) nodeData.get("id"); +// Map dt = (Map) nodeData.get("dt"); +// +// List outEdges = (List) 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 nodeData : jsonData) { +// if (!"way".equals(nodeData.get("t"))) continue; +// +// String nodeId = (String) nodeData.get("id"); +// Map dt = (Map) nodeData.get("dt"); +// +// if (dt.containsKey("linkStore")) { +// List> linkStores = (List>) dt.get("linkStore"); +// for (Map 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; +// } } diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/AGVPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/AGVPathPlanner.java deleted file mode 100644 index 8941ab8..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/AGVPathPlanner.java +++ /dev/null @@ -1,8 +0,0 @@ -package com.galaxis.rcs.plan.path2; - -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; - -public class AGVPathPlanner { -} 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 new file mode 100644 index 0000000..7b9a332 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java @@ -0,0 +1,220 @@ +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 nodeWeights = Maps.newConcurrentMap(); + private final Map blockedNodes = Maps.newConcurrentMap(); + + public AStarPathPlanner(NavigationGraph graph) { + this.graph = graph; + } + + // 路径规划状态 + public List 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 visited = new HashMap<>(); + PriorityQueue 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 open, + Map 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 buildPath(State state) { + LinkedList 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; + } +} 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 4c3acf4..837b1b4 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 @@ -1,83 +1,67 @@ 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 distanceCache = Maps.newConcurrentMap(); + + /** + * 缓存邻居节点列表，避免重复查询 + */ + private final Map> neighborCache = Maps.newConcurrentMap(); + + /** + * 添加路径缓存 + */ + private final Map> pathCache = Maps.newConcurrentMap(); + private final Map nodeMap = new HashMap<>(); private final Map> storeToNodes = new HashMap<>(); - public NavigationGraph() { + public NavigationGraph(LogisticsRuntime runtime) { + this.runtime = runtime; } -// private void initialize(LogisticsRuntime runtime) { -// var items = runtime.getStaticItems(); -// for (Map item : items) { -// String type = (String) item.get("t"); -// if ("way".equals(type)) { -// float[][] tf = (float[][]) item.get("tf"); -// String id = (String) item.get("id"); -// Map dt = (Map) item.get("dt"); -// -// // 提取坐标 -// float x = tf[0][0]; -// float z = tf[0][2]; // Z向下增长 -// -// // 检查可旋转性 -// boolean rotatable = dt.containsKey("agvRotation"); -// -// // 提取邻居节点 -// List in = (List) dt.get("in"); -// List out = (List) dt.get("out"); -// Set neighbors = new HashSet<>(); -// if (in != null) neighbors.addAll(in); -// if (out != null) neighbors.addAll(out); -// -// // 提取货位链接 -// List storeLinks = new ArrayList<>(); -// List> links = (List>) dt.get("linkStore"); -// if (links != null) { -// for (Map 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 it = node.neighbors().iterator(); -// while (it.hasNext()) { -// String neighborId = it.next(); -// if (!nodeMap.containsKey(neighborId)) { -// it.remove(); // 移除无效邻居 -// } -// } -// } -// } + public List getCachedPath(String startId, LCCDirection startDirection, + String endId, LCCDirection endDirection) { + String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection; + return pathCache.get(cacheKey); + } - public Node getNode(String id) { - return nodeMap.get(id); + public void cachePath(String startId, LCCDirection startDirection, + String endId, LCCDirection endDirection, + List path) { + String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection; + pathCache.put(cacheKey, path); } - 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) { @@ -92,4 +76,68 @@ public class NavigationGraph { 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 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 in = (List) dt.get("in"); + List out = (List) dt.get("out"); + Set neighbors = new HashSet<>(); + if (in != null) neighbors.addAll(in); + if (out != null) neighbors.addAll(out); + + // 提取货位链接 + List storeLinks = new ArrayList<>(); + List> links = (List>) dt.get("linkStore"); + if (links != null) { + for (Map 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 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); + } } diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java new file mode 100644 index 0000000..834439d --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java @@ -0,0 +1,168 @@ +package com.galaxis.rcs.plan.path2; + +import com.galaxis.rcs.common.enums.LCCDirection; + +import java.util.*; + +public class PathUtils { + /** + * 计算移动方向 + */ + public static LCCDirection calculateMoveDirection(Node from, Node to) { + float dx = to.x() - from.x(); + float dz = to.z() - from.z(); // 注意：Z轴向下增长 + + // 考虑左手坐标系：X向右，Z向下 + if (Math.abs(dx) > Math.abs(dz)) { + return dx > 0.5 ? LCCDirection.RIGHT : LCCDirection.LEFT; + } else { + // 注意：Z向下增长，所以Z值增加表示向下移动 + return dz > 0.5 ? LCCDirection.DOWN : LCCDirection.UP; + } + } + + /** + * 检查方向是否有效（侧插式专用） + */ + public static boolean isValidForSideLoader(Node node, int bay, LCCDirection direction) { + Optional link = node.storeLinks().stream() + .filter(l -> l.bay() == bay) + .findFirst(); + + if (link.isEmpty()) return false; + + LCCDirection requiredDirection = convertForSideLoader(link.get().direction()); + return direction == requiredDirection; + } + + /** + * 转换货位方向到AGV所需方向（侧插式专用） + */ + public static LCCDirection convertForSideLoader(LCCDirection storeDirection) { + /* + * 侧插式AGV方向规则： + * - 货位在上方 → 车头向右（货叉朝左） + * - 货位在下方 → 车头向左（货叉朝左） + * - 货位在左方 → 车头向上（货叉朝左） + * - 货位在右方 → 车头向下（货叉朝左） + */ + return switch (storeDirection) { + case UP -> LCCDirection.RIGHT; + case DOWN -> LCCDirection.LEFT; + case LEFT -> LCCDirection.UP; + case RIGHT -> LCCDirection.DOWN; + }; + } + + /** + * 获取最近的旋转点 + */ + public static Node findNearestRotationNode(NavigationGraph graph, Node from, LCCDirection currentDir, + LCCDirection requiredDir) { + // 如果当前方向已经匹配，不需要旋转 + if (currentDir == requiredDir) return from; + + // 使用Dijkstra算法查找最近的旋转点 + Map distances = new HashMap<>(); + Map predecessors = new HashMap<>(); + PriorityQueue queue = new PriorityQueue<>(Comparator.comparingDouble(distances::get)); + + distances.put(from, 0f); + queue.add(from); + + Node result = null; + + while (!queue.isEmpty()) { + Node current = queue.poll(); + + // 找到可旋转点 + if (current.rotatable()) { + result = current; + break; + } + + // 探索邻居 + for (Node neighbor : graph.getNeighbors(current)) { + float newDist = distances.get(current) + graph.distance(current, neighbor); + + if (newDist < distances.getOrDefault(neighbor, Float.MAX_VALUE)) { + distances.put(neighbor, newDist); + predecessors.put(neighbor, current); + queue.add(neighbor); + } + } + } + + return result; + } + + /** + * 获取相反方向 + */ + public static LCCDirection getOppositeDirection(LCCDirection dir) { + return switch (dir) { + case UP -> LCCDirection.DOWN; + case DOWN -> LCCDirection.UP; + case LEFT -> LCCDirection.RIGHT; + case RIGHT -> LCCDirection.LEFT; + }; + } + + /** + * 计算旋转代价 + */ + public static float calculateRotationCost(LCCDirection from, LCCDirection to, float ROTATION_COST_PER_DEGREE) { + float angle1 = getRequiredDirection(from); + float angle2 = getRequiredDirection(to); + float diff = Math.abs(angle1 - angle2); + diff = Math.min(diff, 360 - diff); + // return diff * ROTATION_COST_PER_DEGREE; + return 0.1f; + } + + /** + * 获取所需方向的角度 + */ + public static float getRequiredDirection(LCCDirection direction) { + return switch (direction) { + case UP -> 90f; + case DOWN -> 270f; + case LEFT -> 180f; + case RIGHT -> 0f; + }; + } + + /** + * 转换货位方向到AGV所需方向 + */ + public static LCCDirection convertStoreDirection(LCCDirection storeDirection) { + // 转换规则: 货位在路径点的方位 -> AGV所需方向 + return switch (storeDirection) { + case UP -> LCCDirection.RIGHT; // 货位在上方 → 车头向右 + case DOWN -> LCCDirection.LEFT; // 货位在下方 → 车头向左 + case LEFT -> LCCDirection.UP; // 货位在左方 → 车头向上 + case RIGHT -> LCCDirection.DOWN; // 货位在右方 → 车头向下 + }; + } + + /** + * 将角度转换为 LCCDirection + */ + public static LCCDirection convertAngleToDirection(float angle) { + // 标准化角度 + angle = ((angle % 360f) + 360f) % 360f; + + if (angle >= 315f || angle < 45f) return LCCDirection.RIGHT; + if (angle >= 45f && angle < 135f) return LCCDirection.UP; + if (angle >= 135f && angle < 225f) return LCCDirection.LEFT; + return LCCDirection.DOWN; + } + + /** + * 计算方向角度差 + */ + public static float angleDifference(float angle1, float angle2) { + float diff = Math.abs(angle1 - angle2) % 360; + return diff > 180 ? 360 - diff : diff; + } +} 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 new file mode 100644 index 0000000..e11e2de --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java @@ -0,0 +1,142 @@ +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 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 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 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 toRotation = astar.findPath( + lastState.node().id(), lastState.direction(), + rotationNode.id(), loadNodeDirection.direction() + ); + toLoadPath.addAll(toRotation); + + // 从旋转点到目标点 + List fromRotation = astar.findPath( + rotationNode.id(), loadNodeDirection.direction(), + loadNodeDirection.node().id(), loadNodeDirection.direction() + ); + toLoadPath.addAll(fromRotation); + } + } + } + + // 规划到放货点路径 + List 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 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; + } + + /** + * 获取指定节点和货架的所需朝向 + */ + private LCCDirection getRequiredHeading(Node node, int bay) { + return node.storeLinks().stream() + .filter(link -> link.bay() == bay) + .findFirst() + .map(link -> link.direction()) + .orElseThrow(() -> new RuntimeException("Not found storeLink in id=" + node.id() + ", bay=" + bay)); + } + + /** + * 根据A*状态，生成移动指令序列 + */ + private void generateMoves(PlanTaskSequence sequence, List 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) { + } +} 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 new file mode 100644 index 0000000..bd7f3d7 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/State.java @@ -0,0 +1,19 @@ +package com.galaxis.rcs.plan.path2; + +import com.galaxis.rcs.common.enums.LCCDirection; + +/** + * A* 路径规划状态 + */ +public record State(Node node, + LCCDirection direction, + 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/galaxis/rcs/plan/path2/StoreLink.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java index 8008632..f171d5d 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java @@ -2,6 +2,15 @@ package com.galaxis.rcs.plan.path2; import com.galaxis.rcs.common.enums.LCCDirection; +/** + * A* 路径与库位的连接定义 + * + * @param storeId 库位ID + * @param bay 列 + * @param level 层 + * @param cell 格 + * @param direction 方位（路径相对于库位的方位，比如路标的"左边"是库位） + */ public record StoreLink( String storeId, int bay, int level, int cell, LCCDirection direction ) { diff --git a/servo/src/main/java/com/galaxis/rcs/plan/task/CarryTask.java b/servo/src/main/java/com/galaxis/rcs/plan/task/CarryTask.java new file mode 100644 index 0000000..89d1a24 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/task/CarryTask.java @@ -0,0 +1,34 @@ +package com.galaxis.rcs.plan.task; + +import com.galaxis.rcs.common.entity.StoreLocation; + +/** + * 搬运任务 + * 
+ * {
+ *   type: 'carry',          // 任务类型
+ *   agv: 'cl2',             // 车号
+ *   lpn: 'pallet1124',      // 托盘ID,用于校验，规划器不用管
+ *   priority: 1,            // 优先级,用于排车，规划器不用管
+ *   // 搬运源货位
+ *   from: {
+ *     item: 'rack1',// 货架编号
+ *     bay: 0,       // 货架列
+ *     level: 1,     // 货架层(用于机械臂，规划器不用管)
+ *     cell: 0       // 货架格(用于机械臂，规划器不用管)
+ *   },
+ *   // 目标货位
+ *   to: {
+ *     item: '54'    // 地堆货位号
+ *   }
+ * }
+ *
+ */ +public record CarryTask( + String agv, + String lpn, + int priority, + StoreLocation from, + StoreLocation to +) { +} diff --git a/servo/src/main/java/com/yvan/logisticsEnv/EnvStartParam.java b/servo/src/main/java/com/yvan/logisticsEnv/EnvStartParam.java index 52ea822..45d9b27 100644 --- a/servo/src/main/java/com/yvan/logisticsEnv/EnvStartParam.java +++ b/servo/src/main/java/com/yvan/logisticsEnv/EnvStartParam.java @@ -18,6 +18,6 @@ public class EnvStartParam { /** * 时间倍速(仿真环境专用) */ - private Integer timespeed; + private Integer timeRate; } diff --git a/servo/src/main/java/com/yvan/logisticsEnv/LogisticsEnv.java b/servo/src/main/java/com/yvan/logisticsEnv/LogisticsEnv.java index 61f75fd..b8e8ec5 100644 --- a/servo/src/main/java/com/yvan/logisticsEnv/LogisticsEnv.java +++ b/servo/src/main/java/com/yvan/logisticsEnv/LogisticsEnv.java @@ -48,11 +48,15 @@ public class LogisticsEnv { */ private int timeRate; - /** - * 获取AGV车列表 - */ - List getExecutorList() { - return Lists.newArrayList(); + public void start(EnvStartParam param) { + if (this.isRunning) { + throw new IllegalStateException("环境已经在运行中"); + } + this.startParam = param; + this.isRunning = true; + this.startTime = System.currentTimeMillis(); + this.stopTime = 0L; + this.timeRate = this.startParam.getTimeRate(); } public long currentTimeMillis() { diff --git a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java index 84e80a7..4c97685 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java +++ b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java @@ -1,21 +1,22 @@ package com.yvan.logisticsModel; import com.fasterxml.jackson.annotation.JsonIgnore; -import com.galaxis.rcs.common.entity.RcsTaskBiz; -import com.galaxis.rcs.plan.path.NavigationGraph; +import com.galaxis.rcs.plan.path2.NavigationGraph; +import com.galaxis.rcs.plan.path2.PtrPathPlanner; import com.galaxis.rcs.task.TaskDispatchFactory; 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 com.yvan.workbench.model.entity.Vector2; import lombok.extern.slf4j.Slf4j; import org.clever.core.Conv; import org.clever.core.json.JsonWrapper; -import java.util.Iterator; import java.util.List; import java.util.Map; +import java.util.Set; /** * 物流上下文运行时 @@ -54,7 +55,7 @@ public class LogisticsRuntime { * AGV导航地图 车类型 t -> NavigationGraph */ @JsonIgnore - public final Map executorGraphMap = Maps.newHashMap(); + public final Map pathPlannerMap = Maps.newHashMap(); /** * 楼层目录 catalogCode -> Floor @@ -156,21 +157,33 @@ public class LogisticsRuntime { } } - public void start() { + public void start(EnvStartParam param) { // 开启所有机器人的任务调度 + Set executorTypes = Sets.newHashSet(); for (ExecutorItem executorItem : executorItemMap.values()) { executorItem.mapReady(); + executorTypes.add(executorItem.getT()); log.info("Executor {} started", executorItem.getId()); } + // 初始化地图 + NavigationGraph graph = new NavigationGraph(this); + graph.init(); + for (String type : executorTypes) { + this.pathPlannerMap.put(type, new PtrPathPlanner(graph)); + } + + this.logisticsEnv.start(param); this.taskDispatchFactory.startPolling(); } + public boolean isRunning() { + return this.logisticsEnv.isRunning(); + } + public Iterable getStaticItems() { - for (Floor floor : this.floorMap.values()) { - // return floor.itemMap.values(); - return floor.itemMap.values(); - } - return null; + return () -> this.floorMap.values().stream() + .flatMap(floor -> floor.itemMap.values().stream()) + .iterator(); } } diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java index 60e818d..2e1a5f2 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java +++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java @@ -12,6 +12,7 @@ import java.util.ArrayList; import java.util.List; import java.util.Map; import java.util.concurrent.BlockingQueue; + //0.4m/ss // a max 1.2m/s //90 = 3.5s cl2 //90 = 5s // cLX @@ -77,6 +78,7 @@ public class PtrAgvItem extends ExecutorItem { this.startConnector(); } + /** * 启动连接器线程 */ @@ -132,7 +134,7 @@ public class PtrAgvItem extends ExecutorItem { throw new RuntimeException("方向错误"); } - } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX != pointItem.logicX) { + } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX != pointItem.logicX) { 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("方向错误"); @@ -211,7 +213,7 @@ public class PtrAgvItem extends ExecutorItem { } public boolean isFree() { - return (this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning()); + return (this.logisticsRuntime.isRunning() && this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning()); } public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map raw) { @@ -228,7 +230,6 @@ public class PtrAgvItem extends ExecutorItem { } - private static class COperationType { public static final short move = 0; public static final short load = 1; 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..146d183 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,14 @@ public class EnvController { return Model.newSuccess(true); } + public static Model runPath() { + return Model.newSuccess(RCS.runPath()); + } + + public static Model runPath2() { + return Model.newSuccess(RCS.runPath2()); + } + public static Model getDemo() { return Model.newSuccess(RCS.getDemo()); } diff --git a/servo/src/main/resources/application-dev.yml b/servo/src/main/resources/application-dev.yml index f70aaf9..43057c6 100644 --- a/servo/src/main/resources/application-dev.yml +++ b/servo/src/main/resources/application-dev.yml @@ -70,7 +70,7 @@ web: timeout: -1 read-only: false hot-reload: - enable: true + enable: false # watchFile: './build/.hotReload' interval: 1s exclude-packages: [ ]