From 48f1c5f0788c8f5932f1e35b862d6e85bcbd5906 Mon Sep 17 00:00:00 2001 From: luoyifan Date: Mon, 23 Jun 2025 21:59:28 +0800 Subject: [PATCH 1/5] =?UTF-8?q?PtrPathPlanner=20=E7=AE=97=E6=B3=95?= =?UTF-8?q?=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- servo/src/main/java/com/galaxis/rcs/RCS.java | 35 +++++ .../galaxis/rcs/plan/path/GraphInitializer.java | 166 ++++++++++----------- .../com/galaxis/rcs/plan/path2/AGVPathPlanner.java | 8 - .../galaxis/rcs/plan/path2/AStarPathPlanner.java | 115 ++++++++++++++ .../galaxis/rcs/plan/path2/NavigationGraph.java | 116 +++++++------- .../com/galaxis/rcs/plan/path2/PtrPathPlanner.java | 68 +++++++++ .../java/com/galaxis/rcs/plan/path2/State.java | 13 ++ .../java/com/galaxis/rcs/plan/task/CarryTask.java | 34 +++++ .../java/com/yvan/logisticsEnv/EnvStartParam.java | 2 +- .../java/com/yvan/logisticsEnv/LogisticsEnv.java | 13 +- .../com/yvan/logisticsModel/LogisticsRuntime.java | 32 ++-- .../java/com/yvan/logisticsModel/PtrAgvItem.java | 7 +- servo/src/main/resources/application-dev.yml | 2 +- 13 files changed, 443 insertions(+), 168 deletions(-) delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/AGVPathPlanner.java create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/State.java create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/task/CarryTask.java diff --git a/servo/src/main/java/com/galaxis/rcs/RCS.java b/servo/src/main/java/com/galaxis/rcs/RCS.java index 68e39a9..a2c9b75 100644 --- a/servo/src/main/java/com/galaxis/rcs/RCS.java +++ b/servo/src/main/java/com/galaxis/rcs/RCS.java @@ -4,10 +4,12 @@ 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.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.logisticsModel.LogisticsRuntime; @@ -75,6 +77,39 @@ public class RCS { return result; } + public static Model 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("20_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("20", 0, 0, 0) + ); + + logisticsRuntime.pathPlannerMap.get("cl2") + .planCarryTask(planSequence, "17", 270f, carryTask); + return Model.newSuccess(planSequence); + } + public static void runDemo() { String executorId = "10"; // 执行器ID String lpn = "pallet1124"; 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..47882cf --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java @@ -0,0 +1,115 @@ +package com.galaxis.rcs.plan.path2; + +import com.galaxis.rcs.common.enums.LCCDirection; + +import java.util.*; + +public class AStarPathPlanner { + private static final float ROTATION_COST_PER_DEGREE = 0.1f; + private final NavigationGraph graph; + + public AStarPathPlanner(NavigationGraph graph) { + this.graph = graph; + } + + // 路径规划状态 + public List findPath(String startId, float startDirectionAngle, String endId, float endDirectionAngle) { + Node start = graph.getNode(startId); + Node goal = graph.getNode(endId); + if (start == null || goal == null) return Collections.emptyList(); + + // 使用复合键避免状态重复 + Map visited = new HashMap<>(); + PriorityQueue open = new PriorityQueue<>(); + + // 初始状态 + State initialState = new State(start, startDirectionAngle, 0, heuristic(start, goal), null); + open.add(initialState); + visited.put(stateKey(start.id(), startDirectionAngle), initialState); + + while (!open.isEmpty()) { + State current = open.poll(); + + // 到达目标节点且方向匹配 + if (current.node().id().equals(endId) && + current.directionAngle() == endDirectionAngle) { + return buildPath(current); + } + + // 处理邻居移动 + for (Node neighbor : graph.getNeighbors(current.node())) { + // 计算可能的两种方向（前进/后退） + float[] possibleHeadings = { + current.directionAngle(), // 前进方向不变 + (current.directionAngle() + 180) % 360 // 后退方向反转 + }; + + for (float nextHeading : possibleHeadings) { + float moveCost = graph.distance(current.node(), neighbor); + considerState(current, neighbor, nextHeading, moveCost, open, visited, goal); + } + } + + // 处理旋转（仅在可旋转节点） + if (current.node().rotatable()) { + for (float rotation : new float[]{0, 90, 180, 270}) { + if (rotation == current.directionAngle()) continue; + + float angleDiff = Math.min( + Math.abs(rotation - current.directionAngle()), + 360 - Math.abs(rotation - current.directionAngle()) + ); + float rotationCost = angleDiff * ROTATION_COST_PER_DEGREE; + + considerState(current, current.node(), rotation, + rotationCost, open, visited, goal); + } + } + } + return Collections.emptyList(); + } + + private void considerState(State current, Node nextNode, float nextHeading, + float cost, PriorityQueue open, + Map visited, Node goal) { + String key = stateKey(nextNode.id(), nextHeading); + float newG = current.g() + cost; + + if (!visited.containsKey(key) || visited.get(key).g() > newG) { + float h = heuristic(nextNode, goal); + State newState = new State(nextNode, nextHeading, 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.node()); + state = state.parent(); + } + return path; + } + + private float heuristic(Node a, Node b) { + return graph.distance(a, b); + } + + private String stateKey(String nodeId, float directionAngle) { + return nodeId + "|" + directionAngle; + } + + /** + * 根据方向要求计算目标方向 + */ + public static float getRequiredDirection(LCCDirection direction) { + return switch (direction) { + case UP -> 90f; // 车头朝上 + case DOWN -> 270f; // 车头朝下 + case LEFT -> 180f; // 车头朝左 + case RIGHT -> 0f; // 车头朝右 + default -> 0; // 默认为右 + }; + } +} 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..61b36be 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 @@ -2,6 +2,7 @@ package com.galaxis.rcs.plan.path2; import com.galaxis.rcs.common.enums.LCCDirection; import com.yvan.logisticsModel.LogisticsRuntime; +import com.yvan.logisticsModel.StaticItem; import java.util.*; @@ -9,68 +10,69 @@ import java.util.*; * A* 导航图 */ public class NavigationGraph { + private final LogisticsRuntime runtime; 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 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"); + + // 提取邻居节点 + 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/PtrPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java new file mode 100644 index 0000000..7fd6c29 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java @@ -0,0 +1,68 @@ +package com.galaxis.rcs.plan.path2; + +import com.galaxis.rcs.plan.PlanTaskSequence; +import com.galaxis.rcs.plan.task.CarryTask; + +import java.util.List; + +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 seq, String startId, float initDirectionAngle, CarryTask task) { + // 取货点 + String pickupRackId = task.from().rackId(); + int pickupBay = task.from().bay(); + Node pickupNode = findStoreNode(pickupRackId, pickupBay); + float pickupRotationAngle = getRequiredHeading(pickupNode, pickupBay); + + // 放货点 + String dropRackId = task.to().rackId(); + int dropBay = task.to().bay(); + Node dropNode = findStoreNode(dropRackId, dropBay); + float dropRotationAngle = getRequiredHeading(dropNode, 0); + + // 规划到取货点路径 + List toPickupPath = astar.findPath(startId, initDirectionAngle, pickupNode.id(), pickupRotationAngle); + + // 规划到放货点路径 + List toDeliverPath = astar.findPath(pickupNode.id(), pickupRotationAngle, dropNode.id(), dropRotationAngle); + + // 生成指令序列 + 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(); + } + + 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(); + } + + private float getRequiredHeading(Node node, int bay) { + return node.storeLinks().stream() + .filter(link -> link.bay() == bay) + .findFirst() + .map(link -> AStarPathPlanner.getRequiredDirection(link.direction())) + .orElse(0f); + } + + private void generateMoves(PlanTaskSequence seq, List path) { + // 简化的指令生成（实际需处理方向变化） + for (int i = 1; i < path.size(); i++) { + Node node = path.get(i); + seq.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 new file mode 100644 index 0000000..5848382 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/State.java @@ -0,0 +1,13 @@ +package com.galaxis.rcs.plan.path2; + +public record State(Node node, + float 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/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..4e40165 100644 --- a/servo/src/main/java/com/yvan/logisticsEnv/LogisticsEnv.java +++ b/servo/src/main/java/com/yvan/logisticsEnv/LogisticsEnv.java @@ -48,11 +48,14 @@ public class LogisticsEnv { */ private int timeRate; - /** - * 获取AGV车列表 - */ - List getExecutorList() { - return Lists.newArrayList(); + public void start() { + if (this.isRunning) { + throw new IllegalStateException("环境已经在运行中"); + } + 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 5ece3b9..b0e03e2 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java +++ b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java @@ -1,21 +1,21 @@ 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.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 +54,7 @@ public class LogisticsRuntime { * AGV导航地图 车类型 t -> NavigationGraph */ @JsonIgnore - public final Map executorGraphMap = Maps.newHashMap(); + public final Map pathPlannerMap = Maps.newHashMap(); /** * 楼层目录 catalogCode -> Floor @@ -158,19 +158,31 @@ public class LogisticsRuntime { public void start() { // 开启所有机器人的任务调度 + 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(); 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 8ec069d..df0ec98 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java +++ b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java @@ -11,6 +11,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 @@ -70,6 +71,7 @@ public class PtrAgvItem extends ExecutorItem { * 连接器线程 */ private final PtrAgvConnectorThread connectorThread; + /** * 启动连接器线程 */ @@ -125,7 +127,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("方向错误"); @@ -201,7 +203,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) { @@ -218,7 +220,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/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: [ ] From 7aef1e7d84b6010cda397aadacaa0f23e2e44e8b Mon Sep 17 00:00:00 2001 From: luoyifan Date: Mon, 23 Jun 2025 22:56:17 +0800 Subject: [PATCH 2/5] =?UTF-8?q?PtrPathPlanner=20=E7=AE=97=E6=B3=95?= =?UTF-8?q?=E6=B5=8B=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()); } From afd4b862c36b41c7065726f57bbaeb13d54229ad Mon Sep 17 00:00:00 2001 From: luoyifan Date: Tue, 24 Jun 2025 00:08:25 +0800 Subject: [PATCH 3/5] =?UTF-8?q?PtrPathPlanner=20=E7=AE=97=E6=B3=95?= =?UTF-8?q?=E6=80=A7=E8=83=BD=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../com/galaxis/rcs/common/enums/LCCDirection.java | 2 +- .../galaxis/rcs/plan/path2/AStarPathPlanner.java | 172 ++++++++++++++------- .../galaxis/rcs/plan/path2/NavigationGraph.java | 90 +++++++---- .../java/com/galaxis/rcs/plan/path2/PathUtils.java | 149 ++++++++++++++++++ .../com/galaxis/rcs/plan/path2/PtrPathPlanner.java | 73 ++++++--- .../java/com/galaxis/rcs/plan/path2/State.java | 5 +- .../java/com/galaxis/rcs/plan/path2/StoreLink.java | 9 ++ 7 files changed, 389 insertions(+), 111 deletions(-) create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java 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/plan/path2/AStarPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java index 94ce6d0..de1356f 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 @@ -1,128 +1,180 @@ 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.1f; + 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 startDirectionAngle, String endId, LCCDirection endDirectionAngle) { + public List findPath(String startId, LCCDirection startDirection, String endId, LCCDirection endDirection) { Node start = graph.getNode(startId); - Node goal = graph.getNode(endId); - if (start == null || goal == null) return Collections.emptyList(); + 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, startDirectionAngle, 0, heuristic(start, goal), null); + State initialState = new State(start, startDirection, 0, heuristic(start, end), null); open.add(initialState); - visited.put(stateKey(start.id(), startDirectionAngle), initialState); + visited.put(stateKey(start.id(), startDirection), initialState); while (!open.isEmpty()) { State current = open.poll(); // 到达目标节点且方向匹配 if (current.node().id().equals(endId) && - current.directionAngle() == endDirectionAngle) { + current.direction() == endDirection) { return buildPath(current); } - // 处理邻居移动 + // 处理邻边移动 for (Node neighbor : graph.getNeighbors(current.node())) { - // 计算可能的两种方向（前进/后退） - 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}; + // 检查节点是否被阻塞 + if (isBlocked(neighbor.id())) continue; + + LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor); + + // 前进所需方向 + LCCDirection forwardHeading = moveDirection; + // 后退所需方向 + LCCDirection backwardHeading = getOppositeDirection(moveDirection); + + // 尝试前进 + if (current.direction() == forwardHeading) { + float moveCost = calculateMoveCost(current.node(), neighbor); + considerState(current, neighbor, forwardHeading, + moveCost, open, visited, end); + } else if (current.node().rotatable()) { + float rotationCost = calculateRotationCost( + current.direction(), forwardHeading, ROTATION_COST_PER_DEGREE + ); + float moveCost = calculateMoveCost(current.node(), neighbor); + float totalCost = rotationCost + moveCost; + considerState(current, neighbor, forwardHeading, + totalCost, open, visited, end); } - for (LCCDirection nextHeading : possibleHeadings) { - float moveCost = graph.distance(current.node(), neighbor); - considerState(current, neighbor, nextHeading, moveCost, open, visited, goal); + // 尝试后退 + if (current.direction() == backwardHeading) { + float moveCost = calculateMoveCost(current.node(), neighbor); + considerState(current, neighbor, backwardHeading, + moveCost, open, visited, end); + } else if (current.node().rotatable()) { + float rotationCost = calculateRotationCost( + current.direction(), backwardHeading, ROTATION_COST_PER_DEGREE + ); + float moveCost = calculateMoveCost(current.node(), neighbor); + float totalCost = rotationCost + moveCost; + considerState(current, neighbor, backwardHeading, + totalCost, open, visited, end); } } - // 处理旋转（仅在可旋转节点） + // 处理原地旋转 if (current.node().rotatable()) { - for (LCCDirection rotation : new LCCDirection[]{LCCDirection.UP, LCCDirection.DOWN, LCCDirection.LEFT, LCCDirection.RIGHT}) { - if (rotation == current.directionAngle()) continue; - - // 计算旋转代价 - 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); + 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(); } - private void considerState(State current, Node nextNode, LCCDirection nextHeading, + /** + * 考虑新的状态并更新开放列表和访问记录 + * + * @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 goal) { - String key = stateKey(nextNode.id(), nextHeading); + 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, goal); - State newState = new State(nextNode, nextHeading, newG, h, current); + 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<>(); + private List buildPath(State state) { + LinkedList path = new LinkedList<>(); while (state != null) { - path.addFirst(state.node()); + path.addFirst(state); state = state.parent(); } return path; } + /** + * 启发式函数，计算两个节点之间的距离 + */ private float heuristic(Node a, Node b) { return graph.distance(a, b); - } - - private String stateKey(String nodeId, LCCDirection directionAngle) { - return nodeId + "|" + directionAngle; + // 使用曼哈顿距离？？ + // return Math.abs(a.x() - b.x()) + Math.abs(a.z() - b.z()); } /** - * 根据方向要求计算目标方向 + * 生成状态的唯一键 */ - public static float getRequiredDirection(LCCDirection direction) { - return switch (direction) { - case UP -> 90f; // 车头朝上 - case DOWN -> 270f; // 车头朝下 - case LEFT -> 180f; // 车头朝左 - case RIGHT -> 0f; // 车头朝右 - default -> 0; // 默认为右 - }; + 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) { + 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); + } + + private boolean isBlocked(String nodeId) { + return blockedNodes.containsKey(nodeId) && blockedNodes.get(nodeId) > 0.8f; } } 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 11e58af..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,16 +1,35 @@ 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<>(); @@ -18,6 +37,47 @@ public class NavigationGraph { this.runtime = runtime; } + public List getCachedPath(String startId, LCCDirection startDirection, + String endId, LCCDirection endDirection) { + String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection; + return pathCache.get(cacheKey); + } + + public void cachePath(String startId, LCCDirection startDirection, + String endId, LCCDirection endDirection, + List path) { + String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection; + pathCache.put(cacheKey, path); + } + + 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) { + return node.neighbors().stream() + .map(nodeMap::get) + .filter(Objects::nonNull) + .toList(); + } + + public float distance(Node a, Node b) { + float dx = a.x() - b.x(); + float dz = a.z() - b.z(); + return (float) Math.sqrt(dx * dx + dz * dz); + } + + public void init() { var items = runtime.getStaticItems(); for (StaticItem item : items) { @@ -32,6 +92,9 @@ public class NavigationGraph { // 检查可旋转性 boolean rotatable = dt.containsKey("agvRotation"); + if (rotatable) { + log.info("Node {} is rotatable", id); + } // 提取邻居节点 List in = (List) dt.get("in"); @@ -77,31 +140,4 @@ public class NavigationGraph { public Node getNode(String id) { return nodeMap.get(id); } - - 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) { - return node.neighbors().stream() - .map(nodeMap::get) - .filter(Objects::nonNull) - .toList(); - } - - public float distance(Node a, Node b) { - float dx = a.x() - b.x(); - float dz = a.z() - b.z(); - return (float) Math.sqrt(dx * dx + dz * dz); - } } 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..f373245 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java @@ -0,0 +1,149 @@ +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(); + + if (Math.abs(dx) > Math.abs(dz)) { + return dx > 1 ? LCCDirection.RIGHT : LCCDirection.LEFT; + } else { + return dz > 1 ? 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) { + // 使用BFS查找最近的旋转点 + Queue queue = new LinkedList<>(); + Set visited = new HashSet<>(); + queue.add(from); + visited.add(from.id()); + + while (!queue.isEmpty()) { + Node current = queue.poll(); + if (current.rotatable()) { + return current; + } + + for (Node neighbor : graph.getNeighbors(current)) { + if (!visited.contains(neighbor.id())) { + visited.add(neighbor.id()); + queue.add(neighbor); + } + } + } + return null; + } + + /** + * 获取相反方向 + */ + 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; + } + + /** + * 获取所需方向的角度 + */ + 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 index 5deb84e..aa1fddf 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 @@ -6,6 +6,9 @@ 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; @@ -15,43 +18,50 @@ public class PtrPathPlanner { this.astar = new AStarPathPlanner(graph); } - public void planCarryTask(PlanTaskSequence plan, String startId, LCCDirection initDirectionAngle, CarryTask task) { + public void planCarryTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, CarryTask task) { // 取货点 - String pickupRackId = task.from().rackId(); + String loadRackId = task.from().rackId(); int pickupBay = task.from().bay(); - Node pickupNode = findStoreNode(pickupRackId, pickupBay); - LCCDirection pickupDirectionAngle = getRequiredHeading(pickupNode, pickupBay); + NodeDirection loadNodeDirection = findNodeForStore(loadRackId, pickupBay); + if (loadNodeDirection == null) { + throw new RuntimeException("Pickup node not found for rackId=" + loadRackId + ", bay=" + pickupBay); + } // 放货点 - String dropRackId = task.to().rackId(); - int dropBay = task.to().bay(); - Node dropNode = findStoreNode(dropRackId, dropBay); - LCCDirection dropDirectionAngle = getRequiredHeading(dropNode, 0); + 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 toPickupPath = astar.findPath(startId, initDirectionAngle, pickupNode.id(), pickupDirectionAngle); + List toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction()); // 规划到放货点路径 - List toDeliverPath = astar.findPath(pickupNode.id(), pickupDirectionAngle, dropNode.id(), dropDirectionAngle); + List toUnloadPath = astar.findPath(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirection.node().id(), unloadNodeDirection.direction()); // 生成指令序列 - 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()); + 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 Node findStoreNode(String storeId, int bay) { + 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) { - return node; + LCCDirection agvDirection = convertStoreDirection(link.direction()); + return new NodeDirection(node, agvDirection); } } } - throw new RuntimeException("Not found WayPoint link Store, rackId=" + storeId + ", bay=" + bay); + return null; } /** @@ -65,13 +75,32 @@ public class PtrPathPlanner { .orElseThrow(() -> new RuntimeException("Not found storeLink in id=" + node.id() + ", bay=" + bay)); } - private void generateMoves(PlanTaskSequence plan, List path) { - // 简化的指令生成（实际需处理方向变化） + /** + * 根据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++) { - Node node = path.get(i); - plan.addMoveTo(node.id()); + State current = path.get(i); + + // 如果是旋转动作 + if (current.node().equals(prevState.node())) { + float angle = getRequiredDirection(current.direction()); + sequence.addRotationTo(angle); + } + // 移动动作 + 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 index 339ea1a..bd7f3d7 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 @@ -2,8 +2,11 @@ package com.galaxis.rcs.plan.path2; import com.galaxis.rcs.common.enums.LCCDirection; +/** + * A* 路径规划状态 + */ public record State(Node node, - LCCDirection directionAngle, + LCCDirection direction, float g, float h, State parent) 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 ) { From fd8de9e20502fa1b912318aa56944787de87d4f7 Mon Sep 17 00:00:00 2001 From: luoyifan Date: Tue, 24 Jun 2025 00:31:06 +0800 Subject: [PATCH 4/5] =?UTF-8?q?MOVE=5FBACKWARD=20=E6=94=AF=E6=8C=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../com/galaxis/rcs/common/enums/PlanTaskType.java | 1 + .../com/galaxis/rcs/plan/PlanTaskSequence.java | 10 ++ .../galaxis/rcs/plan/path2/AStarPathPlanner.java | 108 +++++++++++++++------ .../java/com/galaxis/rcs/plan/path2/PathUtils.java | 45 ++++++--- .../com/galaxis/rcs/plan/path2/PtrPathPlanner.java | 42 +++++++- 5 files changed, 158 insertions(+), 48 deletions(-) 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 7dff083..5b8beb3 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/PlanTaskSequence.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/PlanTaskSequence.java @@ -63,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()); @@ -116,6 +123,9 @@ public class PlanTaskSequence { 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; 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 de1356f..3cc6306 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 @@ -8,7 +8,7 @@ import java.util.*; import static com.galaxis.rcs.plan.path2.PathUtils.*; public class AStarPathPlanner { - private static final float ROTATION_COST_PER_DEGREE = 0.1f; + 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; // 权重因子 @@ -44,8 +44,7 @@ public class AStarPathPlanner { State current = open.poll(); // 到达目标节点且方向匹配 - if (current.node().id().equals(endId) && - current.direction() == endDirection) { + if (current.node().id().equals(endId) && current.direction() == endDirection) { return buildPath(current); } @@ -54,55 +53,85 @@ public class AStarPathPlanner { // 检查节点是否被阻塞 if (isBlocked(neighbor.id())) continue; + // 计算移动方向 LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor); // 前进所需方向 LCCDirection forwardHeading = moveDirection; // 后退所需方向 LCCDirection backwardHeading = getOppositeDirection(moveDirection); - - // 尝试前进 - if (current.direction() == forwardHeading) { - float moveCost = calculateMoveCost(current.node(), neighbor); - considerState(current, neighbor, forwardHeading, +// 尝试前进 + if (canMoveForward(current.direction(), moveDirection)) { + float moveCost = calculateMoveCost(current.node(), neighbor, false); + considerState(current, neighbor, current.direction(), moveCost, open, visited, end); - } else if (current.node().rotatable()) { - float rotationCost = calculateRotationCost( - current.direction(), forwardHeading, ROTATION_COST_PER_DEGREE - ); - float moveCost = calculateMoveCost(current.node(), neighbor); - float totalCost = rotationCost + moveCost; - considerState(current, neighbor, forwardHeading, - totalCost, open, visited, end); } - // 尝试后退 - if (current.direction() == backwardHeading) { - float moveCost = calculateMoveCost(current.node(), neighbor); - considerState(current, neighbor, backwardHeading, + 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()) { + } + // 需要旋转 + else if (current.node().rotatable()) { + // 计算需要旋转到的方向 + LCCDirection requiredDirection = calculateRequiredDirection(moveDirection); + + // 考虑旋转后移动 float rotationCost = calculateRotationCost( - current.direction(), backwardHeading, ROTATION_COST_PER_DEGREE + current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE ); - float moveCost = calculateMoveCost(current.node(), neighbor); + float moveCost = calculateMoveCost(current.node(), neighbor, false); float totalCost = rotationCost + moveCost; - considerState(current, neighbor, backwardHeading, - totalCost, open, visited, end); + + // 创建旋转状态 + 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()) { - for (LCCDirection rotation : LCCDirection.values()) { - if (rotation == current.direction()) continue; - + // 只考虑旋转到目标方向（如果可能） + if (current.direction() != endDirection) { float rotationCost = calculateRotationCost( - current.direction(), rotation, ROTATION_COST_PER_DEGREE + current.direction(), endDirection, ROTATION_COST_PER_DEGREE ); - considerState(current, current.node(), rotation, + 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(); @@ -166,15 +195,30 @@ public class AStarPathPlanner { blockedNodes.put(nodeId, blockedFactor); } - private float calculateMoveCost(Node from, Node to) { + 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/PathUtils.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java index f373245..a5f5050 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java @@ -10,12 +10,14 @@ public class PathUtils { */ public static LCCDirection calculateMoveDirection(Node from, Node to) { float dx = to.x() - from.x(); - float dz = to.z() - from.z(); + float dz = to.z() - from.z(); // 注意：Z轴向下增长 + // 考虑左手坐标系：X向右，Z向下 if (Math.abs(dx) > Math.abs(dz)) { - return dx > 1 ? LCCDirection.RIGHT : LCCDirection.LEFT; + return dx > 0 ? LCCDirection.RIGHT : LCCDirection.LEFT; } else { - return dz > 1 ? LCCDirection.DOWN : LCCDirection.UP; + // 注意：Z向下增长，所以Z值增加表示向下移动 + return dz > 0 ? LCCDirection.DOWN : LCCDirection.UP; } } @@ -55,27 +57,43 @@ public class PathUtils { /** * 获取最近的旋转点 */ - public static Node findNearestRotationNode(NavigationGraph graph, Node from) { - // 使用BFS查找最近的旋转点 - Queue queue = new LinkedList<>(); - Set visited = new HashSet<>(); + 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); - visited.add(from.id()); + + Node result = null; while (!queue.isEmpty()) { Node current = queue.poll(); + + // 找到可旋转点 if (current.rotatable()) { - return current; + result = current; + break; } + // 探索邻居 for (Node neighbor : graph.getNeighbors(current)) { - if (!visited.contains(neighbor.id())) { - visited.add(neighbor.id()); + 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 null; + + return result; } /** @@ -98,7 +116,8 @@ public class PathUtils { float angle2 = getRequiredDirection(to); float diff = Math.abs(angle1 - angle2); diff = Math.min(diff, 360 - diff); - return diff * ROTATION_COST_PER_DEGREE; + // return diff * ROTATION_COST_PER_DEGREE; + return 0.1f; } /** 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 aa1fddf..e11e2de 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 @@ -38,6 +38,32 @@ public class PtrPathPlanner { // 规划到取货点路径 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()); @@ -89,12 +115,22 @@ public class PtrPathPlanner { // 如果是旋转动作 if (current.node().equals(prevState.node())) { - float angle = getRequiredDirection(current.direction()); + float angle = PathUtils.getRequiredDirection(current.direction()); sequence.addRotationTo(angle); } - // 移动动作 + // 移动动作 - 检查是否需要后退 else { - sequence.addMoveTo(current.node().id()); + // 检查移动方向 + 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; } From f87645a7c907a5ee881d19f5769bef2d35e6f60d Mon Sep 17 00:00:00 2001 From: luoyifan Date: Tue, 24 Jun 2025 00:41:51 +0800 Subject: [PATCH 5/5] =?UTF-8?q?A*=20=E7=AE=97=E6=B3=95=E4=BC=98=E5=8C=96?= =?UTF-8?q?=E5=AE=8C=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- servo/src/main/java/com/galaxis/rcs/RCS.java | 33 ++++++++++++++++++++++ .../galaxis/rcs/plan/path2/AStarPathPlanner.java | 6 +--- .../java/com/galaxis/rcs/plan/path2/PathUtils.java | 4 +-- .../yvan/workbench/controller/EnvController.java | 4 +++ 4 files changed, 40 insertions(+), 7 deletions(-) diff --git a/servo/src/main/java/com/galaxis/rcs/RCS.java b/servo/src/main/java/com/galaxis/rcs/RCS.java index 9897fcb..fe17f4a 100644 --- a/servo/src/main/java/com/galaxis/rcs/RCS.java +++ b/servo/src/main/java/com/galaxis/rcs/RCS.java @@ -112,6 +112,39 @@ public class RCS { 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/plan/path2/AStarPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java index 3cc6306..7b9a332 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 @@ -56,11 +56,7 @@ public class AStarPathPlanner { // 计算移动方向 LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor); - // 前进所需方向 - LCCDirection forwardHeading = moveDirection; - // 后退所需方向 - LCCDirection backwardHeading = getOppositeDirection(moveDirection); -// 尝试前进 + // 尝试前进 if (canMoveForward(current.direction(), moveDirection)) { float moveCost = calculateMoveCost(current.node(), neighbor, false); considerState(current, neighbor, current.direction(), 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 index a5f5050..834439d 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java @@ -14,10 +14,10 @@ public class PathUtils { // 考虑左手坐标系：X向右，Z向下 if (Math.abs(dx) > Math.abs(dz)) { - return dx > 0 ? LCCDirection.RIGHT : LCCDirection.LEFT; + return dx > 0.5 ? LCCDirection.RIGHT : LCCDirection.LEFT; } else { // 注意：Z向下增长，所以Z值增加表示向下移动 - return dz > 0 ? LCCDirection.DOWN : LCCDirection.UP; + return dz > 0.5 ? LCCDirection.DOWN : LCCDirection.UP; } } 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 3f80f4b..146d183 100644 --- a/servo/src/main/java/com/yvan/workbench/controller/EnvController.java +++ b/servo/src/main/java/com/yvan/workbench/controller/EnvController.java @@ -36,6 +36,10 @@ public class EnvController { return Model.newSuccess(RCS.runPath()); } + public static Model runPath2() { + return Model.newSuccess(RCS.runPath2()); + } + public static Model getDemo() { return Model.newSuccess(RCS.getDemo()); }