From 48f1c5f0788c8f5932f1e35b862d6e85bcbd5906 Mon Sep 17 00:00:00 2001 From: luoyifan Date: Mon, 23 Jun 2025 21:59:28 +0800 Subject: [PATCH] =?UTF-8?q?PtrPathPlanner=20=E7=AE=97=E6=B3=95=E6=B5=8B?= =?UTF-8?q?=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 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: [ ]