From dd06e970ace7df66c7e230bc2375f40bcae1dcd9 Mon Sep 17 00:00:00 2001 From: luoyifan Date: Sat, 28 Jun 2025 22:28:54 +0800 Subject: [PATCH] =?UTF-8?q?=E9=87=8D=E6=9E=84=20PTR=20=E7=9A=84=E5=B0=8F?= =?UTF-8?q?=E8=BD=A6=E5=AE=9E=E4=BD=93=E3=80=81=E6=8A=A5=E6=96=87=E6=94=B6?= =?UTF-8?q?=E5=8F=91=E3=80=81=E5=8D=8F=E8=AE=AE=E3=80=81=E5=85=A8=E6=94=BE?= =?UTF-8?q?=E5=9C=A8=E4=B8=80=E8=B5=B7=EF=BC=9B=E5=A2=9E=E5=8A=A0=20agvLoa?= =?UTF-8?q?d/agvUnload/cancelTasks/waitTaskFinish=20=E6=96=B9=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../com/galaxis/rcs/common/enums/LCCDirection.java | 13 + .../rcs/connector/cl2/Cl2DeviceConnector.java | 8 - .../com/galaxis/rcs/connector/cl2/Cl2Item.java | 2 +- .../rcs/connector/cl2/VirtualCl2Connector.java | 4 - .../com/galaxis/rcs/plan/path/AStarNodeState.java | 29 - .../galaxis/rcs/plan/path/AStarPathPlanner.java | 280 ++++--- .../galaxis/rcs/plan/path/GraphInitializer.java | 95 --- .../com/galaxis/rcs/plan/path/NavigationGraph.java | 166 +++-- .../com/galaxis/rcs/plan/path/NavigationNode.java | 25 - .../main/java/com/galaxis/rcs/plan/path/Node.java | 20 + .../com/galaxis/rcs/plan/path/OperationPoint.java | 33 - .../com/galaxis/rcs/plan/path/PathSegment.java | 15 - .../java/com/galaxis/rcs/plan/path/PathUtils.java | 183 +++++ .../com/galaxis/rcs/plan/path/PtrPathPlanner.java | 236 ++++++ .../main/java/com/galaxis/rcs/plan/path/State.java | 19 + .../java/com/galaxis/rcs/plan/path/StoreLink.java | 17 + .../galaxis/rcs/plan/path2/AStarPathPlanner.java | 220 ------ .../galaxis/rcs/plan/path2/NavigationGraph.java | 147 ---- .../main/java/com/galaxis/rcs/plan/path2/Node.java | 20 - .../java/com/galaxis/rcs/plan/path2/PathUtils.java | 183 ----- .../com/galaxis/rcs/plan/path2/PtrPathPlanner.java | 143 ---- .../java/com/galaxis/rcs/plan/path2/State.java | 19 - .../java/com/galaxis/rcs/plan/path2/StoreLink.java | 17 - .../java/com/galaxis/rcs/plan/task/LoadTask.java | 15 + .../java/com/galaxis/rcs/plan/task/MoveTask.java | 3 + .../java/com/galaxis/rcs/plan/task/UnloadTask.java | 15 + .../java/com/galaxis/rcs/ptr/AgvEventManager.java | 1 - .../com/galaxis/rcs/ptr/AmrMessageHandler.java | 1 - .../java/com/galaxis/rcs/ptr/PtrAgvConnector.java | 5 + .../com/galaxis/rcs/ptr/PtrAgvConnectorThread.java | 167 +++++ .../java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java | 62 ++ .../main/java/com/galaxis/rcs/ptr/PtrAgvItem.java | 830 +++++++++++++++++++++ .../java/com/yvan/logisticsModel/BaseItem.java | 22 +- .../com/yvan/logisticsModel/LogisticsRuntime.java | 4 +- .../com/yvan/logisticsModel/PtrAgvConnector.java | 7 - .../yvan/logisticsModel/PtrAgvConnectorThread.java | 167 ----- .../com/yvan/logisticsModel/PtrAgvDeviceTask.java | 60 -- .../java/com/yvan/logisticsModel/PtrAgvItem.java | 829 -------------------- .../yvan/workbench/controller/RcsController.java | 327 ++++---- .../com/yvan/workbench/model/entity/Model.java | 13 + 40 files changed, 2108 insertions(+), 2314 deletions(-) delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/AStarNodeState.java delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/GraphInitializer.java delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/NavigationNode.java create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/Node.java delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/OperationPoint.java delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/PathSegment.java create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/State.java create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/StoreLink.java delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/Node.java delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/State.java delete mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/task/LoadTask.java create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/task/UnloadTask.java create mode 100644 servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnector.java create mode 100644 servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnectorThread.java create mode 100644 servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java create mode 100644 servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java delete mode 100644 servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnector.java delete mode 100644 servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java delete mode 100644 servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java delete mode 100644 servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.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 7b073ef..4cc23ea 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 @@ -35,4 +35,17 @@ public enum LCCDirection { throw new IllegalArgumentException("No constant with name: " + value); } + + public static LCCDirection fromString(String value, LCCDirection defaultValue) { + if (value == null) + throw new IllegalArgumentException("Value cannot be null"); + + for (LCCDirection type : LCCDirection.values()) { + if (type.toString().equalsIgnoreCase(value.trim())) { + return type; + } + } + + return defaultValue; + } } diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java index 6a29965..f6fb011 100644 --- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java +++ b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java @@ -1,20 +1,12 @@ package com.galaxis.rcs.connector.cl2; import com.fasterxml.jackson.core.JsonProcessingException; -import com.galaxis.rcs.plan.PlanTaskSequence; -import com.galaxis.rcs.ptr.sendEntity.RcsConfigMessage; import com.galaxis.rcs.ptr.sendEntity.RcsTaskMessage; import com.galaxis.rcs.ptr.AmrMessageHandler; -import com.google.common.base.Splitter; import com.yvan.logisticsModel.LogisticsRuntime; -import com.yvan.logisticsModel.PtrAgvItem; import lombok.extern.slf4j.Slf4j; -import org.clever.core.BannerUtils; -import org.clever.core.json.JsonWrapper; import org.eclipse.paho.mqttv5.common.MqttException; -import java.util.Map; - /** * CL2 车型报文推送 */ diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java index e490862..3d2d893 100644 --- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java +++ b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Item.java @@ -2,7 +2,7 @@ package com.galaxis.rcs.connector.cl2; import com.galaxis.rcs.ptr.sendEntity.RcsConfigMessage; import com.yvan.logisticsModel.LogisticsRuntime; -import com.yvan.logisticsModel.PtrAgvItem; +import com.galaxis.rcs.ptr.PtrAgvItem; import java.util.Map; diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java b/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java index 28ea82c..a42cdd1 100644 --- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java +++ b/servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java @@ -1,9 +1,5 @@ package com.galaxis.rcs.connector.cl2; -import com.galaxis.rcs.common.entity.RcsTaskPlan; -import com.yvan.logisticsModel.ExecutorItem; -import com.yvan.logisticsModel.LogisticsRuntime; -import com.yvan.logisticsModel.PtrAgvItem; import lombok.extern.slf4j.Slf4j; @Slf4j diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/AStarNodeState.java b/servo/src/main/java/com/galaxis/rcs/plan/path/AStarNodeState.java deleted file mode 100644 index e7f5f10..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path/AStarNodeState.java +++ /dev/null @@ -1,29 +0,0 @@ -package com.galaxis.rcs.plan.path; - -/** - * A*路径节点状态 - */ -public record AStarNodeState( - String nodeId, // 当前节点ID - int direction, // 当前方向 (0,90,180,270) - float gCost, // 实际代价 - float hCost, // 启发式代价 - AStarNodeState parent // 父节点 -) implements Comparable { - - // 状态唯一标识 - public String stateKey() { - return nodeId + ":" + direction; - } - - // 总代价 - public float fCost() { - return gCost + hCost; - } - - @Override - public int compareTo(AStarNodeState other) { - return Float.compare(this.fCost(), other.fCost()); - } -} - diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java index 0c1a830..8347234 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java @@ -1,122 +1,220 @@ package com.galaxis.rcs.plan.path; +import com.galaxis.rcs.common.enums.LCCDirection; +import com.google.common.collect.Maps; + import java.util.*; -/** - * A*路径规划器 - */ +import static com.galaxis.rcs.plan.path.PathUtils.*; + public class AStarPathPlanner { + private static final float ROTATION_COST_PER_DEGREE = 0.01f; + private static final float BLOCKED_COST = 10000f; // 阻塞系数成本 + private static final float WEIGHT_FACTOR = 1.5f; // 权重因子 + private final NavigationGraph graph; + private final Map nodeWeights = Maps.newConcurrentMap(); + private final Map blockedNodes = Maps.newConcurrentMap(); public AStarPathPlanner(NavigationGraph graph) { this.graph = graph; } - /** - * 规划路径 - * - * @param startNodeId 起始节点ID - * @param startDirection 起始方向 - * @param endNodeId 目标节点ID - * @param endDirection 目标方向 - * @return 路径节点序列 (包含方向信息) - */ - public List planPath(String startNodeId, int startDirection, String endNodeId, int endDirection) { - // 开放集 (优先队列) - PriorityQueue openSet = new PriorityQueue<>(); - - // 状态管理 - Map gScoreMap = new HashMap<>(); - Map cameFrom = new HashMap<>(); - - // 初始化起点 - AStarNodeState start = new AStarNodeState( - startNodeId, - startDirection, - 0, - graph.heuristicCost(startNodeId, endNodeId), - null - ); - - openSet.add(start); - gScoreMap.put(start.stateKey(), 0.0f); - - while (!openSet.isEmpty()) { - AStarNodeState current = openSet.poll(); - - // 到达目标状态 - if (current.nodeId().equals(endNodeId) && - current.direction() == endDirection) { - return reconstructPath(cameFrom, current); + // 路径规划状态 + public List findPath(String startId, LCCDirection startDirection, String endId, LCCDirection endDirection) { + Node start = graph.getNode(startId); + Node end = graph.getNode(endId); + if (start == null) { + throw new RuntimeException("Start node not found: " + startId); + } + if (end == null) { + throw new RuntimeException("End node not found: " + endId); + } + + // 使用复合键避免状态重复 + Map visited = new HashMap<>(); + PriorityQueue open = new PriorityQueue<>(); + + // 初始状态 + State initialState = new State(start, startDirection, 0, heuristic(start, end), null); + open.add(initialState); + visited.put(stateKey(start.id(), startDirection), initialState); + + while (!open.isEmpty()) { + State current = open.poll(); + + // 到达目标节点且方向匹配 + if (current.node().id().equals(endId) && current.direction() == endDirection) { + return buildPath(current); } - // 处理移动操作 (到相邻节点) - for (NavigationNode neighbor : graph.getAdjacentNodes(current.nodeId())) { - PathSegment segment = graph.getPathSegment(current.nodeId(), neighbor.id()); - if (segment == null) continue; - - float moveCost = segment.distance(); - float tentativeGCost = current.gCost() + moveCost; - String neighborKey = neighbor.id() + ":" + current.direction(); - - // 发现更好路径 - if (tentativeGCost < gScoreMap.getOrDefault(neighborKey, Float.MAX_VALUE)) { - AStarNodeState neighborState = new AStarNodeState( - neighbor.id(), - current.direction(), - tentativeGCost, - graph.heuristicCost(neighbor.id(), endNodeId), + // 处理邻边移动 + for (Node neighbor : graph.getNeighbors(current.node())) { + // 检查节点是否被阻塞 + if (isBlocked(neighbor.id())) continue; + + // 计算移动方向 + LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor); + + // 尝试前进 + if (canMoveForward(current.direction(), moveDirection)) { + float moveCost = calculateMoveCost(current.node(), neighbor, false); + considerState(current, neighbor, current.direction(), + moveCost, open, visited, end); + } + // 尝试后退 + else if (canMoveBackward(current.direction(), moveDirection)) { + float moveCost = calculateMoveCost(current.node(), neighbor, true); + considerState(current, neighbor, current.direction(), + moveCost, open, visited, end); + } + // 需要旋转 + else if (current.node().rotatable()) { + // 计算需要旋转到的方向 + LCCDirection requiredDirection = calculateRequiredDirection(moveDirection); + + // 考虑旋转后移动 + float rotationCost = calculateRotationCost( + current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE + ); + float moveCost = calculateMoveCost(current.node(), neighbor, false); + float totalCost = rotationCost + moveCost; + + // 创建旋转状态 + State rotatedState = new State( + current.node(), requiredDirection, + current.g() + rotationCost, + heuristic(current.node(), end), current ); - cameFrom.put(neighborKey, current); - gScoreMap.put(neighborKey, tentativeGCost); - openSet.add(neighborState); + // 考虑旋转后的移动 + considerState(rotatedState, neighbor, requiredDirection, + moveCost, open, visited, end); } } - // 处理旋转操作 (当前节点可旋转时) - NavigationNode currentNode = graph.nodes.get(current.nodeId()); - if (currentNode != null && currentNode.rotatable()) { - for (int rotation : new int[]{90, -90}) { - int newDirection = (current.direction() + rotation + 360) % 360; - float rotateCost = 1.0f; // 旋转代价 - float tentativeGCost = current.gCost() + rotateCost; - String rotatedKey = current.nodeId() + ":" + newDirection; - - // 发现更好路径 - if (tentativeGCost < gScoreMap.getOrDefault(rotatedKey, Float.MAX_VALUE)) { - AStarNodeState rotatedState = new AStarNodeState( - current.nodeId(), - newDirection, - tentativeGCost, - current.hCost(), // 旋转不改变位置，启发值不变 - current - ); + // 处理原地旋转 - 只考虑目标方向 + if (current.node().rotatable()) { + // 只考虑旋转到目标方向（如果可能） + if (current.direction() != endDirection) { + float rotationCost = calculateRotationCost( + current.direction(), endDirection, ROTATION_COST_PER_DEGREE + ); + considerState(current, current.node(), endDirection, + rotationCost, open, visited, end); + } + + // 另外考虑旋转到移动所需的方向 + for (Node neighbor : graph.getNeighbors(current.node())) { + LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor); + LCCDirection requiredDirection = calculateRequiredDirection(moveDirection); - cameFrom.put(rotatedKey, current); - gScoreMap.put(rotatedKey, tentativeGCost); - openSet.add(rotatedState); + 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(); // 未找到路径 + return Collections.emptyList(); } - private List reconstructPath( - Map cameFrom, - AStarNodeState endState - ) { - LinkedList path = new LinkedList<>(); - AStarNodeState current = endState; - - while (current != null) { - path.addFirst(current); - current = cameFrom.get(current.stateKey()); + /** + * 考虑新的状态并更新开放列表和访问记录 + * + * @param current 当前状态 + * @param nextNode 下一个节点 + * @param nextDirection 下一个方向 + * @param cost 移动成本 + * @param open 开放列表 + * @param visited 访问记录 + * @param end 目标节点 + */ + private void considerState(State current, Node nextNode, LCCDirection nextDirection, + float cost, PriorityQueue open, + Map visited, Node end) { + String key = stateKey(nextNode.id(), nextDirection); + float newG = current.g() + cost; + + if (!visited.containsKey(key) || visited.get(key).g() > newG) { + float h = heuristic(nextNode, end); + State newState = new State(nextNode, nextDirection, newG, h, current); + open.add(newState); + visited.put(key, newState); } + } + private List buildPath(State state) { + LinkedList path = new LinkedList<>(); + while (state != null) { + path.addFirst(state); + state = state.parent(); + } return path; } + + /** + * 启发式函数，计算两个节点之间的距离 + */ + private float heuristic(Node a, Node b) { + return graph.distance(a, b); + // 使用曼哈顿距离？？ + // return Math.abs(a.x() - b.x()) + Math.abs(a.z() - b.z()); + } + + /** + * 生成状态的唯一键 + */ + private String stateKey(String nodeId, LCCDirection direction) { + return nodeId + "|" + direction; + } + + public void setNodeWeight(String nodeId, float weight) { + nodeWeights.put(nodeId, weight); + } + + public void setBlocked(String nodeId, float blockedFactor) { + blockedNodes.put(nodeId, blockedFactor); + } + + private float calculateMoveCost(Node from, Node to, boolean isBackward) { + float baseCost = graph.distance(from, to); + float weight = nodeWeights.getOrDefault(to.id(), 1.0f); + float blockedFactor = blockedNodes.getOrDefault(to.id(), 0f); + + // 后退移动增加额外成本?? + // return baseCost * weight * (1 + blockedFactor) * (isBackward ? 1.2f : 1.0f); + return baseCost * weight * (1 + blockedFactor); + } + + private boolean isBlocked(String nodeId) { + return blockedNodes.containsKey(nodeId) && blockedNodes.get(nodeId) > 0.8f; + } + + private boolean canMoveForward(LCCDirection currentDir, LCCDirection moveDir) { + return currentDir == moveDir; + } + + private boolean canMoveBackward(LCCDirection currentDir, LCCDirection moveDir) { + return currentDir == getOppositeDirection(moveDir); + } + + private LCCDirection calculateRequiredDirection(LCCDirection moveDirection) { + // 侧插式AGV只能前进或后退，不能横移 + return moveDirection; + } } diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/GraphInitializer.java b/servo/src/main/java/com/galaxis/rcs/plan/path/GraphInitializer.java deleted file mode 100644 index e3dd8cf..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path/GraphInitializer.java +++ /dev/null @@ -1,95 +0,0 @@ -package com.galaxis.rcs.plan.path; - -import com.galaxis.rcs.common.enums.OperationSide; -import com.yvan.logisticsModel.LogisticsRuntime; -import org.clever.core.Conv; - -import java.util.List; -import java.util.Map; - -public class GraphInitializer { - -// 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/path/NavigationGraph.java b/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationGraph.java index 08a4db7..595f381 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationGraph.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationGraph.java @@ -1,69 +1,147 @@ package com.galaxis.rcs.plan.path; +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.*; -import java.util.concurrent.ConcurrentHashMap; /** - * 导航图管理器 + * A* 导航图 */ +@Slf4j public class NavigationGraph { - final Map nodes = new ConcurrentHashMap<>(); - final Map operationPoints = new ConcurrentHashMap<>(); - final Map pathSegments = new ConcurrentHashMap<>(); - final Map> adjacencyList = new ConcurrentHashMap<>(); - - // 添加节点 - public void addNode(NavigationNode node) { - nodes.put(node.id(), node); - adjacencyList.put(node.id(), new ArrayList<>()); + private final LogisticsRuntime runtime; + + /** + * 缓存距离计算结果，避免重复计算 + */ + private final Map distanceCache = Maps.newConcurrentMap(); + + /** + * 缓存邻居节点列表，避免重复查询 + */ + private final Map> neighborCache = Maps.newConcurrentMap(); + + /** + * 添加路径缓存 + */ + private final Map> pathCache = Maps.newConcurrentMap(); + + private final Map nodeMap = new HashMap<>(); + private final Map> storeToNodes = new HashMap<>(); + + public NavigationGraph(LogisticsRuntime runtime) { + this.runtime = runtime; } - // 添加双向路径 - public void addBidirectionalPath(NavigationNode a, NavigationNode b) { - float distance = a.distanceTo(b); - pathSegments.put(a.id() + "->" + b.id(), new PathSegment(a, b, distance)); - pathSegments.put(b.id() + "->" + a.id(), new PathSegment(b, a, distance)); + public List getCachedPath(String startId, LCCDirection startDirection, + String endId, LCCDirection endDirection) { + String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection; + return pathCache.get(cacheKey); + } - adjacencyList.get(a.id()).add(b); - adjacencyList.get(b.id()).add(a); + public void cachePath(String startId, LCCDirection startDirection, + String endId, LCCDirection endDirection, + List path) { + String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection; + pathCache.put(cacheKey, path); } - // 添加操作点 - public void addOperationPoint(OperationPoint point) { - operationPoints.put(point.locationKey(), point); + public Node getNodeById(String nodeId){ + return nodeMap.get(nodeId); } - // 查找操作点 - public OperationPoint findOperationPoint(String locationKey) { - return operationPoints.get(locationKey); + 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 PathSegment getPathSegment(String startId, String endId) { - return pathSegments.get(startId + "->" + endId); + public List getNeighbors(Node node) { + return node.neighbors().stream() + .map(nodeMap::get) + .filter(Objects::nonNull) + .toList(); } - // 获取相邻节点 - public List getAdjacentNodes(String nodeId) { - return adjacencyList.getOrDefault(nodeId, Collections.emptyList()); + 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 NavigationNode findNearestRotationPoint(String startId) { - NavigationNode start = nodes.get(startId); - if (start == null) return null; - return nodes.values().parallelStream() - .filter(NavigationNode::rotatable) - .min(Comparator.comparing(start::distanceTo)) - .orElse(null); + public void init() { + var items = runtime.getStaticItems(); + for (StaticItem item : items) { + if ("way".equals(item.getT())) { + float[][] tf = item.getTf(); + String id = item.getId(); + Map dt = item.getDt(); + + // 提取坐标 + float x = tf[0][0]; + float z = tf[0][2]; // Z向下增长 + + // 检查可旋转性 + boolean rotatable = dt.containsKey("agvRotation"); + if (rotatable) { + log.info("Node {} is rotatable", id); + } + + // 提取邻居节点 + List in = (List) dt.get("in"); + List out = (List) dt.get("out"); + Set neighbors = new HashSet<>(); + if (in != null) neighbors.addAll(in); + if (out != null) neighbors.addAll(out); + + // 提取货位链接 + List storeLinks = new ArrayList<>(); + List> links = (List>) dt.get("linkStore"); + if (links != null) { + for (Map link : links) { + String storeId = (String) link.get("item"); + int bay = (Integer) link.get("bay"); + int level = (Integer) link.get("level"); + int cell = (Integer) link.get("cell"); + String direction = (String) link.get("direction"); + storeLinks.add(new StoreLink(storeId, bay, level, cell, LCCDirection.fromString(direction))); + + // 建立货位到节点的反向映射 + storeToNodes.computeIfAbsent(storeId, k -> new ArrayList<>()).add(id); + } + } + + nodeMap.put(id, new Node(id, x, z, rotatable, + new ArrayList<>(neighbors), storeLinks)); + } + } + + // 2. 验证邻居双向连接 + for (Node node : nodeMap.values()) { + Iterator it = node.neighbors().iterator(); + while (it.hasNext()) { + String neighborId = it.next(); + if (!nodeMap.containsKey(neighborId)) { + it.remove(); // 移除无效邻居 + } + } + } } - // 计算启发式代价 (使用欧几里得距离) - public float heuristicCost(String fromId, String toId) { - NavigationNode from = nodes.get(fromId); - NavigationNode to = nodes.get(toId); - if (from == null || to == null) return Float.MAX_VALUE; - return from.distanceTo(to); + public Node getNode(String id) { + return nodeMap.get(id); } } diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationNode.java b/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationNode.java deleted file mode 100644 index 2cac8a6..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path/NavigationNode.java +++ /dev/null @@ -1,25 +0,0 @@ -package com.galaxis.rcs.plan.path; - -import org.clever.core.Conv; - -/** - * 路标节点（地图中的顶点） - */ -public record NavigationNode( - String id, // 节点ID (如 "20", "charger1") - float x, // 向右增长 - float z, // 向下增长 (y无效) - boolean rotatable // 是否为可旋转位 -) implements Comparable { - // 计算到另一个节点的欧几里得距离 - public float distanceTo(NavigationNode other) { - float dx = this.x - other.x; - float dz = this.z - other.z; - return Conv.asFloat(Math.sqrt(dx * dx + dz * dz)); - } - - @Override - public int compareTo(NavigationNode other) { - return this.id.compareTo(other.id); - } -} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/Node.java b/servo/src/main/java/com/galaxis/rcs/plan/path/Node.java new file mode 100644 index 0000000..6b865bf --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/Node.java @@ -0,0 +1,20 @@ +package com.galaxis.rcs.plan.path; + +import java.util.List; + +/** + * 路径节点定义 + * + * @param id 节点ID + * @param x 节点X坐标 + * @param z 节点Z坐标 + * @param rotatable 节点是否可旋转 + * @param neighbors 邻居节点ID列表 + */ +public record Node(String id, + float x, + float z, + boolean rotatable, + List neighbors, + List storeLinks) { +} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/OperationPoint.java b/servo/src/main/java/com/galaxis/rcs/plan/path/OperationPoint.java deleted file mode 100644 index 3d5926d..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path/OperationPoint.java +++ /dev/null @@ -1,33 +0,0 @@ -package com.galaxis.rcs.plan.path; - -import com.galaxis.rcs.common.enums.OperationSide; - -/** - * 操作位置（取货/放货/充电点） - */ -public record OperationPoint( - NavigationNode node, // 关联的路标节点 - String targetId, // 货位ID (如 "54") 或货架ID (如 "rack1") - OperationSide side, // 方位 - Integer bay, // 列 (货架时非空) - Integer level, // 层 (货架时非空) - Integer cell // 格 (货架时非空) -) { - // 计算操作所需方向 - public int requiredDirection() { - return switch (side) { - case LEFT -> 90; - case RIGHT -> 270; - case TOP -> 0; - case BOTTOM -> 180; - }; - } - - // 位置唯一标识 - public String locationKey() { - if (bay == null || level == null || cell == null) { - return targetId; // 地堆货位 - } - return targetId + "-" + bay + "-" + level + "-" + cell; - } -} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/PathSegment.java b/servo/src/main/java/com/galaxis/rcs/plan/path/PathSegment.java deleted file mode 100644 index 68c7b0e..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path/PathSegment.java +++ /dev/null @@ -1,15 +0,0 @@ -package com.galaxis.rcs.plan.path; - -/** - * 路径段（节点之间的连接） - */ -public record PathSegment( - NavigationNode start, // 起点节点 - NavigationNode end, // 终点节点 - float distance // 路径距离 -) { - // 路径唯一标识 - public String key() { - return start.id() + "->" + end.id(); - } -} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java b/servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java new file mode 100644 index 0000000..439cd4a --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java @@ -0,0 +1,183 @@ +package com.galaxis.rcs.plan.path; + +import com.galaxis.rcs.common.enums.LCCDirection; + +import java.util.*; + +public class PathUtils { + + /** + * 从 LCCDirection 转换为 AMR 的方向单位 + * 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 + */ + public static short convertDirectionToPtrDiretion(LCCDirection value) { + return switch (value) { + case UP -> 3; + case DOWN -> 1; + case LEFT -> 2; + case RIGHT -> 0; + default -> throw new IllegalArgumentException("Unknown direction: " + value); + }; + } + + /** + * 计算移动方向 + */ + public static LCCDirection calculateMoveDirection(Node from, Node to) { + float dx = to.x() - from.x(); + float dz = to.z() - from.z(); // 注意：Z轴向下增长 + + // 考虑左手坐标系：X向右，Z向下 + if (Math.abs(dx) > Math.abs(dz)) { + return dx > 0.5 ? LCCDirection.RIGHT : LCCDirection.LEFT; + } else { + // 注意：Z向下增长，所以Z值增加表示向下移动 + return dz > 0.5 ? LCCDirection.DOWN : LCCDirection.UP; + } + } + + /** + * 检查方向是否有效（侧插式专用） + */ + public static boolean isValidForSideLoader(Node node, int bay, LCCDirection direction) { + Optional link = node.storeLinks().stream() + .filter(l -> l.bay() == bay) + .findFirst(); + + if (link.isEmpty()) return false; + + LCCDirection requiredDirection = convertForSideLoader(link.get().direction()); + return direction == requiredDirection; + } + + /** + * 转换货位方向到AGV所需方向（侧插式专用） + */ + public static LCCDirection convertForSideLoader(LCCDirection storeDirection) { + /* + * 侧插式AGV方向规则： + * - 货位在上方 → 车头向右（货叉朝左） + * - 货位在下方 → 车头向左（货叉朝左） + * - 货位在左方 → 车头向上（货叉朝左） + * - 货位在右方 → 车头向下（货叉朝左） + */ + return switch (storeDirection) { + case UP -> LCCDirection.RIGHT; + case DOWN -> LCCDirection.LEFT; + case LEFT -> LCCDirection.UP; + case RIGHT -> LCCDirection.DOWN; + }; + } + + /** + * 获取最近的旋转点 + */ + public static Node findNearestRotationNode(NavigationGraph graph, Node from, LCCDirection currentDir, + LCCDirection requiredDir) { + // 如果当前方向已经匹配，不需要旋转 + if (currentDir == requiredDir) return from; + + // 使用Dijkstra算法查找最近的旋转点 + Map distances = new HashMap<>(); + Map predecessors = new HashMap<>(); + PriorityQueue queue = new PriorityQueue<>(Comparator.comparingDouble(distances::get)); + + distances.put(from, 0f); + queue.add(from); + + Node result = null; + + while (!queue.isEmpty()) { + Node current = queue.poll(); + + // 找到可旋转点 + if (current.rotatable()) { + result = current; + break; + } + + // 探索邻居 + for (Node neighbor : graph.getNeighbors(current)) { + float newDist = distances.get(current) + graph.distance(current, neighbor); + + if (newDist < distances.getOrDefault(neighbor, Float.MAX_VALUE)) { + distances.put(neighbor, newDist); + predecessors.put(neighbor, current); + queue.add(neighbor); + } + } + } + + return result; + } + + /** + * 获取相反方向 + */ + public static LCCDirection getOppositeDirection(LCCDirection dir) { + return switch (dir) { + case UP -> LCCDirection.DOWN; + case DOWN -> LCCDirection.UP; + case LEFT -> LCCDirection.RIGHT; + case RIGHT -> LCCDirection.LEFT; + }; + } + + /** + * 计算旋转代价 + */ + public static float calculateRotationCost(LCCDirection from, LCCDirection to, float ROTATION_COST_PER_DEGREE) { + float angle1 = getRequiredDirection(from); + float angle2 = getRequiredDirection(to); + float diff = Math.abs(angle1 - angle2); + diff = Math.min(diff, 360 - diff); + // return diff * ROTATION_COST_PER_DEGREE; + return 0.1f; + } + + /** + * 获取所需方向的角度 + */ + public static float getRequiredDirection(LCCDirection direction) { + return switch (direction) { + case UP -> 90f; + case DOWN -> 270f; + case LEFT -> 180f; + case RIGHT -> 0f; + }; + } + + /** + * 转换货位方向到AGV所需方向 + */ + public static LCCDirection convertStoreDirection(LCCDirection storeDirection) { + // 转换规则: 货位在路径点的方位 -> AGV所需方向 + return switch (storeDirection) { + case UP -> LCCDirection.RIGHT; // 货位在上方 → 车头向右 + case DOWN -> LCCDirection.LEFT; // 货位在下方 → 车头向左 + case LEFT -> LCCDirection.UP; // 货位在左方 → 车头向上 + case RIGHT -> LCCDirection.DOWN; // 货位在右方 → 车头向下 + }; + } + + /** + * 将角度转换为 LCCDirection + */ + public static LCCDirection convertAngleToDirection(float angle) { + // 标准化角度 + angle = ((angle % 360f) + 360f) % 360f; + + if (angle >= 315f || angle < 45f) return LCCDirection.RIGHT; + if (angle >= 45f && angle < 135f) return LCCDirection.UP; + if (angle >= 135f && angle < 225f) return LCCDirection.LEFT; + return LCCDirection.DOWN; + } + + /** + * 计算方向角度差 + */ + public static float angleDifference(float angle1, float angle2) { + float diff = Math.abs(angle1 - angle2) % 360; + return diff > 180 ? 360 - diff : diff; + } +} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java new file mode 100644 index 0000000..665d6f0 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java @@ -0,0 +1,236 @@ +package com.galaxis.rcs.plan.path; + +import com.galaxis.rcs.common.enums.LCCDirection; +import com.galaxis.rcs.plan.PlanTaskSequence; +import com.galaxis.rcs.plan.task.CarryTask; +import com.galaxis.rcs.plan.task.LoadTask; +import com.galaxis.rcs.plan.task.MoveTask; +import com.galaxis.rcs.plan.task.UnloadTask; + +import java.util.List; + +import static com.galaxis.rcs.plan.path.PathUtils.convertStoreDirection; + +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 planMoveTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, MoveTask moveTask) { + + Node node = this.graph.getNodeById(moveTask.targetWayPointId()); + + List toPath = astar.findPath(startNodeId, startDirection, node.id(), moveTask.targetDirection()); + generateMoves(plan, toPath); + plan.addFinish(); + } + + public void planCarryTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, CarryTask task) { + // 取货点 + String loadRackId = task.from().rackId(); + int pickupBay = task.from().bay(); + NodeDirection loadNodeDirection = findNodeForStore(loadRackId, pickupBay); + if (loadNodeDirection == null) { + throw new RuntimeException("Pickup node not found for rackId=" + loadRackId + ", bay=" + pickupBay); + } + + // 放货点 + String unloadRackId = task.to().rackId(); + int unloadBay = task.to().bay(); + NodeDirection unloadNodeDirection = findNodeForStore(unloadRackId, unloadBay); + if (unloadNodeDirection == null) { + throw new RuntimeException("Drop node not found for rackId=" + unloadRackId + ", bay=" + unloadBay); + } + + // 规划到取货点路径 + List toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction()); + + // 检查方向是否匹配，如果不匹配则插入旋转点 + if (!toLoadPath.isEmpty()) { + State lastState = toLoadPath.get(toLoadPath.size() - 1); + if (lastState.direction() != loadNodeDirection.direction()) { + Node rotationNode = PathUtils.findNearestRotationNode( + graph, lastState.node(), lastState.direction(), loadNodeDirection.direction() + ); + + if (rotationNode != null) { + // 插入旋转路径 + List toRotation = astar.findPath( + lastState.node().id(), lastState.direction(), + rotationNode.id(), loadNodeDirection.direction() + ); + toLoadPath.addAll(toRotation); + + // 从旋转点到目标点 + List fromRotation = astar.findPath( + rotationNode.id(), loadNodeDirection.direction(), + loadNodeDirection.node().id(), loadNodeDirection.direction() + ); + toLoadPath.addAll(fromRotation); + } + } + } + + // 规划到放货点路径 + List toUnloadPath = astar.findPath(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirection.node().id(), unloadNodeDirection.direction()); + + // 生成指令序列 + generateMoves(plan, toLoadPath); + plan.addLoad(task.lpn(), loadRackId, pickupBay, task.from().level(), task.from().cell()); + + generateMoves(plan, toUnloadPath); + plan.addUnload(unloadRackId, task.to().level(), task.to().bay(), task.to().cell()); + + plan.addFinish(); + } + + + public void planUnloadTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, UnloadTask task) { + Node fromNode = this.graph.getNodeById(startNodeId); + + // 放货点 + 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 toUnloadPath = astar.findPath(startNodeId, startDirection, unloadNodeDirection.node().id(), unloadNodeDirection.direction()); + + // 检查方向是否匹配，如果不匹配则插入旋转点 + // State lastState = toUnloadPath.get(toUnloadPath.size() - 1); + // if (startDirection != lastState.direction()) { + // Node rotationNode = PathUtils.findNearestRotationNode( + // graph, fromNode, startDirection, lastState.direction() + // ); + // + // if (rotationNode != null) { + // // 插入旋转路径 + // List toRotation = astar.findPath( + // startNodeId, startDirection, + // rotationNode.id(), unloadNodeDirection.direction() + // ); + // toUnloadPath.addAll(toRotation); + // + // // 从旋转点到目标点 + // List fromRotation = astar.findPath( + // rotationNode.id(), unloadNodeDirection.direction(), + // unloadNodeDirection.node().id(), unloadNodeDirection.direction() + // ); + // toUnloadPath.addAll(fromRotation); + // } + // } + + // 生成指令序列 + generateMoves(plan, toUnloadPath); + plan.addUnload(unloadRackId, task.to().level(), task.to().bay(), task.to().cell()); + + plan.addFinish(); + } + + + public void planLoadTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, LoadTask task) { + Node fromNode = this.graph.getNodeById(startNodeId); + + // 放货点 + String loadRackId = task.to().rackId(); + int unloadBay = task.to().bay(); + NodeDirection loadNodeDirection = findNodeForStore(loadRackId, unloadBay); + if (loadNodeDirection == null) { + throw new RuntimeException("Drop node not found for rackId=" + loadRackId + ", bay=" + unloadBay); + } + + // 规划到取货点路径 + List toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction()); + + // 检查方向是否匹配，如果不匹配则插入旋转点 + // State lastState = toLoadPath.get(toLoadPath.size() - 1); + // if (startDirection != lastState.direction()) { + // Node rotationNode = PathUtils.findNearestRotationNode( + // graph, fromNode, startDirection, lastState.direction() + // ); + // + // if (rotationNode != null) { + // // 插入旋转路径 + // List toRotation = astar.findPath( + // startNodeId, startDirection, + // rotationNode.id(), loadNodeDirection.direction() + // ); + // toLoadPath.addAll(toRotation); + // + // // 从旋转点到目标点 + // List fromRotation = astar.findPath( + // rotationNode.id(), loadNodeDirection.direction(), + // loadNodeDirection.node().id(), loadNodeDirection.direction() + // ); + // toLoadPath.addAll(fromRotation); + // } + // } + + // 生成指令序列 + generateMoves(plan, toLoadPath); + plan.addLoad("N/A", loadRackId, task.to().level(), task.to().bay(), task.to().cell()); + + plan.addFinish(); + } + + private NodeDirection findNodeForStore(String storeId, int bay) { + List nodes = this.graph.getNodesForStore(storeId); + for (Node node : nodes) { + for (StoreLink link : node.storeLinks()) { + if (link.storeId().equals(storeId) && link.bay() == bay) { + LCCDirection agvDirection = convertStoreDirection(link.direction()); + return new NodeDirection(node, agvDirection); + } + } + } + return null; + } + + /** + * 根据A*状态，生成移动指令序列 + */ + private void generateMoves(PlanTaskSequence sequence, List path) { + if (path.isEmpty()) return; + + // 第一个状态是起点，跳过 + State prevState = path.get(0); + + for (int i = 1; i < path.size(); i++) { + State current = path.get(i); + + // 如果是旋转动作 + if (current.node().equals(prevState.node())) { + float angle = PathUtils.getRequiredDirection(current.direction()); + sequence.addRotationTo(angle); + } + // 移动动作 - 检查是否需要后退 + else { + // 检查移动方向 + LCCDirection moveDir = PathUtils.calculateMoveDirection(prevState.node(), current.node()); + + // 如果移动方向与车头方向相反，需要后退 + boolean isBackward = (current.direction() == PathUtils.getOppositeDirection(moveDir)); + + if (isBackward) { + sequence.addMoveBackward(current.node().id()); + } else { + sequence.addMoveTo(current.node().id()); + } + } + prevState = current; + } + } + + + // 辅助记录类 + private record NodeDirection(Node node, LCCDirection direction) { + } +} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/State.java b/servo/src/main/java/com/galaxis/rcs/plan/path/State.java new file mode 100644 index 0000000..bc9a087 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/State.java @@ -0,0 +1,19 @@ +package com.galaxis.rcs.plan.path; + +import com.galaxis.rcs.common.enums.LCCDirection; + +/** + * A* 路径规划状态 + */ +public record State(Node node, + LCCDirection direction, + float g, + float h, + State parent) + implements Comparable { + + @Override + public int compareTo(State other) { + return Float.compare(g + h, other.g + other.h); + } +} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/StoreLink.java b/servo/src/main/java/com/galaxis/rcs/plan/path/StoreLink.java new file mode 100644 index 0000000..d94923e --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/StoreLink.java @@ -0,0 +1,17 @@ +package com.galaxis.rcs.plan.path; + +import com.galaxis.rcs.common.enums.LCCDirection; + +/** + * A* 路径与库位的连接定义 + * + * @param storeId 库位ID + * @param bay 列 + * @param level 层 + * @param cell 格 + * @param direction 方位（路径相对于库位的方位，比如路标的"左边"是库位） + */ +public record StoreLink( + String storeId, int bay, int level, int cell, LCCDirection direction +) { +} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java deleted file mode 100644 index 7b9a332..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java +++ /dev/null @@ -1,220 +0,0 @@ -package com.galaxis.rcs.plan.path2; - -import com.galaxis.rcs.common.enums.LCCDirection; -import com.google.common.collect.Maps; - -import java.util.*; - -import static com.galaxis.rcs.plan.path2.PathUtils.*; - -public class AStarPathPlanner { - private static final float ROTATION_COST_PER_DEGREE = 0.01f; - private static final float BLOCKED_COST = 10000f; // 阻塞系数成本 - private static final float WEIGHT_FACTOR = 1.5f; // 权重因子 - - private final NavigationGraph graph; - private final Map nodeWeights = Maps.newConcurrentMap(); - private final Map blockedNodes = Maps.newConcurrentMap(); - - public AStarPathPlanner(NavigationGraph graph) { - this.graph = graph; - } - - // 路径规划状态 - public List findPath(String startId, LCCDirection startDirection, String endId, LCCDirection endDirection) { - Node start = graph.getNode(startId); - Node end = graph.getNode(endId); - if (start == null) { - throw new RuntimeException("Start node not found: " + startId); - } - if (end == null) { - throw new RuntimeException("End node not found: " + endId); - } - - // 使用复合键避免状态重复 - Map visited = new HashMap<>(); - PriorityQueue open = new PriorityQueue<>(); - - // 初始状态 - State initialState = new State(start, startDirection, 0, heuristic(start, end), null); - open.add(initialState); - visited.put(stateKey(start.id(), startDirection), initialState); - - while (!open.isEmpty()) { - State current = open.poll(); - - // 到达目标节点且方向匹配 - if (current.node().id().equals(endId) && current.direction() == endDirection) { - return buildPath(current); - } - - // 处理邻边移动 - for (Node neighbor : graph.getNeighbors(current.node())) { - // 检查节点是否被阻塞 - if (isBlocked(neighbor.id())) continue; - - // 计算移动方向 - LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor); - - // 尝试前进 - if (canMoveForward(current.direction(), moveDirection)) { - float moveCost = calculateMoveCost(current.node(), neighbor, false); - considerState(current, neighbor, current.direction(), - moveCost, open, visited, end); - } - // 尝试后退 - else if (canMoveBackward(current.direction(), moveDirection)) { - float moveCost = calculateMoveCost(current.node(), neighbor, true); - considerState(current, neighbor, current.direction(), - moveCost, open, visited, end); - } - // 需要旋转 - else if (current.node().rotatable()) { - // 计算需要旋转到的方向 - LCCDirection requiredDirection = calculateRequiredDirection(moveDirection); - - // 考虑旋转后移动 - float rotationCost = calculateRotationCost( - current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE - ); - float moveCost = calculateMoveCost(current.node(), neighbor, false); - float totalCost = rotationCost + moveCost; - - // 创建旋转状态 - State rotatedState = new State( - current.node(), requiredDirection, - current.g() + rotationCost, - heuristic(current.node(), end), - current - ); - - // 考虑旋转后的移动 - considerState(rotatedState, neighbor, requiredDirection, - moveCost, open, visited, end); - } - } - - // 处理原地旋转 - 只考虑目标方向 - if (current.node().rotatable()) { - // 只考虑旋转到目标方向（如果可能） - if (current.direction() != endDirection) { - float rotationCost = calculateRotationCost( - current.direction(), endDirection, ROTATION_COST_PER_DEGREE - ); - considerState(current, current.node(), endDirection, - rotationCost, open, visited, end); - } - - // 另外考虑旋转到移动所需的方向 - for (Node neighbor : graph.getNeighbors(current.node())) { - LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor); - LCCDirection requiredDirection = calculateRequiredDirection(moveDirection); - - if (requiredDirection != current.direction()) { - float rotationCost = calculateRotationCost( - current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE - ); - considerState(current, current.node(), requiredDirection, - rotationCost, open, visited, end); - } - } - - // for (LCCDirection rotation : LCCDirection.values()) { - // if (rotation == current.direction()) continue; - // - // float rotationCost = calculateRotationCost( - // current.direction(), rotation, ROTATION_COST_PER_DEGREE - // ); - // considerState(current, current.node(), rotation, - // rotationCost, open, visited, end); - // } - } - } - return Collections.emptyList(); - } - - /** - * 考虑新的状态并更新开放列表和访问记录 - * - * @param current 当前状态 - * @param nextNode 下一个节点 - * @param nextDirection 下一个方向 - * @param cost 移动成本 - * @param open 开放列表 - * @param visited 访问记录 - * @param end 目标节点 - */ - private void considerState(State current, Node nextNode, LCCDirection nextDirection, - float cost, PriorityQueue open, - Map visited, Node end) { - String key = stateKey(nextNode.id(), nextDirection); - float newG = current.g() + cost; - - if (!visited.containsKey(key) || visited.get(key).g() > newG) { - float h = heuristic(nextNode, end); - State newState = new State(nextNode, nextDirection, newG, h, current); - open.add(newState); - visited.put(key, newState); - } - } - - private List buildPath(State state) { - LinkedList path = new LinkedList<>(); - while (state != null) { - path.addFirst(state); - state = state.parent(); - } - return path; - } - - /** - * 启发式函数，计算两个节点之间的距离 - */ - private float heuristic(Node a, Node b) { - return graph.distance(a, b); - // 使用曼哈顿距离？？ - // return Math.abs(a.x() - b.x()) + Math.abs(a.z() - b.z()); - } - - /** - * 生成状态的唯一键 - */ - private String stateKey(String nodeId, LCCDirection direction) { - return nodeId + "|" + direction; - } - - public void setNodeWeight(String nodeId, float weight) { - nodeWeights.put(nodeId, weight); - } - - public void setBlocked(String nodeId, float blockedFactor) { - blockedNodes.put(nodeId, blockedFactor); - } - - private float calculateMoveCost(Node from, Node to, boolean isBackward) { - float baseCost = graph.distance(from, to); - float weight = nodeWeights.getOrDefault(to.id(), 1.0f); - float blockedFactor = blockedNodes.getOrDefault(to.id(), 0f); - - // 后退移动增加额外成本?? - // return baseCost * weight * (1 + blockedFactor) * (isBackward ? 1.2f : 1.0f); - return baseCost * weight * (1 + blockedFactor); - } - - private boolean isBlocked(String nodeId) { - return blockedNodes.containsKey(nodeId) && blockedNodes.get(nodeId) > 0.8f; - } - - private boolean canMoveForward(LCCDirection currentDir, LCCDirection moveDir) { - return currentDir == moveDir; - } - - private boolean canMoveBackward(LCCDirection currentDir, LCCDirection moveDir) { - return currentDir == getOppositeDirection(moveDir); - } - - private LCCDirection calculateRequiredDirection(LCCDirection moveDirection) { - // 侧插式AGV只能前进或后退，不能横移 - return moveDirection; - } -} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java deleted file mode 100644 index bb1722d..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/NavigationGraph.java +++ /dev/null @@ -1,147 +0,0 @@ -package com.galaxis.rcs.plan.path2; - -import com.galaxis.rcs.common.enums.LCCDirection; -import com.google.common.collect.Maps; -import com.yvan.logisticsModel.LogisticsRuntime; -import com.yvan.logisticsModel.StaticItem; -import lombok.extern.slf4j.Slf4j; - -import java.util.*; - -/** - * A* 导航图 - */ -@Slf4j -public class NavigationGraph { - private final LogisticsRuntime runtime; - - /** - * 缓存距离计算结果，避免重复计算 - */ - private final Map distanceCache = Maps.newConcurrentMap(); - - /** - * 缓存邻居节点列表，避免重复查询 - */ - private final Map> neighborCache = Maps.newConcurrentMap(); - - /** - * 添加路径缓存 - */ - private final Map> pathCache = Maps.newConcurrentMap(); - - private final Map nodeMap = new HashMap<>(); - private final Map> storeToNodes = new HashMap<>(); - - public NavigationGraph(LogisticsRuntime runtime) { - 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 Node getNodeById(String nodeId){ - return nodeMap.get(nodeId); - } - - 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) { - if ("way".equals(item.getT())) { - float[][] tf = item.getTf(); - String id = item.getId(); - Map dt = item.getDt(); - - // 提取坐标 - float x = tf[0][0]; - float z = tf[0][2]; // Z向下增长 - - // 检查可旋转性 - boolean rotatable = dt.containsKey("agvRotation"); - if (rotatable) { - log.info("Node {} is rotatable", id); - } - - // 提取邻居节点 - List in = (List) dt.get("in"); - List out = (List) dt.get("out"); - Set neighbors = new HashSet<>(); - if (in != null) neighbors.addAll(in); - if (out != null) neighbors.addAll(out); - - // 提取货位链接 - List storeLinks = new ArrayList<>(); - List> links = (List>) dt.get("linkStore"); - if (links != null) { - for (Map link : links) { - String storeId = (String) link.get("item"); - int bay = (Integer) link.get("bay"); - int level = (Integer) link.get("level"); - int cell = (Integer) link.get("cell"); - String direction = (String) link.get("direction"); - storeLinks.add(new StoreLink(storeId, bay, level, cell, LCCDirection.fromString(direction))); - - // 建立货位到节点的反向映射 - storeToNodes.computeIfAbsent(storeId, k -> new ArrayList<>()).add(id); - } - } - - nodeMap.put(id, new Node(id, x, z, rotatable, - new ArrayList<>(neighbors), storeLinks)); - } - } - - // 2. 验证邻居双向连接 - for (Node node : nodeMap.values()) { - Iterator it = node.neighbors().iterator(); - while (it.hasNext()) { - String neighborId = it.next(); - if (!nodeMap.containsKey(neighborId)) { - it.remove(); // 移除无效邻居 - } - } - } - } - - public Node getNode(String id) { - return nodeMap.get(id); - } -} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/Node.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/Node.java deleted file mode 100644 index 252bb11..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/Node.java +++ /dev/null @@ -1,20 +0,0 @@ -package com.galaxis.rcs.plan.path2; - -import java.util.List; - -/** - * 路径节点定义 - * - * @param id 节点ID - * @param x 节点X坐标 - * @param z 节点Z坐标 - * @param rotatable 节点是否可旋转 - * @param neighbors 邻居节点ID列表 - */ -public record Node(String id, - float x, - float z, - boolean rotatable, - List neighbors, - List storeLinks) { -} 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 deleted file mode 100644 index cfffe83..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java +++ /dev/null @@ -1,183 +0,0 @@ -package com.galaxis.rcs.plan.path2; - -import com.galaxis.rcs.common.enums.LCCDirection; - -import java.util.*; - -public class PathUtils { - - /** - * 从 LCCDirection 转换为 AMR 的方向单位 - * 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 - */ - public static short convertDirectionToPtrDiretion(LCCDirection value) { - return switch (value) { - case UP -> 3; - case DOWN -> 1; - case LEFT -> 2; - case RIGHT -> 0; - default -> throw new IllegalArgumentException("Unknown direction: " + value); - }; - } - - /** - * 计算移动方向 - */ - public static LCCDirection calculateMoveDirection(Node from, Node to) { - float dx = to.x() - from.x(); - float dz = to.z() - from.z(); // 注意：Z轴向下增长 - - // 考虑左手坐标系：X向右，Z向下 - if (Math.abs(dx) > Math.abs(dz)) { - return dx > 0.5 ? LCCDirection.RIGHT : LCCDirection.LEFT; - } else { - // 注意：Z向下增长，所以Z值增加表示向下移动 - return dz > 0.5 ? LCCDirection.DOWN : LCCDirection.UP; - } - } - - /** - * 检查方向是否有效（侧插式专用） - */ - public static boolean isValidForSideLoader(Node node, int bay, LCCDirection direction) { - Optional link = node.storeLinks().stream() - .filter(l -> l.bay() == bay) - .findFirst(); - - if (link.isEmpty()) return false; - - LCCDirection requiredDirection = convertForSideLoader(link.get().direction()); - return direction == requiredDirection; - } - - /** - * 转换货位方向到AGV所需方向（侧插式专用） - */ - public static LCCDirection convertForSideLoader(LCCDirection storeDirection) { - /* - * 侧插式AGV方向规则： - * - 货位在上方 → 车头向右（货叉朝左） - * - 货位在下方 → 车头向左（货叉朝左） - * - 货位在左方 → 车头向上（货叉朝左） - * - 货位在右方 → 车头向下（货叉朝左） - */ - return switch (storeDirection) { - case UP -> LCCDirection.RIGHT; - case DOWN -> LCCDirection.LEFT; - case LEFT -> LCCDirection.UP; - case RIGHT -> LCCDirection.DOWN; - }; - } - - /** - * 获取最近的旋转点 - */ - public static Node findNearestRotationNode(NavigationGraph graph, Node from, LCCDirection currentDir, - LCCDirection requiredDir) { - // 如果当前方向已经匹配，不需要旋转 - if (currentDir == requiredDir) return from; - - // 使用Dijkstra算法查找最近的旋转点 - Map distances = new HashMap<>(); - Map predecessors = new HashMap<>(); - PriorityQueue queue = new PriorityQueue<>(Comparator.comparingDouble(distances::get)); - - distances.put(from, 0f); - queue.add(from); - - Node result = null; - - while (!queue.isEmpty()) { - Node current = queue.poll(); - - // 找到可旋转点 - if (current.rotatable()) { - result = current; - break; - } - - // 探索邻居 - for (Node neighbor : graph.getNeighbors(current)) { - float newDist = distances.get(current) + graph.distance(current, neighbor); - - if (newDist < distances.getOrDefault(neighbor, Float.MAX_VALUE)) { - distances.put(neighbor, newDist); - predecessors.put(neighbor, current); - queue.add(neighbor); - } - } - } - - return result; - } - - /** - * 获取相反方向 - */ - public static LCCDirection getOppositeDirection(LCCDirection dir) { - return switch (dir) { - case UP -> LCCDirection.DOWN; - case DOWN -> LCCDirection.UP; - case LEFT -> LCCDirection.RIGHT; - case RIGHT -> LCCDirection.LEFT; - }; - } - - /** - * 计算旋转代价 - */ - public static float calculateRotationCost(LCCDirection from, LCCDirection to, float ROTATION_COST_PER_DEGREE) { - float angle1 = getRequiredDirection(from); - float angle2 = getRequiredDirection(to); - float diff = Math.abs(angle1 - angle2); - diff = Math.min(diff, 360 - diff); - // return diff * ROTATION_COST_PER_DEGREE; - return 0.1f; - } - - /** - * 获取所需方向的角度 - */ - public static float getRequiredDirection(LCCDirection direction) { - return switch (direction) { - case UP -> 90f; - case DOWN -> 270f; - case LEFT -> 180f; - case RIGHT -> 0f; - }; - } - - /** - * 转换货位方向到AGV所需方向 - */ - public static LCCDirection convertStoreDirection(LCCDirection storeDirection) { - // 转换规则: 货位在路径点的方位 -> AGV所需方向 - return switch (storeDirection) { - case UP -> LCCDirection.RIGHT; // 货位在上方 → 车头向右 - case DOWN -> LCCDirection.LEFT; // 货位在下方 → 车头向左 - case LEFT -> LCCDirection.UP; // 货位在左方 → 车头向上 - case RIGHT -> LCCDirection.DOWN; // 货位在右方 → 车头向下 - }; - } - - /** - * 将角度转换为 LCCDirection - */ - public static LCCDirection convertAngleToDirection(float angle) { - // 标准化角度 - angle = ((angle % 360f) + 360f) % 360f; - - if (angle >= 315f || angle < 45f) return LCCDirection.RIGHT; - if (angle >= 45f && angle < 135f) return LCCDirection.UP; - if (angle >= 135f && angle < 225f) return LCCDirection.LEFT; - return LCCDirection.DOWN; - } - - /** - * 计算方向角度差 - */ - public static float angleDifference(float angle1, float angle2) { - float diff = Math.abs(angle1 - angle2) % 360; - return diff > 180 ? 360 - diff : diff; - } -} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java deleted file mode 100644 index 309dfba..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java +++ /dev/null @@ -1,143 +0,0 @@ -package com.galaxis.rcs.plan.path2; - -import com.galaxis.rcs.common.enums.LCCDirection; -import com.galaxis.rcs.plan.PlanTaskSequence; -import com.galaxis.rcs.plan.task.CarryTask; -import com.galaxis.rcs.plan.task.MoveTask; - -import java.util.List; - -import static com.galaxis.rcs.plan.path2.PathUtils.convertStoreDirection; -import static com.galaxis.rcs.plan.path2.PathUtils.getRequiredDirection; - -public class PtrPathPlanner { - private final NavigationGraph graph; - private final AStarPathPlanner astar; - - public PtrPathPlanner(NavigationGraph graph) { - this.graph = graph; - this.astar = new AStarPathPlanner(graph); - } - - - public void planMoveTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, MoveTask moveTask) { - - Node node = this.graph.getNodeById(moveTask.targetWayPointId()); - - List toPath = astar.findPath(startNodeId, startDirection, node.id(), startDirection); - generateMoves(plan, toPath); - plan.addFinish(); - } - - public void planCarryTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, CarryTask task) { - // 取货点 - String loadRackId = task.from().rackId(); - int pickupBay = task.from().bay(); - NodeDirection loadNodeDirection = findNodeForStore(loadRackId, pickupBay); - if (loadNodeDirection == null) { - throw new RuntimeException("Pickup node not found for rackId=" + loadRackId + ", bay=" + pickupBay); - } - - // 放货点 - String unloadRackId = task.to().rackId(); - int unloadBay = task.to().bay(); - NodeDirection unloadNodeDirection = findNodeForStore(unloadRackId, unloadBay); - if (unloadNodeDirection == null) { - throw new RuntimeException("Drop node not found for rackId=" + unloadRackId + ", bay=" + unloadBay); - } - - // 规划到取货点路径 - List toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction()); - - // 检查方向是否匹配，如果不匹配则插入旋转点 - if (!toLoadPath.isEmpty()) { - State lastState = toLoadPath.get(toLoadPath.size() - 1); - if (lastState.direction() != loadNodeDirection.direction()) { - Node rotationNode = PathUtils.findNearestRotationNode( - graph, lastState.node(), lastState.direction(), loadNodeDirection.direction() - ); - - if (rotationNode != null) { - // 插入旋转路径 - List toRotation = astar.findPath( - lastState.node().id(), lastState.direction(), - rotationNode.id(), loadNodeDirection.direction() - ); - toLoadPath.addAll(toRotation); - - // 从旋转点到目标点 - List fromRotation = astar.findPath( - rotationNode.id(), loadNodeDirection.direction(), - loadNodeDirection.node().id(), loadNodeDirection.direction() - ); - toLoadPath.addAll(fromRotation); - } - } - } - - // 规划到放货点路径 - List toUnloadPath = astar.findPath(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirection.node().id(), unloadNodeDirection.direction()); - - // 生成指令序列 - generateMoves(plan, toLoadPath); - plan.addLoad(task.lpn(), loadRackId, pickupBay, task.from().level(), task.from().cell()); - - generateMoves(plan, toUnloadPath); - plan.addUnload(unloadRackId, task.to().level(), task.to().bay(), task.to().cell()); - - plan.addFinish(); - } - - private NodeDirection findNodeForStore(String storeId, int bay) { - List nodes = this.graph.getNodesForStore(storeId); - for (Node node : nodes) { - for (StoreLink link : node.storeLinks()) { - if (link.storeId().equals(storeId) && link.bay() == bay) { - LCCDirection agvDirection = convertStoreDirection(link.direction()); - return new NodeDirection(node, agvDirection); - } - } - } - return null; - } - - /** - * 根据A*状态，生成移动指令序列 - */ - private void generateMoves(PlanTaskSequence sequence, List path) { - if (path.isEmpty()) return; - - // 第一个状态是起点，跳过 - State prevState = path.get(0); - - for (int i = 1; i < path.size(); i++) { - State current = path.get(i); - - // 如果是旋转动作 - if (current.node().equals(prevState.node())) { - float angle = PathUtils.getRequiredDirection(current.direction()); - sequence.addRotationTo(angle); - } - // 移动动作 - 检查是否需要后退 - else { - // 检查移动方向 - LCCDirection moveDir = PathUtils.calculateMoveDirection(prevState.node(), current.node()); - - // 如果移动方向与车头方向相反，需要后退 - boolean isBackward = (current.direction() == PathUtils.getOppositeDirection(moveDir)); - - if (isBackward) { - sequence.addMoveBackward(current.node().id()); - } else { - sequence.addMoveTo(current.node().id()); - } - } - prevState = current; - } - } - - - // 辅助记录类 - private record NodeDirection(Node node, LCCDirection direction) { - } -} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/State.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/State.java deleted file mode 100644 index bd7f3d7..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/State.java +++ /dev/null @@ -1,19 +0,0 @@ -package com.galaxis.rcs.plan.path2; - -import com.galaxis.rcs.common.enums.LCCDirection; - -/** - * A* 路径规划状态 - */ -public record State(Node node, - LCCDirection direction, - float g, - float h, - State parent) - implements Comparable { - - @Override - public int compareTo(State other) { - return Float.compare(g + h, other.g + other.h); - } -} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java deleted file mode 100644 index f171d5d..0000000 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java +++ /dev/null @@ -1,17 +0,0 @@ -package com.galaxis.rcs.plan.path2; - -import com.galaxis.rcs.common.enums.LCCDirection; - -/** - * A* 路径与库位的连接定义 - * - * @param storeId 库位ID - * @param bay 列 - * @param level 层 - * @param cell 格 - * @param direction 方位（路径相对于库位的方位，比如路标的"左边"是库位） - */ -public record StoreLink( - String storeId, int bay, int level, int cell, LCCDirection direction -) { -} diff --git a/servo/src/main/java/com/galaxis/rcs/plan/task/LoadTask.java b/servo/src/main/java/com/galaxis/rcs/plan/task/LoadTask.java new file mode 100644 index 0000000..ead42d5 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/task/LoadTask.java @@ -0,0 +1,15 @@ +package com.galaxis.rcs.plan.task; + +import com.galaxis.rcs.common.entity.StoreLocation; + +/** + * 装入任务 + */ +public record LoadTask( + String agv, + int priority, + StoreLocation to +) { +} + + diff --git a/servo/src/main/java/com/galaxis/rcs/plan/task/MoveTask.java b/servo/src/main/java/com/galaxis/rcs/plan/task/MoveTask.java index 43fecf0..427251b 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/task/MoveTask.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/task/MoveTask.java @@ -1,8 +1,11 @@ package com.galaxis.rcs.plan.task; +import com.galaxis.rcs.common.enums.LCCDirection; + public record MoveTask( String agv, String targetWayPointId, + LCCDirection targetDirection, int priority ) { } diff --git a/servo/src/main/java/com/galaxis/rcs/plan/task/UnloadTask.java b/servo/src/main/java/com/galaxis/rcs/plan/task/UnloadTask.java new file mode 100644 index 0000000..1ade43a --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/task/UnloadTask.java @@ -0,0 +1,15 @@ +package com.galaxis.rcs.plan.task; + +import com.galaxis.rcs.common.entity.StoreLocation; + +/** + * 卸载任务 + */ +public record UnloadTask( + String agv, + int priority, + StoreLocation to +) { +} + + diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/AgvEventManager.java b/servo/src/main/java/com/galaxis/rcs/ptr/AgvEventManager.java index 5f013c8..a0c3bc8 100644 --- a/servo/src/main/java/com/galaxis/rcs/ptr/AgvEventManager.java +++ b/servo/src/main/java/com/galaxis/rcs/ptr/AgvEventManager.java @@ -2,7 +2,6 @@ package com.galaxis.rcs.ptr; import com.galaxis.rcs.common.enums.AgvEventType; import com.yvan.logisticsModel.LogisticsRuntime; -import com.yvan.logisticsModel.PtrAgvItem; import lombok.extern.slf4j.Slf4j; import java.util.List; diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java b/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java index eb41787..8106a7e 100644 --- a/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java +++ b/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java @@ -11,7 +11,6 @@ import com.google.common.collect.Maps; import com.google.common.collect.Sets; import com.yvan.logisticsEnv.EnvPayload; import com.yvan.logisticsModel.LogisticsRuntime; -import com.yvan.logisticsModel.PtrAgvItem; import lombok.SneakyThrows; import lombok.extern.slf4j.Slf4j; import org.clever.core.BannerUtils; diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnector.java b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnector.java new file mode 100644 index 0000000..859cca1 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnector.java @@ -0,0 +1,5 @@ +package com.galaxis.rcs.ptr; + +public abstract class PtrAgvConnector { + +} diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnectorThread.java b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnectorThread.java new file mode 100644 index 0000000..472c753 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnectorThread.java @@ -0,0 +1,167 @@ +package com.galaxis.rcs.ptr; + +import com.fasterxml.jackson.core.JsonProcessingException; +import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector; +import com.galaxis.rcs.ptr.sendEntity.RcsTaskMessage; +import com.yvan.logisticsModel.LogisticsRuntime; +import lombok.extern.slf4j.Slf4j; +import org.eclipse.paho.mqttv5.common.MqttException; + +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.locks.LockSupport; + +@Slf4j +public class PtrAgvConnectorThread extends Thread { + private final AtomicBoolean running = new AtomicBoolean(false); + private final AtomicBoolean paused = new AtomicBoolean(false); + private final Object pauseLock = new Object(); + + private final Cl2DeviceConnector cl2DeviceConnector; + private final PtrAgvItem ptrAgvItem; + private final LogisticsRuntime logisticsRuntime; + private volatile int __currentTaskSeqNo = 0; + volatile PtrAgvDeviceTask __currentTask; + + public PtrAgvConnectorThread(PtrAgvItem ptrAgvItem, Cl2DeviceConnector cl2DeviceConnector, LogisticsRuntime logisticsRuntime) { + super("ExecutorConnector-" + ptrAgvItem.getId()); + this.cl2DeviceConnector = cl2DeviceConnector; + this.ptrAgvItem = ptrAgvItem; + this.logisticsRuntime = logisticsRuntime; + } + + @Override + public void run() { + running.set(true); + log.info("Connector thread started for executor: {}", this.ptrAgvItem.getId()); + + try { + float distance = 0; + int taskCount = 0; + + PtrAgvDeviceTask startTask = null; + RcsTaskMessage taskMessage = null; + + // 计算中的任务 + List computingTaskList = new ArrayList<>(); + while (running.get()) { + if (paused.get()) { + synchronized (pauseLock) { + while (paused.get()) { + pauseLock.wait(); + } + } + } + + PtrAgvDeviceTask currentTask = this.ptrAgvItem.deviceTaskQueue.take(); + if (startTask == null) { + startTask = currentTask; + taskMessage = new RcsTaskMessage(this.logisticsRuntime); + taskMessage.OperationType = startTask.operationType; + taskMessage.PickMode = startTask.pickMode; + taskMessage.GoNow = true; + taskMessage.StartX = startTask.startPoint.logicX; + taskMessage.StartY = startTask.startPoint.logicY; + taskMessage.Link = new ArrayList<>(); + } + currentTask.seqNo = taskMessage.SeqNo; + RcsTaskMessage.LinkData link = new RcsTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed); + taskMessage.Link.add(link); + taskCount++; + distance += euclideanDistance(currentTask.startPoint.tf[0], currentTask.endPoint.tf[0]); + computingTaskList.add(currentTask); + + + PtrAgvDeviceTask nextTask = this.ptrAgvItem.deviceTaskQueue.peek(); + if (currentTask.isGroupEnd + || (taskMessage.Link.size() > 0 && nextTask != null && ((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction)) // 下一个任务和开始任务方向不一致 + // 单向移动距离大于2m时并且点位数量大于1 + || (distance > 2 && taskCount > 1)) { + taskMessage.OperationType = currentTask.operationType; + taskMessage.PickMode = currentTask.pickMode; + taskMessage.EndX = currentTask.groupEndPoint.logicX; + taskMessage.EndY = currentTask.groupEndPoint.logicY; + taskMessage.GoodsSlotDirection = currentTask.goodsSlotDirection; + taskMessage.GoodsSlotHeight = currentTask.goodsSlotHeight; + taskMessage.LinkCounts = (short) taskMessage.Link.size(); + taskMessage.ChargeDirection = currentTask.chargeDirection; + taskMessage.ChargeLocation = currentTask.chargeLocation; + try { + // 发送任务 + this.__currentTaskSeqNo = taskMessage.SeqNo; + this.__currentTask = currentTask; + cl2DeviceConnector.sendTask(ptrAgvItem.getId(), taskMessage); + this.ptrAgvItem.runningDeviceTaskList.addAll(computingTaskList); + for (PtrAgvDeviceTask task : computingTaskList) { + task.taskStatus = 1; + task.taskGroupStatus = 1; + } + computingTaskList.clear(); + } catch (MqttException | JsonProcessingException e) { + log.error("Cl2DeviceConnector robotMove: id=" + ptrAgvItem.getId() + ", task=" + currentTask.movePlanTaskId, e); + } + distance = 0; + taskCount = 0; + taskMessage = null; + startTask = null; + + if (currentTask.isGroupEnd) { + // 当一组任务结束时，阻塞当前线程，等当前任务执组行完毕后在外部线程中唤醒 + LockSupport.park(); // 阻塞当前线程 + } else { + for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) { + if (task.taskGroupStatus < 4) { + LockSupport.park(); // 阻塞当前线程 + break; + } + } + } + } + + } + } catch (InterruptedException e) { + System.out.println("Connector thread interrupted for executor: " + this.ptrAgvItem.getId()); + } finally { + System.out.println("Connector thread stopped for executor: " + this.ptrAgvItem.getId()); + } + } + + public int getCurrentTaskSeqNo() { + return __currentTaskSeqNo; + } + + /** + * 停止连接器线程 + */ + public void stopConnector() { + running.set(false); + this.interrupt(); // 中断阻塞状态 + } + + public void pauseProcessing() { + paused.set(true); + } + + public void resumeProcessing() { + paused.set(false); + synchronized (pauseLock) { + pauseLock.notifyAll(); + } + } + + /** + * 检查连接器是否在运行 + */ + public boolean isRunning() { + return running.get(); + } + + public static float euclideanDistance(float[] p1, float[] p2) { + if (p1.length != 3 || p2.length != 3) throw new IllegalArgumentException(); + float dx = p1[0] - p2[0]; + float dy = p1[1] - p2[1]; + float dz = p1[2] - p2[2]; + return (float) Math.sqrt(dx * dx + dy * dy + dz * dz); + } +} diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java new file mode 100644 index 0000000..313ad9d --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java @@ -0,0 +1,62 @@ +package com.galaxis.rcs.ptr; + +import com.yvan.logisticsModel.StaticItem; + +import java.util.HashSet; +import java.util.Set; + +public class PtrAgvDeviceTask { + //该段目标点X坐标 UInt16 逻辑单位，乘以一定系数才是物理距离 + public int x; + //该段目标点Y坐标 UInt16 逻辑单位，乘以一定系数才是物理距离 + public int y; + // 该段行驶速度 Int16 mm/s + public int speed; + // 该段车头朝向 + public short direction; + // 该段起始点 + public StaticItem startPoint; + // 该段目标点 + public StaticItem endPoint; + // 分组起始点 + public StaticItem groupStartPoint; + // 分组目标点 + public StaticItem groupEndPoint; + // 作业类型 UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业） + public short operationType; + // 提升移栽货物拣货模式 UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作 + public short pickMode; + + // 目标货位朝向 + public short goodsSlotDirection; + + // 目标货位相对于地面的绝对高度 + public int goodsSlotHeight; + + //充电桩朝向UseBriefLocation UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 + public Short chargeDirection; + // 充电工位坐标和充电桩之间距离 UInt16 单位：mm + public Integer chargeLocation; + // 目标货架编号 String 仅做校验使用（仅接货用） + public String storageRacksNo; + // 结束朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 + public Short endDirection; + + /** 移动规划ID */ + public Long movePlanTaskId; + /** 转动取货放货充电等规划ID */ + public Set planTaskIdSet = new HashSet<>(); + /** 业务任务ID */ + public Long bizTaskId; + // 作业序号 发送给小车的作业序号 + public int seqNo; + + // 任务状态 0创建 1任务模式改变 2任务接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更 + public int taskStatus = 0; + public int taskGroupStatus = 0; + + // 任务分组结束标记 生成报文时作为判断依据 + public boolean isGroupEnd = false; + public int checkLogicX; + public int checkLogicY; +} diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java new file mode 100644 index 0000000..aa1c4df --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java @@ -0,0 +1,830 @@ +package com.galaxis.rcs.ptr; + +import com.fasterxml.jackson.annotation.JsonIgnore; +import com.galaxis.rcs.common.entity.RcsTaskPlan; +import com.galaxis.rcs.common.enums.AgvEventType; +import com.galaxis.rcs.common.enums.LCCDirection; +import com.galaxis.rcs.common.enums.PlanTaskStatus; +import com.galaxis.rcs.common.enums.PlanTaskType; +import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector; +import com.galaxis.rcs.plan.PlanTaskSequence; +import com.galaxis.rcs.ptr.receiveEntity.AmrHeartbeatMessage; +import com.galaxis.rcs.ptr.receiveEntity.base.CurBatteryData; +import com.galaxis.rcs.ptr.sendEntity.RcsConfigMessage; +import com.galaxis.rcs.ptr.sendEntity.RcsSRMessage; +import com.galaxis.rcs.ptr.sendEntity.RcsSetLocationMessage; +import com.google.common.base.Joiner; +import com.google.common.collect.Queues; +import com.yvan.logisticsModel.ExecutorItem; +import com.yvan.logisticsModel.LogisticsRuntime; +import com.yvan.logisticsModel.StaticItem; +import lombok.SneakyThrows; +import lombok.extern.slf4j.Slf4j; +import org.clever.core.BannerUtils; +import org.clever.core.json.JsonWrapper; +import org.clever.data.redis.Redis; +import org.clever.data.redis.RedisAdmin; + +import java.util.*; +import java.util.concurrent.BlockingQueue; +import java.util.concurrent.CopyOnWriteArraySet; +import java.util.concurrent.locks.LockSupport; + +//0.4m/ss // a max 1.2m/s +//90 = 3.5s cl2 +//90 = 5s // cLX +@Slf4j +public abstract class PtrAgvItem extends ExecutorItem { + private static final int BLOCKING_QUEUE_CAPACITY = 100; + private static final Redis redis = RedisAdmin.getRedis(); + + // ip + public String ip; + // agv名称 + public String agvName; + // agv类型 + public String agvType; + // agv型号 + public String agvModel; + // AMR功能型号 + public String agvFnModel; + // 电池信息 + public CurBatteryData battery; + // agv当前x坐标 + public double x; + // agv当前y坐标 + public double y; + // agv当前z坐标 + public double z; + // 当前所在站点的逻辑X坐标 Int32 + public int logicX; + // 当前所在站点的逻辑Y坐标 Int32 + public int logicY; + // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 + public short direction; + // agv当前转动角度值 + public double orientation; + + // 任务模式 + public AmrTaskMode taskMode; + + private volatile boolean isManualMode = false; + private volatile boolean isPaused = false; + private volatile PosDirection lastPausedPosition; + private volatile boolean isOnline = false; + + private final Set eventListeners = new CopyOnWriteArraySet<>(); + + // 执行中的任务 + @JsonIgnore + public List runningDeviceTaskList = new ArrayList<>(); + + /** + * 当前执行的任务规划列表 + */ + @JsonIgnore + public volatile PlanTaskSequence planTaskSequence; + + /** + * 当前执行的设备任务列表 + */ + @JsonIgnore + final BlockingQueue deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY); + + final Cl2DeviceConnector cl2DeviceConnector = new Cl2DeviceConnector(this.runtime); + + @JsonIgnore + public final AmrMessageHandler amrMessageHandler; + + /** + * 连接器线程 + */ + private final PtrAgvConnectorThread connectorThread; + + public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map raw) { + super(logisticsRuntime, raw); + this.connectorThread = new PtrAgvConnectorThread(this, this.cl2DeviceConnector, logisticsRuntime); + this.amrMessageHandler = logisticsRuntime.amrMessageHandler; + } + + public abstract RcsConfigMessage getConfig(); + + @SneakyThrows + public synchronized void initialize() { + this.amrMessageHandler.registeHeartBeatSet(this); + + + // 查询当前状态 + requestCurrentStatus(); + this.isInitialized = true; + + this.startConnector(); + } + + public synchronized void shutdown() { + this.stopConnector(); + this.amrMessageHandler.unregisteHeartBeatSet(this); + } + + public synchronized void dispatchTask(PlanTaskSequence taskSequence) { + if (!isFree()) { + throw new IllegalStateException("AGV is not free to accept new tasks"); + } + + if (isManualMode) { + throw new IllegalStateException("AGV is in manual mode and cannot accept tasks"); + } + + this.planTaskSequence = taskSequence; + buildPlanToDeviceTask(); + fireEvent(AgvEventType.PLAN_ACCEPT, this); + connectorThread.resumeProcessing(); + } + + public synchronized void pauseTask() { + if (planTaskSequence == null) { + throw new IllegalStateException("No active task to pause"); + } + + isPaused = true; + lastPausedPosition = new PosDirection(logicX, logicY, direction); + + // 发送停止指令 + RcsSRMessage stopMsg = new RcsSRMessage(this.runtime); + stopMsg.SeqNo = amrMessageHandler.getNewSeqNo(); + stopMsg.OperationCode = 0; // 停止 + stopMsg.StopX = logicX; + stopMsg.StopY = logicY; + + try { + amrMessageHandler.sendCmdSR(this.getId(), stopMsg); + } catch (Exception e) { + log.error("Failed to send stop command to AGV {}", this.getId(), e); + } + + fireEvent(AgvEventType.PLAN_PAUSE, this); + } + + public synchronized void resumeTask() { + if (!isPaused) { + throw new IllegalStateException("Task is not paused"); + } + + // 检查当前位置是否与暂停位置一致 + if (Math.abs(logicX - lastPausedPosition.logicX()) > 1 || + Math.abs(logicY - lastPausedPosition.logicY()) > 1 || + direction != lastPausedPosition.direction()) { + // 需要返回暂停位置 + throw new RuntimeException("AGV position has changed since pause, cannot resume task safely"); + } + + isPaused = false; + connectorThread.resumeProcessing(); + fireEvent(AgvEventType.PLAN_RESUME, this); + } + + @SneakyThrows + public synchronized void cancelTask() { + // 发送取消指令 + amrMessageHandler.sendCmdCancelTask(this.getId(), this.connectorThread.getCurrentTaskSeqNo()); + + if (planTaskSequence != null) { + planTaskSequence = null; + } + if (!deviceTaskQueue.isEmpty()) { + deviceTaskQueue.clear(); + } + + fireEvent(AgvEventType.PLAN_CANCEL, this); + } + + @SneakyThrows + public void setPositionAndDirection(int x, int y, short direction) { + RcsSetLocationMessage setLoc = new RcsSetLocationMessage(amrMessageHandler.getNewSeqNo()); + setLoc.X = (short) x; + setLoc.Y = (short) y; + setLoc.Direction = direction; + + amrMessageHandler.sendCmdSetLocation(this.getId(), setLoc); + } + + public void setControlMode(ControlMode mode) { + // 硬件控制模式设置逻辑 + this.isManualMode = (mode == ControlMode.MANUAL); + + if (mode != ControlMode.FULL_AUTO && planTaskSequence != null) { + cancelTask(); + } + + // 更新Redis状态 + updateRedisStatus(); + } + + @SneakyThrows + public void requestCurrentStatus() { + amrMessageHandler.sendCmdQueryStatus(this.getId()); + } + + public boolean isFree() { + // return (this.logisticsRuntime.isRunning() && this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning()); + if (!this.runtime.isRunning()) { + return false; + } + if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) { + return false; + } + if (!deviceTaskQueue.isEmpty()) { + return false; + } + if (this.isPaused) { + return false; + } + return true; + } + + public void taskCompleted(int logicX, int logicY, short direction, int taskStatus) { + + updatePosition(logicX, logicY, direction); + // 查找当前分组任务 + for (PtrAgvDeviceTask task : runningDeviceTaskList) { + task.taskGroupStatus = taskStatus; + if (taskStatus == 4) { + // 更新计划任务 + List planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList(); + for (RcsTaskPlan planTask : planTaskList) { + planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString()); + planTaskSequence.savePlanTask(planTask); + } + } + } + + if (planTaskSequence != null && planTaskSequence.isAllCompleted()) { + fireEvent(AgvEventType.PLAN_COMPLETE, this); + this.runningDeviceTaskList.clear(); + planTaskSequence = null; + } + LockSupport.unpark(connectorThread); + } + + public void updatePosition(int logicX, int logicY, short direction) { + int oldX = this.logicX; + int oldY = this.logicY; + short oldDirection = this.direction; + + this.logicX = logicX; + this.logicY = logicY; + this.direction = direction; + + // 更新Redis + updateRedisStatus(); + + // 触发位置变化事件 + if (oldX != logicX || oldY != logicY) { + fireEvent(AgvEventType.POS_CHANGED, this, + new PosDirection(oldX, oldY, oldDirection), + new PosDirection(logicX, logicY, direction)); + } + + if (oldDirection != direction) { + fireEvent(AgvEventType.DIRECTION_CHANGED, this, + oldDirection, direction); + } + + boolean needCompute = false; + + // 从 runningDeviceTaskList 里面，找到完成到什么阶段 + // 比如 (1,2) -> (2,2) -> (3,2) , 如果 updatePosition=3,2 ，那么前2个任务都要完成 + int finishTargetIndex = -1; + if (this.runningDeviceTaskList != null && !this.runningDeviceTaskList.isEmpty() && + this.planTaskSequence != null && !this.planTaskSequence.isEmpty()) { + + for (int i = 0; i < runningDeviceTaskList.size(); i++) { + PtrAgvDeviceTask task = runningDeviceTaskList.get(i); + if (task.checkLogicX == logicX && task.checkLogicY == logicY && task.taskStatus < 4) { + finishTargetIndex = i; + break; + } + } + + if (finishTargetIndex >= 0) { + needCompute = true; + // 标记前面的任务都完成了 + for (int i = 0; i <= finishTargetIndex; i++) { + PtrAgvDeviceTask task = runningDeviceTaskList.get(i); + task.taskStatus = 4; // 标记为完成 + fireEvent(AgvEventType.DEVICE_TASK_COMPLETE, this, task); + + // 更新计划任务 + RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId); + if (planTask != null) { + planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString()); + planTaskSequence.savePlanTask(planTask); + } + } + + if (planTaskSequence.isAllCompleted()) { + fireEvent(AgvEventType.PLAN_COMPLETE, this); + this.runningDeviceTaskList.clear(); + planTaskSequence = null; + } + } + } + + BannerUtils.printConfig(log, "updatePosition", new String[]{ + "logicX: " + logicX, + "logicY: " + logicY, + "direction: " + direction, + "finishTargetIndex: " + finishTargetIndex, + "runningDeviceSize:" + (this.runningDeviceTaskList == null ? "null" : this.runningDeviceTaskList.size()), + "planTask:" + (this.planTaskSequence == null ? "null" : + ("\n" + Joiner.on("\n").join((List) this.planTaskSequence.toPrettyMap().get("items"))) + ) + }); + + if (needCompute && this.runningDeviceTaskList.size() > 0) { + int index = this.runningDeviceTaskList.size() - 1; + PtrAgvDeviceTask task = this.runningDeviceTaskList.get(index); + if (task.groupEndPoint != task.endPoint) { + LockSupport.unpark(connectorThread); + } + } + } + + /** + * 更新设备任务状态 暂时没有处理任务取消相关的状态 + * + * @param seqNo + * @param x + * @param y + * @param messageStatus + */ + public void updateDeviceTaskStatus(int seqNo, int x, int y, int messageStatus) { + // 更新任务状态逻辑 + if (messageStatus < 2) { + return; + } + + // 任务完成逻辑，在地标检查里 + } + + public void updateTaskMode(int taskMode) { + + } + + public void updateRedisStatus() { + String statusKey = getRedisKey("status"); + Map statusMap = new HashMap<>(); + statusMap.put("x", String.valueOf(x)); + statusMap.put("y", String.valueOf(y)); + statusMap.put("z", String.valueOf(z)); + statusMap.put("logicX", String.valueOf(logicX)); + statusMap.put("logicY", String.valueOf(logicY)); + statusMap.put("direction", String.valueOf(direction)); + statusMap.put("orientation", String.valueOf(orientation)); + statusMap.put("soc", this.battery == null ? "-1" : String.valueOf(this.battery.SOC)); + statusMap.put("mode", isManualMode ? "MANUAL" : "AUTO"); + statusMap.put("taskStatus", getTaskStatus()); + + redis.hPutAll(statusKey, statusMap); + redis.kExpire(statusKey, 10); // 10秒过期 + } + + public void handleHeartbeat(AmrHeartbeatMessage heartbeat) { + // 更新在线状态 + String aliveKey = getRedisKey("alive"); + redis.vSet(aliveKey, "1"); + redis.kExpire(aliveKey, 5); // 5秒过期 + + // 更新状态信息 + if (this.battery == null) { + this.battery = new CurBatteryData(); + } + this.battery.SOC = heartbeat.Battery; + this.battery.setTemperature(heartbeat.Temperature.Battery); + + // 检查低电量 + if (this.battery.SOC < 20) { + fireEvent(AgvEventType.LOW_BATTERY, this); + } + + updateRedisStatus(); + } + + private void handleOnlineEvent() { + isOnline = true; + fireEvent(AgvEventType.ONLINE, this); + requestCurrentStatus(); + } + + private void handleOfflineEvent() { + isOnline = false; + fireEvent(AgvEventType.OFFLINE, this); + } + + // 事件监听管理 + public void addEventListener(AgvEventListener listener) { + eventListeners.add(listener); + } + + public void removeEventListener(AgvEventListener listener) { + eventListeners.remove(listener); + } + + private void fireEvent(AgvEventType type, Object... args) { + for (AgvEventListener listener : eventListeners) { + listener.onEvent(type, args); + } + } + + private String getTaskStatus() { + if (planTaskSequence == null) return "IDLE"; + if (isPaused) return "PAUSED"; + return "EXECUTING"; + } + + + /** + * 启动连接器线程 + */ + public void startConnector() { + if (!connectorThread.isRunning()) { + connectorThread.start(); + System.out.println("Connector started for executor: " + this.getId()); + } + } + + /** + * 停止连接器线程 + */ + public void stopConnector() { + connectorThread.stop(); + System.out.println("Connector stopped for executor: " + this.getId()); + } + + private static final int speed = 1000; + + /** + * 添加任务序列到当前执行器 + */ + protected void buildPlanToDeviceTask() { + PlanTaskSequence sequence = this.planTaskSequence; + LogisticsRuntime runtime = sequence.logisticsRuntime; + + short direction = this.direction; + + // 获取当前设备点位(逻辑点位) + StaticItem startPoint = runtime.getStaticItemByLogicXY(this.logicX, this.logicY); + if (startPoint == null) { + log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, agv当前点位为空 地图上没有标记", this.getId(), sequence.bizTask.getBizTaskId()); + } + StaticItem groupStartPoint = startPoint; + + Set rotationPlanTaskIdSet = new HashSet<>(); + + // 生成移动报文 + List deviceTaskList = new ArrayList<>(); + List> linkStore = null; + // 检查 planList 是不是全都是我的任务 + for (int i = 0; i < sequence.taskList.size(); i++) { + RcsTaskPlan plan = sequence.taskList.get(i); + String endPointId = plan.getTargetId(); + + if (plan.getPlanType().equals(PlanTaskType.MOVE.toString()) || plan.getPlanType().equals(PlanTaskType.MOVE_BACKWARD.toString())) { + // 获取目标点信息 + StaticItem pointItem = runtime.getStaticItemById(endPointId); + linkStore = (List>) pointItem.dt.get("linkStore"); + int d = -1; + if (startPoint.logicX == pointItem.logicX && startPoint.logicY != pointItem.logicY) { + d = pointItem.logicY >= startPoint.logicY ? CDirection.db : CDirection.dt; + if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) { + throw new RuntimeException("方向错误"); + } + + } 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("方向错误"); + } + // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX()); + + } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX == pointItem.logicX) { + d = direction; + // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX()); + + } else { + throw new RuntimeException("无法识别的点位关系"); + } + PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); + deviceTask.x = pointItem.logicX; + deviceTask.y = pointItem.logicY; + deviceTask.speed = d == direction ? (speed) : (-speed); + deviceTask.direction = direction; + deviceTask.pickMode = 0; + deviceTask.startPoint = startPoint; + deviceTask.endPoint = pointItem; + deviceTask.bizTaskId = plan.getBizTaskId(); + deviceTask.movePlanTaskId = plan.getPlanTaskId(); + deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); + rotationPlanTaskIdSet.clear(); + // 行走任务完成后，检查用的字段 + deviceTask.checkLogicX = pointItem.logicX; + deviceTask.checkLogicY = pointItem.logicY; + deviceTaskList.add(deviceTask); + // 设置新的起点 + startPoint = pointItem; + + } else if (plan.getPlanType().equals(PlanTaskType.ROTATION.toString())) { + + float r = plan.getTargetRotation().floatValue(); + while (r > 360) { + r -= 360; + } + while (r < 0) { + r += 360; + } + + if (r >= 315 || r < 45) { + direction = CDirection.dr; + + } else if (r >= 45 && r < 135) { + direction = CDirection.dt; + + } else if (r >= 135 && r < 225) { + direction = CDirection.dl; + + } else if (r >= 225 && r < 315) { + direction = CDirection.db; + } + rotationPlanTaskIdSet.add(plan.getPlanTaskId()); + + } else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) { + + if (deviceTaskList.isEmpty()) { + PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); + deviceTask.x = startPoint.logicX; + deviceTask.y = startPoint.logicY; + deviceTask.speed = speed; + deviceTask.direction = direction; + deviceTask.pickMode = 0; + deviceTask.startPoint = startPoint; + deviceTask.endPoint = startPoint; + deviceTask.bizTaskId = plan.getBizTaskId(); + deviceTask.movePlanTaskId = plan.getPlanTaskId(); + deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); + rotationPlanTaskIdSet.clear(); + // 行走任务完成后，检查用的字段 + deviceTask.checkLogicX = startPoint.logicX; + deviceTask.checkLogicY = startPoint.logicY; + deviceTaskList.add(deviceTask); + linkStore = (List>) startPoint.dt.get("linkStore"); + } + PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); + deviceTask.operationType = COperationType.transplantLoadAndUnload; + deviceTask.pickMode = CPickMode.load; + deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); + //处理取货高度 + StaticItem storeItem = runtime.getStaticItemById(endPointId); + Map storeItemRaw = storeItem.dt; + if (storeItemRaw.containsKey("bays")) { + List> bays = (List>) storeItemRaw.get("bays"); + Map bay = bays.get(plan.getTargetBay()); + List levelHeight = (List) bay.get("levelHeight"); + deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000); + } else { + deviceTask.goodsSlotHeight = 1; + } + if (linkStore != null) { + for (Map store : linkStore) { + if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) { + short d = 0; + switch (store.get("direction").toString()) { + case "up": + d = 1; + break; + case "right": + d = 2; + break; + case "down": + d = 3; + break; + case "left": + d = 0; + break; + } + deviceTask.goodsSlotDirection = d; + } + } + } + + // 标记任务分组结束 + deviceTask.isGroupEnd = true; + deviceTask.groupEndPoint = deviceTask.endPoint; + deviceTask.groupStartPoint = groupStartPoint; + groupStartPoint = deviceTask.endPoint; + + } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) { + if (deviceTaskList.isEmpty()) { + PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); + deviceTask.x = startPoint.logicX; + deviceTask.y = startPoint.logicY; + deviceTask.speed = speed; + deviceTask.direction = direction; + deviceTask.pickMode = 0; + deviceTask.startPoint = startPoint; + deviceTask.endPoint = startPoint; + deviceTask.bizTaskId = plan.getBizTaskId(); + deviceTask.movePlanTaskId = plan.getPlanTaskId(); + deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); + rotationPlanTaskIdSet.clear(); + // 行走任务完成后，检查用的字段 + deviceTask.checkLogicX = startPoint.logicX; + deviceTask.checkLogicY = startPoint.logicY; + deviceTaskList.add(deviceTask); + linkStore = (List>) startPoint.dt.get("linkStore"); + } + PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); + deviceTask.operationType = COperationType.transplantLoadAndUnload; + deviceTask.pickMode = CPickMode.unload; + deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); + // 处理卸货高度 + StaticItem storeItem = runtime.getStaticItemById(endPointId); + Map storeItemRaw = storeItem.dt; + if (storeItemRaw.containsKey("bays")) { + List> bays = (List>) storeItemRaw.get("bays"); + Map bay = bays.get(plan.getTargetBay()); + List levelHeight = (List) bay.get("levelHeight"); + deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000); + } else { + deviceTask.goodsSlotHeight = 1; + } + if (linkStore != null) { + for (Map store : linkStore) { + if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) { + short d = 0; + switch (store.get("direction").toString()) { + case "up": + d = 1; + break; + case "right": + d = 2; + break; + case "down": + d = 3; + break; + case "left": + d = 0; + break; + } + deviceTask.goodsSlotDirection = d; + } + } + } + // 标记任务分组结束 + deviceTask.isGroupEnd = true; + deviceTask.groupEndPoint = deviceTask.endPoint; + deviceTask.groupStartPoint = groupStartPoint; + groupStartPoint = deviceTask.endPoint; + + } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) { + if (deviceTaskList.isEmpty()) { + PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); + deviceTask.x = startPoint.logicX; + deviceTask.y = startPoint.logicY; + deviceTask.speed = speed; + deviceTask.direction = direction; + deviceTask.pickMode = 0; + deviceTask.startPoint = startPoint; + deviceTask.endPoint = startPoint; + deviceTask.bizTaskId = plan.getBizTaskId(); + deviceTask.movePlanTaskId = plan.getPlanTaskId(); + deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); + rotationPlanTaskIdSet.clear(); + // 行走任务完成后，检查用的字段 + deviceTask.checkLogicX = startPoint.logicX; + deviceTask.checkLogicY = startPoint.logicY; + deviceTaskList.add(deviceTask); + } + PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); + deviceTask.operationType = COperationType.charge; + deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); + // 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm) + deviceTask.chargeDirection = 0; + deviceTask.chargeLocation = 200; + // 标记任务分组结束 + deviceTask.isGroupEnd = true; + deviceTask.groupEndPoint = deviceTask.endPoint; + deviceTask.groupStartPoint = groupStartPoint; + groupStartPoint = deviceTask.endPoint; + } + + if (!plan.getExecutorId().equals(this.getId())) { + throw new RuntimeException("plan not belong executor:" + this.getId() + ", " + plan.getExecutorId()); + } + + } + + if (deviceTaskList.size() <= 0) { + return; + } + + // 标记任务分组结束 + PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); + deviceTask.groupEndPoint = deviceTask.endPoint; + deviceTask.groupStartPoint = groupStartPoint; + deviceTask.isGroupEnd = true; + // 最后一个规划任务为旋转时需要添加一个endDirection + if (rotationPlanTaskIdSet.size() > 0) { + deviceTask.endDirection = direction; + deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); + rotationPlanTaskIdSet.clear(); + } + + // 反向标记任务组 + int lastIndex = deviceTaskList.size() - 1; + for (int i = deviceTaskList.size() - 1; i >= 0; i--) { + PtrAgvDeviceTask d = deviceTaskList.get(i); + if (d.isGroupEnd) { + lastIndex = i; + } else { + d.operationType = deviceTaskList.get(lastIndex).operationType; + d.pickMode = deviceTaskList.get(lastIndex).pickMode; + d.groupStartPoint = deviceTaskList.get(lastIndex).groupStartPoint; + d.groupEndPoint = deviceTaskList.get(lastIndex).groupEndPoint; + d.goodsSlotHeight = deviceTaskList.get(lastIndex).goodsSlotHeight; + d.goodsSlotDirection = deviceTaskList.get(lastIndex).goodsSlotDirection; + d.chargeDirection = deviceTaskList.get(lastIndex).chargeDirection; + d.chargeLocation = deviceTaskList.get(lastIndex).chargeLocation; + } + } + + +// planQueue.addAll(sequence.taskList); + deviceTaskQueue.addAll(deviceTaskList); + + String json = JsonWrapper.toJson(deviceTaskList); + log.info("deviceTaskList: {}", json); + + // TODO: 开启轮询线程，等待下一个待执行任务 + } + + public boolean isSamePosition(PosDirection startPos) { + return this.logicX == startPos.logicX() && this.logicY == startPos.logicY() && this.direction == startPos.direction(); + } + + private static class CDirection { + private static final short dr = 0; + private static final short db = 1; + private static final short dl = 2; + private static final short dt = 3; + } + + + private static class COperationType { + public static final short move = 0; + public static final short load = 1; + public static final short unpick = 2; + public static final short charge = 3; + public static final short transplantLoadAndUnload = 4; + public static final short rollerLoadAndUnload = 5; + } + + private static class CPickMode { + public static final short normal = 0; + public static final short load = 1; + public static final short unload = 2; + public static final short adjustHeight = 3; + public static final short adjustHeightToLoad = 5; + public static final short adjustHeightToUnload = 6; + } + + + /** + * 从 AMR 方向转换为 LCC 方向枚举 + * + * @return + */ + public LCCDirection getLCCDirection() { + return switch (direction) { + case 0 -> LCCDirection.RIGHT; + case 1 -> LCCDirection.DOWN; + case 2 -> LCCDirection.LEFT; + case 3 -> LCCDirection.UP; + default -> null; + }; + } + + public short getAmrDirection(LCCDirection lccDirection) { + return switch (lccDirection) { + case RIGHT -> 0; + case DOWN -> 1; + case LEFT -> 2; + case UP -> 3; + default -> -1; // 未知方向 + }; + } + + private String getRedisKey(String type) { + return String.format("lcc:%s:%s:rcs:%s_%s", + runtime.projectUUID, runtime.envId, type, this.getId()); + } +} diff --git a/servo/src/main/java/com/yvan/logisticsModel/BaseItem.java b/servo/src/main/java/com/yvan/logisticsModel/BaseItem.java index 77f7ac0..7c094ba 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/BaseItem.java +++ b/servo/src/main/java/com/yvan/logisticsModel/BaseItem.java @@ -12,22 +12,22 @@ import java.util.Map; */ @Data public abstract class BaseItem { - final String id; - final String t; + public final String id; + public final String t; /** * 变换矩阵, 3x3矩阵, tf[0]=position,采用 X向右增长，Y轴向屏幕外增长，Z向下增长, tf[1]=rotation, tf[2]=scale */ - final float[][] tf; + public final float[][] tf; /** * 物品的自定义数据 */ @JsonIgnore - final Map dt; + public final Map dt; @JsonIgnore - final Map raw; + public final Map raw; @JsonIgnore public final LogisticsRuntime runtime; @@ -60,16 +60,4 @@ public abstract class BaseItem { this.dt = (Map) map.get("dt"); } - - public float getTransformationX() { - return tf[0][0]; - } - - public float getTransformationY() { - return tf[0][2]; - } - - public float getTransformationZ() { - return tf[0][1]; - } } diff --git a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java index 6694ba4..e83ee5a 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java +++ b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java @@ -2,8 +2,8 @@ package com.yvan.logisticsModel; import com.fasterxml.jackson.annotation.JsonIgnore; import com.galaxis.rcs.connector.cl2.Cl2Item; -import com.galaxis.rcs.plan.path2.NavigationGraph; -import com.galaxis.rcs.plan.path2.PtrPathPlanner; +import com.galaxis.rcs.plan.path.NavigationGraph; +import com.galaxis.rcs.plan.path.PtrPathPlanner; import com.galaxis.rcs.ptr.AgvEventManager; import com.galaxis.rcs.ptr.AmrMessageHandler; import com.galaxis.rcs.task.TaskDispatchFactory; diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnector.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnector.java deleted file mode 100644 index de74dbb..0000000 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnector.java +++ /dev/null @@ -1,7 +0,0 @@ -package com.yvan.logisticsModel; - -import com.galaxis.rcs.common.entity.RcsTaskPlan; - -public abstract class PtrAgvConnector { - -} diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java deleted file mode 100644 index 97dd691..0000000 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java +++ /dev/null @@ -1,167 +0,0 @@ -package com.yvan.logisticsModel; - -import com.fasterxml.jackson.core.JsonProcessingException; -import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector; -import com.galaxis.rcs.ptr.sendEntity.RcsTaskMessage; -import com.yvan.logisticsMonitor.task.DeviceTask; -import lombok.extern.slf4j.Slf4j; -import org.eclipse.paho.mqttv5.common.MqttException; - -import java.util.ArrayList; -import java.util.List; -import java.util.concurrent.atomic.AtomicBoolean; -import java.util.concurrent.locks.LockSupport; - -@Slf4j -public class PtrAgvConnectorThread extends Thread { - private final AtomicBoolean running = new AtomicBoolean(false); - private final AtomicBoolean paused = new AtomicBoolean(false); - private final Object pauseLock = new Object(); - - private final Cl2DeviceConnector cl2DeviceConnector; - private final PtrAgvItem ptrAgvItem; - private final LogisticsRuntime logisticsRuntime; - private volatile int __currentTaskSeqNo = 0; - volatile PtrAgvDeviceTask __currentTask; - - public PtrAgvConnectorThread(PtrAgvItem ptrAgvItem, Cl2DeviceConnector cl2DeviceConnector, LogisticsRuntime logisticsRuntime) { - super("ExecutorConnector-" + ptrAgvItem.getId()); - this.cl2DeviceConnector = cl2DeviceConnector; - this.ptrAgvItem = ptrAgvItem; - this.logisticsRuntime = logisticsRuntime; - } - - @Override - public void run() { - running.set(true); - log.info("Connector thread started for executor: {}", this.ptrAgvItem.getId()); - - try { - float distance = 0; - int taskCount = 0; - - PtrAgvDeviceTask startTask = null; - RcsTaskMessage taskMessage = null; - - // 计算中的任务 - List computingTaskList = new ArrayList<>(); - while (running.get()) { - if (paused.get()) { - synchronized (pauseLock) { - while (paused.get()) { - pauseLock.wait(); - } - } - } - - PtrAgvDeviceTask currentTask = this.ptrAgvItem.deviceTaskQueue.take(); - if (startTask == null) { - startTask = currentTask; - taskMessage = new RcsTaskMessage(this.logisticsRuntime); - taskMessage.OperationType = startTask.operationType; - taskMessage.PickMode = startTask.pickMode; - taskMessage.GoNow = true; - taskMessage.StartX = startTask.startPoint.logicX; - taskMessage.StartY = startTask.startPoint.logicY; - taskMessage.Link = new ArrayList<>(); - } - currentTask.seqNo = taskMessage.SeqNo; - RcsTaskMessage.LinkData link = new RcsTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed); - taskMessage.Link.add(link); - taskCount++; - distance += euclideanDistance(currentTask.startPoint.tf[0], currentTask.endPoint.tf[0]); - computingTaskList.add(currentTask); - - - PtrAgvDeviceTask nextTask = this.ptrAgvItem.deviceTaskQueue.peek(); - if (currentTask.isGroupEnd - || (taskMessage.Link.size() > 0 && nextTask != null && ((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction)) // 下一个任务和开始任务方向不一致 - // 单向移动距离大于2m时并且点位数量大于1 - || (distance > 2 && taskCount > 1)) { - taskMessage.OperationType = currentTask.operationType; - taskMessage.PickMode = currentTask.pickMode; - taskMessage.EndX = currentTask.groupEndPoint.logicX; - taskMessage.EndY = currentTask.groupEndPoint.logicY; - taskMessage.GoodsSlotDirection = currentTask.goodsSlotDirection; - taskMessage.GoodsSlotHeight = currentTask.goodsSlotHeight; - taskMessage.LinkCounts = (short) taskMessage.Link.size(); - taskMessage.ChargeDirection = currentTask.chargeDirection; - taskMessage.ChargeLocation = currentTask.chargeLocation; - try { - // 发送任务 - this.__currentTaskSeqNo = taskMessage.SeqNo; - this.__currentTask = currentTask; - cl2DeviceConnector.sendTask(ptrAgvItem.getId(), taskMessage); - this.ptrAgvItem.runningDeviceTaskList.addAll(computingTaskList); - for (PtrAgvDeviceTask task : computingTaskList) { - task.taskStatus = 1; - task.taskGroupStatus = 1; - } - computingTaskList.clear(); - } catch (MqttException | JsonProcessingException e) { - log.error("Cl2DeviceConnector robotMove: id=" + ptrAgvItem.getId() + ", task=" + currentTask.movePlanTaskId, e); - } - distance = 0; - taskCount = 0; - taskMessage = null; - startTask = null; - - if (currentTask.isGroupEnd) { - // 当一组任务结束时，阻塞当前线程，等当前任务执组行完毕后在外部线程中唤醒 - LockSupport.park(); // 阻塞当前线程 - } else { - for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) { - if (task.taskGroupStatus < 4) { - LockSupport.park(); // 阻塞当前线程 - break; - } - } - } - } - - } - } catch (InterruptedException e) { - System.out.println("Connector thread interrupted for executor: " + this.ptrAgvItem.getId()); - } finally { - System.out.println("Connector thread stopped for executor: " + this.ptrAgvItem.getId()); - } - } - - public int getCurrentTaskSeqNo() { - return __currentTaskSeqNo; - } - - /** - * 停止连接器线程 - */ - public void stopConnector() { - running.set(false); - this.interrupt(); // 中断阻塞状态 - } - - public void pauseProcessing() { - paused.set(true); - } - - public void resumeProcessing() { - paused.set(false); - synchronized (pauseLock) { - pauseLock.notifyAll(); - } - } - - /** - * 检查连接器是否在运行 - */ - public boolean isRunning() { - return running.get(); - } - - public static float euclideanDistance(float[] p1, float[] p2) { - if (p1.length != 3 || p2.length != 3) throw new IllegalArgumentException(); - float dx = p1[0] - p2[0]; - float dy = p1[1] - p2[1]; - float dz = p1[2] - p2[2]; - return (float) Math.sqrt(dx * dx + dy * dy + dz * dz); - } -} diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java deleted file mode 100644 index 0b6b70f..0000000 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java +++ /dev/null @@ -1,60 +0,0 @@ -package com.yvan.logisticsModel; - -import java.util.HashSet; -import java.util.Set; - -public class PtrAgvDeviceTask { - //该段目标点X坐标 UInt16 逻辑单位，乘以一定系数才是物理距离 - public int x; - //该段目标点Y坐标 UInt16 逻辑单位，乘以一定系数才是物理距离 - public int y; - // 该段行驶速度 Int16 mm/s - public int speed; - // 该段车头朝向 - public short direction; - // 该段起始点 - public StaticItem startPoint; - // 该段目标点 - public StaticItem endPoint; - // 分组起始点 - public StaticItem groupStartPoint; - // 分组目标点 - public StaticItem groupEndPoint; - // 作业类型 UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业） - public short operationType; - // 提升移栽货物拣货模式 UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作 - public short pickMode; - - // 目标货位朝向 - public short goodsSlotDirection; - - // 目标货位相对于地面的绝对高度 - public int goodsSlotHeight; - - //充电桩朝向UseBriefLocation UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 - public Short chargeDirection; - // 充电工位坐标和充电桩之间距离 UInt16 单位：mm - public Integer chargeLocation; - // 目标货架编号 String 仅做校验使用（仅接货用） - public String storageRacksNo; - // 结束朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 - public Short endDirection; - - /** 移动规划ID */ - public Long movePlanTaskId; - /** 转动取货放货充电等规划ID */ - public Set planTaskIdSet = new HashSet<>(); - /** 业务任务ID */ - public Long bizTaskId; - // 作业序号 发送给小车的作业序号 - public int seqNo; - - // 任务状态 0创建 1任务模式改变 2任务接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更 - public int taskStatus = 0; - public int taskGroupStatus = 0; - - // 任务分组结束标记 生成报文时作为判断依据 - public boolean isGroupEnd = false; - public int checkLogicX; - public int checkLogicY; -} diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java b/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java deleted file mode 100644 index 2e5c610..0000000 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java +++ /dev/null @@ -1,829 +0,0 @@ -package com.yvan.logisticsModel; - -import com.fasterxml.jackson.annotation.JsonIgnore; -import com.galaxis.rcs.common.entity.RcsTaskPlan; -import com.galaxis.rcs.common.enums.AgvEventType; -import com.galaxis.rcs.common.enums.LCCDirection; -import com.galaxis.rcs.common.enums.PlanTaskStatus; -import com.galaxis.rcs.common.enums.PlanTaskType; -import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector; -import com.galaxis.rcs.plan.PlanTaskSequence; -import com.galaxis.rcs.ptr.*; -import com.galaxis.rcs.ptr.receiveEntity.AmrHeartbeatMessage; -import com.galaxis.rcs.ptr.receiveEntity.base.CurBatteryData; -import com.galaxis.rcs.ptr.sendEntity.RcsConfigMessage; -import com.galaxis.rcs.ptr.sendEntity.RcsSRMessage; -import com.galaxis.rcs.ptr.sendEntity.RcsSetLocationMessage; -import com.google.common.base.Joiner; -import com.google.common.collect.Queues; -import com.yvan.logisticsMonitor.task.PlanTask; -import lombok.SneakyThrows; -import lombok.extern.slf4j.Slf4j; -import org.clever.core.BannerUtils; -import org.clever.core.json.JsonWrapper; -import org.clever.data.redis.Redis; -import org.clever.data.redis.RedisAdmin; - -import java.util.*; -import java.util.concurrent.BlockingQueue; -import java.util.concurrent.CopyOnWriteArraySet; -import java.util.concurrent.locks.LockSupport; -import java.util.stream.Collectors; - -//0.4m/ss // a max 1.2m/s -//90 = 3.5s cl2 -//90 = 5s // cLX -@Slf4j -public abstract class PtrAgvItem extends ExecutorItem { - private static final int BLOCKING_QUEUE_CAPACITY = 100; - private static final Redis redis = RedisAdmin.getRedis(); - - // ip - public String ip; - // agv名称 - public String agvName; - // agv类型 - public String agvType; - // agv型号 - public String agvModel; - // AMR功能型号 - public String agvFnModel; - // 电池信息 - public CurBatteryData battery; - // agv当前x坐标 - public double x; - // agv当前y坐标 - public double y; - // agv当前z坐标 - public double z; - // 当前所在站点的逻辑X坐标 Int32 - public int logicX; - // 当前所在站点的逻辑Y坐标 Int32 - public int logicY; - // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向 - public short direction; - // agv当前转动角度值 - public double orientation; - - // 任务模式 - public AmrTaskMode taskMode; - - private volatile boolean isManualMode = false; - private volatile boolean isPaused = false; - private volatile PosDirection lastPausedPosition; - private volatile boolean isOnline = false; - - private final Set eventListeners = new CopyOnWriteArraySet<>(); - - // 执行中的任务 - @JsonIgnore - public List runningDeviceTaskList = new ArrayList<>(); - - /** - * 当前执行的任务规划列表 - */ - @JsonIgnore - private volatile PlanTaskSequence planTaskSequence; - - /** - * 当前执行的设备任务列表 - */ - @JsonIgnore - final BlockingQueue deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY); - - final Cl2DeviceConnector cl2DeviceConnector = new Cl2DeviceConnector(this.runtime); - - @JsonIgnore - public final AmrMessageHandler amrMessageHandler; - - /** - * 连接器线程 - */ - private final PtrAgvConnectorThread connectorThread; - - public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map raw) { - super(logisticsRuntime, raw); - this.connectorThread = new PtrAgvConnectorThread(this, this.cl2DeviceConnector, logisticsRuntime); - this.amrMessageHandler = logisticsRuntime.amrMessageHandler; - } - - public abstract RcsConfigMessage getConfig(); - - @SneakyThrows - public synchronized void initialize() { - this.amrMessageHandler.registeHeartBeatSet(this); - - - // 查询当前状态 - requestCurrentStatus(); - this.isInitialized = true; - - this.startConnector(); - } - - public synchronized void shutdown() { - this.stopConnector(); - this.amrMessageHandler.unregisteHeartBeatSet(this); - } - - public synchronized void dispatchTask(PlanTaskSequence taskSequence) { - if (!isFree()) { - throw new IllegalStateException("AGV is not free to accept new tasks"); - } - - if (isManualMode) { - throw new IllegalStateException("AGV is in manual mode and cannot accept tasks"); - } - - this.planTaskSequence = taskSequence; - buildPlanToDeviceTask(); - fireEvent(AgvEventType.PLAN_ACCEPT, this); - connectorThread.resumeProcessing(); - } - - public synchronized void pauseTask() { - if (planTaskSequence == null) { - throw new IllegalStateException("No active task to pause"); - } - - isPaused = true; - lastPausedPosition = new PosDirection(logicX, logicY, direction); - - // 发送停止指令 - RcsSRMessage stopMsg = new RcsSRMessage(this.runtime); - stopMsg.SeqNo = amrMessageHandler.getNewSeqNo(); - stopMsg.OperationCode = 0; // 停止 - stopMsg.StopX = logicX; - stopMsg.StopY = logicY; - - try { - amrMessageHandler.sendCmdSR(this.getId(), stopMsg); - } catch (Exception e) { - log.error("Failed to send stop command to AGV {}", this.getId(), e); - } - - fireEvent(AgvEventType.PLAN_PAUSE, this); - } - - public synchronized void resumeTask() { - if (!isPaused) { - throw new IllegalStateException("Task is not paused"); - } - - // 检查当前位置是否与暂停位置一致 - if (Math.abs(logicX - lastPausedPosition.logicX()) > 1 || - Math.abs(logicY - lastPausedPosition.logicY()) > 1 || - direction != lastPausedPosition.direction()) { - // 需要返回暂停位置 - throw new RuntimeException("AGV position has changed since pause, cannot resume task safely"); - } - - isPaused = false; - connectorThread.resumeProcessing(); - fireEvent(AgvEventType.PLAN_RESUME, this); - } - - @SneakyThrows - public synchronized void cancelTask() { - if (planTaskSequence == null) { - throw new IllegalStateException("No active task to cancel"); - } - - // 发送取消指令 - amrMessageHandler.sendCmdCancelTask(this.getId(), this.connectorThread.getCurrentTaskSeqNo()); - - planTaskSequence = null; - deviceTaskQueue.clear(); - fireEvent(AgvEventType.PLAN_CANCEL, this); - } - - @SneakyThrows - public void setPositionAndDirection(int x, int y, short direction) { - RcsSetLocationMessage setLoc = new RcsSetLocationMessage(amrMessageHandler.getNewSeqNo()); - setLoc.X = (short) x; - setLoc.Y = (short) y; - setLoc.Direction = direction; - - amrMessageHandler.sendCmdSetLocation(this.getId(), setLoc); - } - - public void setControlMode(ControlMode mode) { - // 硬件控制模式设置逻辑 - this.isManualMode = (mode == ControlMode.MANUAL); - - if (mode != ControlMode.FULL_AUTO && planTaskSequence != null) { - cancelTask(); - } - - // 更新Redis状态 - updateRedisStatus(); - } - - @SneakyThrows - public void requestCurrentStatus() { - amrMessageHandler.sendCmdQueryStatus(this.getId()); - } - - public boolean isFree() { - // return (this.logisticsRuntime.isRunning() && this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning()); - if (!this.runtime.isRunning()) { - return false; - } - if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) { - return false; - } - if (!deviceTaskQueue.isEmpty()) { - return false; - } - if (this.isPaused) { - return false; - } - return true; - } - - public void taskCompleted(int logicX, int logicY, short direction, int taskStatus) { - - updatePosition(logicX, logicY, direction); - // 查找当前分组任务 - for (PtrAgvDeviceTask task : runningDeviceTaskList) { - task.taskGroupStatus = taskStatus; - if (taskStatus == 4) { - // 更新计划任务 - List planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList(); - for (RcsTaskPlan planTask : planTaskList) { - planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString()); - planTaskSequence.savePlanTask(planTask); - } - } - } - - if (planTaskSequence != null && planTaskSequence.isAllCompleted()) { - fireEvent(AgvEventType.PLAN_COMPLETE, this); - this.runningDeviceTaskList.clear(); - planTaskSequence = null; - } - LockSupport.unpark(connectorThread); - } - - public void updatePosition(int logicX, int logicY, short direction) { - int oldX = this.logicX; - int oldY = this.logicY; - short oldDirection = this.direction; - - this.logicX = logicX; - this.logicY = logicY; - this.direction = direction; - - // 更新Redis - updateRedisStatus(); - - // 触发位置变化事件 - if (oldX != logicX || oldY != logicY) { - fireEvent(AgvEventType.POS_CHANGED, this, - new PosDirection(oldX, oldY, oldDirection), - new PosDirection(logicX, logicY, direction)); - } - - if (oldDirection != direction) { - fireEvent(AgvEventType.DIRECTION_CHANGED, this, - oldDirection, direction); - } - - boolean needCompute = false; - - // 从 runningDeviceTaskList 里面，找到完成到什么阶段 - // 比如 (1,2) -> (2,2) -> (3,2) , 如果 updatePosition=3,2 ，那么前2个任务都要完成 - int finishTargetIndex = -1; - if (this.runningDeviceTaskList != null && !this.runningDeviceTaskList.isEmpty() && - this.planTaskSequence != null && !this.planTaskSequence.isEmpty()) { - - for (int i = 0; i < runningDeviceTaskList.size(); i++) { - PtrAgvDeviceTask task = runningDeviceTaskList.get(i); - if (task.checkLogicX == logicX && task.checkLogicY == logicY && task.taskStatus < 4) { - finishTargetIndex = i; - break; - } - } - - if (finishTargetIndex >= 0) { - needCompute = true; - // 标记前面的任务都完成了 - for (int i = 0; i <= finishTargetIndex; i++) { - PtrAgvDeviceTask task = runningDeviceTaskList.get(i); - task.taskStatus = 4; // 标记为完成 - fireEvent(AgvEventType.DEVICE_TASK_COMPLETE, this, task); - - // 更新计划任务 - RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId); - if (planTask != null) { - planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString()); - planTaskSequence.savePlanTask(planTask); - } - } - - if (planTaskSequence.isAllCompleted()) { - fireEvent(AgvEventType.PLAN_COMPLETE, this); - this.runningDeviceTaskList.clear(); - planTaskSequence = null; - } - } - } - - BannerUtils.printConfig(log, "updatePosition", new String[]{ - "logicX: " + logicX, - "logicY: " + logicY, - "direction: " + direction, - "finishTargetIndex: " + finishTargetIndex, - "runningDeviceSize:" + (this.runningDeviceTaskList == null ? "null" : this.runningDeviceTaskList.size()), - "planTask:" + (this.planTaskSequence == null ? "null" : - ("\n" + Joiner.on("\n").join((List) this.planTaskSequence.toPrettyMap().get("items"))) - ) - }); - - if (needCompute && this.runningDeviceTaskList.size() > 0) { - int index = this.runningDeviceTaskList.size() - 1; - PtrAgvDeviceTask task = this.runningDeviceTaskList.get(index); - if (task.groupEndPoint != task.endPoint) { - LockSupport.unpark(connectorThread); - } - } - } - - /** - * 更新设备任务状态 暂时没有处理任务取消相关的状态 - * - * @param seqNo - * @param x - * @param y - * @param messageStatus - */ - public void updateDeviceTaskStatus(int seqNo, int x, int y, int messageStatus) { - // 更新任务状态逻辑 - if (messageStatus < 2) { - return; - } - - // 任务完成逻辑，在地标检查里 - } - - public void updateTaskMode(int taskMode) { - - } - - public void updateRedisStatus() { - String statusKey = getRedisKey("status"); - Map statusMap = new HashMap<>(); - statusMap.put("x", String.valueOf(x)); - statusMap.put("y", String.valueOf(y)); - statusMap.put("z", String.valueOf(z)); - statusMap.put("logicX", String.valueOf(logicX)); - statusMap.put("logicY", String.valueOf(logicY)); - statusMap.put("direction", String.valueOf(direction)); - statusMap.put("orientation", String.valueOf(orientation)); - statusMap.put("soc", this.battery == null ? "-1" : String.valueOf(this.battery.SOC)); - statusMap.put("mode", isManualMode ? "MANUAL" : "AUTO"); - statusMap.put("taskStatus", getTaskStatus()); - - redis.hPutAll(statusKey, statusMap); - redis.kExpire(statusKey, 10); // 10秒过期 - } - - public void handleHeartbeat(AmrHeartbeatMessage heartbeat) { - // 更新在线状态 - String aliveKey = getRedisKey("alive"); - redis.vSet(aliveKey, "1"); - redis.kExpire(aliveKey, 5); // 5秒过期 - - // 更新状态信息 - if (this.battery == null) { - this.battery = new CurBatteryData(); - } - this.battery.SOC = heartbeat.Battery; - this.battery.setTemperature(heartbeat.Temperature.Battery); - - // 检查低电量 - if (this.battery.SOC < 20) { - fireEvent(AgvEventType.LOW_BATTERY, this); - } - - updateRedisStatus(); - } - - private void handleOnlineEvent() { - isOnline = true; - fireEvent(AgvEventType.ONLINE, this); - requestCurrentStatus(); - } - - private void handleOfflineEvent() { - isOnline = false; - fireEvent(AgvEventType.OFFLINE, this); - } - - // 事件监听管理 - public void addEventListener(AgvEventListener listener) { - eventListeners.add(listener); - } - - public void removeEventListener(AgvEventListener listener) { - eventListeners.remove(listener); - } - - private void fireEvent(AgvEventType type, Object... args) { - for (AgvEventListener listener : eventListeners) { - listener.onEvent(type, args); - } - } - - private String getTaskStatus() { - if (planTaskSequence == null) return "IDLE"; - if (isPaused) return "PAUSED"; - return "EXECUTING"; - } - - - /** - * 启动连接器线程 - */ - public void startConnector() { - if (!connectorThread.isRunning()) { - connectorThread.start(); - System.out.println("Connector started for executor: " + this.getId()); - } - } - - /** - * 停止连接器线程 - */ - public void stopConnector() { - connectorThread.stop(); - System.out.println("Connector stopped for executor: " + this.getId()); - } - - private static final int speed = 1000; - - /** - * 添加任务序列到当前执行器 - */ - protected void buildPlanToDeviceTask() { - PlanTaskSequence sequence = this.planTaskSequence; - LogisticsRuntime runtime = sequence.logisticsRuntime; - - short direction = this.direction; - - // 获取当前设备点位(逻辑点位) - StaticItem startPoint = runtime.getStaticItemByLogicXY(this.logicX, this.logicY); - if (startPoint == null) { - log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, agv当前点位为空 地图上没有标记", this.getId(), sequence.bizTask.getBizTaskId()); - } - StaticItem groupStartPoint = startPoint; - - Set rotationPlanTaskIdSet = new HashSet<>(); - - // 生成移动报文 - List deviceTaskList = new ArrayList<>(); - List> linkStore = null; - // 检查 planList 是不是全都是我的任务 - for (int i = 0; i < sequence.taskList.size(); i++) { - RcsTaskPlan plan = sequence.taskList.get(i); - String endPointId = plan.getTargetId(); - - if (plan.getPlanType().equals(PlanTaskType.MOVE.toString()) || plan.getPlanType().equals(PlanTaskType.MOVE_BACKWARD.toString())) { - // 获取目标点信息 - StaticItem pointItem = runtime.getStaticItemById(endPointId); - linkStore = (List>) pointItem.dt.get("linkStore"); - int d = -1; - if (startPoint.logicX == pointItem.logicX && startPoint.logicY != pointItem.logicY) { - d = pointItem.logicY >= startPoint.logicY ? CDirection.db : CDirection.dt; - if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) { - throw new RuntimeException("方向错误"); - } - - } 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("方向错误"); - } - // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX()); - - } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX == pointItem.logicX) { - d = direction; - // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX()); - - } else { - throw new RuntimeException("无法识别的点位关系"); - } - PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); - deviceTask.x = pointItem.logicX; - deviceTask.y = pointItem.logicY; - deviceTask.speed = d == direction ? (speed) : (-speed); - deviceTask.direction = direction; - deviceTask.pickMode = 0; - deviceTask.startPoint = startPoint; - deviceTask.endPoint = pointItem; - deviceTask.bizTaskId = plan.getBizTaskId(); - deviceTask.movePlanTaskId = plan.getPlanTaskId(); - deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); - rotationPlanTaskIdSet.clear(); - // 行走任务完成后，检查用的字段 - deviceTask.checkLogicX = pointItem.logicX; - deviceTask.checkLogicY = pointItem.logicY; - deviceTaskList.add(deviceTask); - // 设置新的起点 - startPoint = pointItem; - - } else if (plan.getPlanType().equals(PlanTaskType.ROTATION.toString())) { - - float r = plan.getTargetRotation().floatValue(); - while (r > 360) { - r -= 360; - } - while (r < 0) { - r += 360; - } - - if (r >= 315 || r < 45) { - direction = CDirection.dr; - - } else if (r >= 45 && r < 135) { - direction = CDirection.dt; - - } else if (r >= 135 && r < 225) { - direction = CDirection.dl; - - } else if (r >= 225 && r < 315) { - direction = CDirection.db; - } - rotationPlanTaskIdSet.add(plan.getPlanTaskId()); - - } else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) { - - if (deviceTaskList.isEmpty()) { - PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); - deviceTask.x = startPoint.logicX; - deviceTask.y = startPoint.logicY; - deviceTask.speed = speed; - deviceTask.direction = direction; - deviceTask.pickMode = 0; - deviceTask.startPoint = startPoint; - deviceTask.endPoint = startPoint; - deviceTask.bizTaskId = plan.getBizTaskId(); - deviceTask.movePlanTaskId = plan.getPlanTaskId(); - deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); - rotationPlanTaskIdSet.clear(); - // 行走任务完成后，检查用的字段 - deviceTask.checkLogicX = startPoint.logicX; - deviceTask.checkLogicY = startPoint.logicY; - deviceTaskList.add(deviceTask); - linkStore = (List>) startPoint.dt.get("linkStore"); - } - PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); - deviceTask.operationType = COperationType.transplantLoadAndUnload; - deviceTask.pickMode = CPickMode.load; - deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); - //处理取货高度 - StaticItem storeItem = runtime.getStaticItemById(endPointId); - Map storeItemRaw = storeItem.dt; - if (storeItemRaw.containsKey("bays")) { - List> bays = (List>) storeItemRaw.get("bays"); - Map bay = bays.get(plan.getTargetBay()); - List levelHeight = (List) bay.get("levelHeight"); - deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000); - } else { - deviceTask.goodsSlotHeight = 1; - } - if (linkStore != null) { - for (Map store : linkStore) { - if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) { - short d = 0; - switch (store.get("direction").toString()) { - case "up": - d = 1; - break; - case "right": - d = 2; - break; - case "down": - d = 3; - break; - case "left": - d = 0; - break; - } - deviceTask.goodsSlotDirection = d; - } - } - } - - // 标记任务分组结束 - deviceTask.isGroupEnd = true; - deviceTask.groupEndPoint = deviceTask.endPoint; - deviceTask.groupStartPoint = groupStartPoint; - groupStartPoint = deviceTask.endPoint; - - } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) { - if (deviceTaskList.isEmpty()) { - PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); - deviceTask.x = startPoint.logicX; - deviceTask.y = startPoint.logicY; - deviceTask.speed = speed; - deviceTask.direction = direction; - deviceTask.pickMode = 0; - deviceTask.startPoint = startPoint; - deviceTask.endPoint = startPoint; - deviceTask.bizTaskId = plan.getBizTaskId(); - deviceTask.movePlanTaskId = plan.getPlanTaskId(); - deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); - rotationPlanTaskIdSet.clear(); - // 行走任务完成后，检查用的字段 - deviceTask.checkLogicX = startPoint.logicX; - deviceTask.checkLogicY = startPoint.logicY; - deviceTaskList.add(deviceTask); - linkStore = (List>) startPoint.dt.get("linkStore"); - } - PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); - deviceTask.operationType = COperationType.transplantLoadAndUnload; - deviceTask.pickMode = CPickMode.unload; - deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); - // 处理卸货高度 - StaticItem storeItem = runtime.getStaticItemById(endPointId); - Map storeItemRaw = storeItem.dt; - if (storeItemRaw.containsKey("bays")) { - List> bays = (List>) storeItemRaw.get("bays"); - Map bay = bays.get(plan.getTargetBay()); - List levelHeight = (List) bay.get("levelHeight"); - deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000); - } else { - deviceTask.goodsSlotHeight = 1; - } - if (linkStore != null) { - for (Map store : linkStore) { - if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) { - short d = 0; - switch (store.get("direction").toString()) { - case "up": - d = 1; - break; - case "right": - d = 2; - break; - case "down": - d = 3; - break; - case "left": - d = 0; - break; - } - deviceTask.goodsSlotDirection = d; - } - } - } - // 标记任务分组结束 - deviceTask.isGroupEnd = true; - deviceTask.groupEndPoint = deviceTask.endPoint; - deviceTask.groupStartPoint = groupStartPoint; - groupStartPoint = deviceTask.endPoint; - - } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) { - if (deviceTaskList.isEmpty()) { - PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask(); - deviceTask.x = startPoint.logicX; - deviceTask.y = startPoint.logicY; - deviceTask.speed = speed; - deviceTask.direction = direction; - deviceTask.pickMode = 0; - deviceTask.startPoint = startPoint; - deviceTask.endPoint = startPoint; - deviceTask.bizTaskId = plan.getBizTaskId(); - deviceTask.movePlanTaskId = plan.getPlanTaskId(); - deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); - rotationPlanTaskIdSet.clear(); - // 行走任务完成后，检查用的字段 - deviceTask.checkLogicX = startPoint.logicX; - deviceTask.checkLogicY = startPoint.logicY; - deviceTaskList.add(deviceTask); - } - PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); - deviceTask.operationType = COperationType.charge; - deviceTask.planTaskIdSet.add(plan.getPlanTaskId()); - // 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm) - deviceTask.chargeDirection = 0; - deviceTask.chargeLocation = 200; - // 标记任务分组结束 - deviceTask.isGroupEnd = true; - deviceTask.groupEndPoint = deviceTask.endPoint; - deviceTask.groupStartPoint = groupStartPoint; - groupStartPoint = deviceTask.endPoint; - } - - if (!plan.getExecutorId().equals(this.getId())) { - throw new RuntimeException("plan not belong executor:" + this.getId() + ", " + plan.getExecutorId()); - } - - } - - if (deviceTaskList.size() <= 0) { - return; - } - - // 标记任务分组结束 - PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1); - deviceTask.groupEndPoint = deviceTask.endPoint; - deviceTask.groupStartPoint = groupStartPoint; - deviceTask.isGroupEnd = true; - // 最后一个规划任务为旋转时需要添加一个endDirection - if (rotationPlanTaskIdSet.size() > 0) { - deviceTask.endDirection = direction; - deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet); - rotationPlanTaskIdSet.clear(); - } - - // 反向标记任务组 - int lastIndex = deviceTaskList.size() - 1; - for (int i = deviceTaskList.size() - 1; i >= 0 ; i--) { - PtrAgvDeviceTask d = deviceTaskList.get(i); - if (d.isGroupEnd) { - lastIndex = i; - } else { - d.operationType = deviceTaskList.get(lastIndex).operationType; - d.pickMode = deviceTaskList.get(lastIndex).pickMode; - d.groupStartPoint = deviceTaskList.get(lastIndex).groupStartPoint; - d.groupEndPoint = deviceTaskList.get(lastIndex).groupEndPoint; - d.goodsSlotHeight = deviceTaskList.get(lastIndex).goodsSlotHeight; - d.goodsSlotDirection = deviceTaskList.get(lastIndex).goodsSlotDirection; - d.chargeDirection = deviceTaskList.get(lastIndex).chargeDirection; - d.chargeLocation = deviceTaskList.get(lastIndex).chargeLocation; - } - } - - -// planQueue.addAll(sequence.taskList); - deviceTaskQueue.addAll(deviceTaskList); - - String json = JsonWrapper.toJson(deviceTaskList); - log.info("deviceTaskList: {}", json); - - // TODO: 开启轮询线程，等待下一个待执行任务 - } - - public boolean isSamePosition(PosDirection startPos) { - return this.logicX == startPos.logicX() && this.logicY == startPos.logicY() && this.direction == startPos.direction(); - } - - private static class CDirection { - private static final short dr = 0; - private static final short db = 1; - private static final short dl = 2; - private static final short dt = 3; - } - - - private static class COperationType { - public static final short move = 0; - public static final short load = 1; - public static final short unpick = 2; - public static final short charge = 3; - public static final short transplantLoadAndUnload = 4; - public static final short rollerLoadAndUnload = 5; - } - - private static class CPickMode { - public static final short normal = 0; - public static final short load = 1; - public static final short unload = 2; - public static final short adjustHeight = 3; - public static final short adjustHeightToLoad = 5; - public static final short adjustHeightToUnload = 6; - } - - - /** - * 从 AMR 方向转换为 LCC 方向枚举 - * - * @return - */ - public LCCDirection getLCCDirection() { - return switch (direction) { - case 0 -> LCCDirection.RIGHT; - case 1 -> LCCDirection.DOWN; - case 2 -> LCCDirection.LEFT; - case 3 -> LCCDirection.UP; - default -> null; - }; - } - - public short getAmrDirection(LCCDirection lccDirection) { - return switch (lccDirection) { - case RIGHT -> 0; - case DOWN -> 1; - case LEFT -> 2; - case UP -> 3; - default -> -1; // 未知方向 - }; - } - - private String getRedisKey(String type) { - return String.format("lcc:%s:%s:rcs:%s_%s", - runtime.projectUUID, runtime.envId, type, this.getId()); - } -} diff --git a/servo/src/main/java/com/yvan/workbench/controller/RcsController.java b/servo/src/main/java/com/yvan/workbench/controller/RcsController.java index de9444d..a18fce1 100644 --- a/servo/src/main/java/com/yvan/workbench/controller/RcsController.java +++ b/servo/src/main/java/com/yvan/workbench/controller/RcsController.java @@ -1,15 +1,12 @@ package com.yvan.workbench.controller; -import com.galaxis.rcs.RCSService; -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.common.entity.*; +import com.galaxis.rcs.common.enums.*; import com.galaxis.rcs.plan.PlanTaskSequence; -import com.galaxis.rcs.plan.task.CarryTask; -import com.galaxis.rcs.plan.task.MoveTask; +import com.galaxis.rcs.plan.task.*; +import com.galaxis.rcs.ptr.PtrAgvItem; import com.google.common.base.Strings; +import com.google.common.collect.Maps; import com.yvan.logisticsModel.*; import com.yvan.workbench.model.entity.Model; import org.apache.commons.lang3.NotImplementedException; @@ -20,118 +17,48 @@ import org.clever.web.mvc.annotation.RequestBody; import java.util.Map; public class RcsController { -// /** -// * 后台机器人移动 -// * @param agvId 机器人ID -// * @param targetWayPointId 目标路径点ID -// * @param option 选项 -// */ -// agvMove(agvId: string, targetWayPointId: string, option: any = {}): Promise> -// -// /** -// * 后台机器人搬运（库存点 -> 库存点） -// * @param agvId 机器人ID -// * @param fromStoreLoc 库存点ID -// * @param targetStoreLoc 目标库存点ID -// * @param option 其他选项 -// */ -// agvCarry(agvId: string, fromStoreLoc: string, targetStoreLoc: string, option: any = {}): Promise> -// -// /** -// * 机器人充电 -// * @param agvId 机器人ID -// * @param chargerId 充电桩ID -// * @param option 其他选项 -// */ -// agvToCharger(agvId: string, chargerId: string, option: any = {}): Promise> - static final SnowFlake snowFlake = new SnowFlake(); public static Model agvMove(@RequestBody Map params) { - String projectUUID = Conv.asString(params.get("projectUUID")); - String catalogCode = Conv.asString(params.get("catalogCode")); - Long envId = Conv.asLong(params.get("envId")); - String agvId = Conv.asString(params.get("agvId")); - String targetWayPointId = Conv.asString(params.get("targetWayPointId")); - Map option = (Map) params.get("option"); - - if (Strings.isNullOrEmpty(agvId)) { - return Model.newFail("agvId Must not be empty"); + Object ret = getCommonParamAndCreateBizTask(params); + if (ret instanceof Model) { + // 异常 + return (Model) ret; } + RcsCommonParam ps = (RcsCommonParam) ret; + + String targetWayPointId = Conv.asString(params.get("targetWayPointId")); if (Strings.isNullOrEmpty(targetWayPointId)) { return Model.newFail("targetWayPointId Must not be empty"); } + String targetDirection = Conv.asString(params.get("targetDirection")); + if (Strings.isNullOrEmpty(targetDirection)) { + targetDirection = ps.fromDirection.toString(); + } - LogisticsRuntime runtime = LogisticsRuntimeService.INSTANCE.getByProjectEnv(projectUUID, envId); - StaticItem toItem = runtime.getStaticItemById(targetWayPointId); + StaticItem toItem = ps.runtime.getStaticItemById(targetWayPointId); if (toItem == null) { return Model.newFail("target wayPoint not found!"); } - ExecutorItem executorItem = runtime.executorItemMap.get(agvId); - if (executorItem == null) { - return Model.newFail("executor not found: " + agvId); - } - if (!(executorItem instanceof PtrAgvItem)) { - return Model.newFail("executor is not a PtrAgvItem id=" + agvId); - } - - // 获取机器人当前所在位置, 也可以前端强制指定 - // forceStartWayPointId: '6_2', forceStartDirectior: 'right' - StaticItem fromItem = null; - LCCDirection fromDirection = null; - if (option.get("forceStartWayPointId") != null) { - fromItem = runtime.getStaticItemById(Conv.asString(option.get("forceStartWayPointId"))); - } else { - fromItem = runtime.getStaticItemByLogicXY(((PtrAgvItem) executorItem).logicX, ((PtrAgvItem) executorItem).logicY); - } - if (option.get("forceStartDirection") != null) { - fromDirection = LCCDirection.fromString(Conv.asString(option.get("forceStartDirection"))); - } else { - fromDirection = ((PtrAgvItem) executorItem).getLCCDirection(); - } - - if (fromItem == null) { - return Model.newFail("PtrAgvItem not found at current location: " + ((PtrAgvItem) executorItem).logicX + "_" + ((PtrAgvItem) executorItem).logicY + ", id=" + agvId); - } - if (fromDirection == null) { - return Model.newFail("PtrAgvItem unkown direction id=" + agvId); - } - - RcsTaskBiz bizTask = new RcsTaskBiz(); - bizTask.setBizTaskId(snowFlake.nextId()); - bizTask.setEnvId(envId); - bizTask.setBizType(BizTaskType.MOVE.toString()); - bizTask.setLpn("N/A"); - bizTask.setPriority(Conv.asInteger(option.get("priority"), 1)); - bizTask.setTaskFrom(fromItem.getId()); - bizTask.setTaskTo(targetWayPointId); - bizTask.setAllocatedExecutorId(agvId); - bizTask.setBizTaskPayload("N/A"); - bizTask.setBizTaskErrorInfo("N/A"); - bizTask.setBizTaskDescription("N/A"); - bizTask.setBizTaskStatus(BizTaskStatus.WAITING_FOR_DISPATCH.toString()); - - PlanTaskSequence planSequence = new PlanTaskSequence(agvId, runtime, bizTask, "demo"); + ps.bizTask.setTaskTo(targetWayPointId); MoveTask moveTask = new MoveTask( - agvId, bizTask.getTaskTo(), bizTask.getPriority() + ps.agvId, ps.bizTask.getTaskTo(), LCCDirection.fromString(targetDirection, ps.fromDirection), ps.bizTask.getPriority() ); - runtime.pathPlannerMap.get(executorItem.getT()) - .planMoveTask(planSequence, fromItem.getId(), fromDirection, moveTask); + ps.runtime.pathPlannerMap.get(ps.agv.getT()) + .planMoveTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, moveTask); - PtrAgvItem agvItem = (PtrAgvItem) executorItem; - agvItem.logicX = fromItem.logicX; - agvItem.logicY = fromItem.logicY; - agvItem.dispatchTask(planSequence); + ps.agv.logicX = ps.fromItem.logicX; + ps.agv.logicY = ps.fromItem.logicY; + ps.agv.dispatchTask(ps.planSequence); - return Model.newSuccess(planSequence.toPrettyMap()); + return Model.newSuccess(ps.planSequence.toPrettyMap()); } public static Model agvInfo(@RequestBody Map params) { String projectUUID = Conv.asString(params.get("projectUUID")); - String catalogCode = Conv.asString(params.get("catalogCode")); Long envId = Conv.asLong(params.get("envId")); String agvId = Conv.asString(params.get("agvId")); @@ -152,18 +79,111 @@ public class RcsController { return Model.newSuccess(executorItem); } + public static Model agvUnload(@RequestBody Map params) { + Object ret = getCommonParamAndCreateBizTask(params); + if (ret instanceof Model) { + // 异常 + return (Model) ret; + } + RcsCommonParam ps = (RcsCommonParam) ret; + + String targetStoreLoc = Conv.asString(params.get("targetStoreLoc")); + if (Strings.isNullOrEmpty(targetStoreLoc)) { + return Model.newFail("targetStoreLoc Must not be empty"); + } + StoreLocation targetLocation = StoreLocation.of(targetStoreLoc, "/"); + + UnloadTask unloadTask = new UnloadTask( + ps.agvId, ps.bizTask.getPriority(), + targetLocation + ); + + ps.runtime.pathPlannerMap.get(ps.agv.getT()) + .planUnloadTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, unloadTask); + + ps.agv.logicX = ps.fromItem.logicX; + ps.agv.logicY = ps.fromItem.logicY; + ps.agv.dispatchTask(ps.planSequence); + + return Model.newSuccess(ps.planSequence.toPrettyMap()); + } + + public static Model agvLoad(@RequestBody Map params) { + Object ret = getCommonParamAndCreateBizTask(params); + if (ret instanceof Model) { + // 异常 + return (Model) ret; + } + RcsCommonParam ps = (RcsCommonParam) ret; + + String targetStoreLoc = Conv.asString(params.get("targetStoreLoc")); + if (Strings.isNullOrEmpty(targetStoreLoc)) { + return Model.newFail("targetStoreLoc Must not be empty"); + } + StoreLocation targetLocation = StoreLocation.of(targetStoreLoc, "/"); + + LoadTask loadTask = new LoadTask( + ps.agvId, ps.bizTask.getPriority(), + targetLocation + ); + + ps.runtime.pathPlannerMap.get(ps.agv.getT()) + .planLoadTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, loadTask); + + ps.agv.logicX = ps.fromItem.logicX; + ps.agv.logicY = ps.fromItem.logicY; + ps.agv.dispatchTask(ps.planSequence); + + return Model.newSuccess(ps.planSequence.toPrettyMap()); + } + + public static Model cancelTasks(@RequestBody Map params) { + Object ret = getCommonParamAndCreateBizTask(params); + if (ret instanceof Model) { + // 异常 + return (Model) ret; + } + RcsCommonParam ps = (RcsCommonParam) ret; + + ps.agv.cancelTask(); + return Model.newSuccess("AGV tasks cancelled successfully"); + } + + public static Model waitTaskFinish(@RequestBody Map params) { + Object ret = getCommonParamAndCreateBizTask(params); + if (ret instanceof Model) { + // 异常 + return (Model) ret; + } + RcsCommonParam ps = (RcsCommonParam) ret; + + var now = System.currentTimeMillis(); + + // 循环等待 ps.agv.planTaskSequence 清空或没值 + while (ps.agv.planTaskSequence != null && !ps.agv.planTaskSequence.isEmpty()) { + try { + Thread.sleep(100); // 等待100毫秒 + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); // 恢复中断状态 + return Model.newFail("Interrupted while waiting for task to finish"); + } + } + + var cost = System.currentTimeMillis() - now; + return Model.newSuccess(cost); + } + public static Model agvCarry(@RequestBody Map params) { - String projectUUID = Conv.asString(params.get("projectUUID")); - String catalogCode = Conv.asString(params.get("catalogCode")); - Long envId = Conv.asLong(params.get("envId")); - String agvId = Conv.asString(params.get("agvId")); + Object ret = getCommonParamAndCreateBizTask(params); + if (ret instanceof Model) { + // 异常 + return (Model) ret; + } + RcsCommonParam ps = (RcsCommonParam) ret; + String fromStoreLoc = Conv.asString(params.get("fromStoreLoc")); String targetStoreLoc = Conv.asString(params.get("targetStoreLoc")); - Map option = (Map) params.get("option"); - if (Strings.isNullOrEmpty(agvId)) { - return Model.newFail("agvId Must not be empty"); - } if (Strings.isNullOrEmpty(fromStoreLoc)) { return Model.newFail("fromStoreLoc Must not be empty"); } @@ -171,25 +191,67 @@ public class RcsController { return Model.newFail("targetStoreLoc Must not be empty"); } - LogisticsRuntime runtime = LogisticsRuntimeService.INSTANCE.getByProjectEnv(projectUUID, envId); - StoreLocation sourceLocation = StoreLocation.of(fromStoreLoc, "/"); StoreLocation targetLocation = StoreLocation.of(targetStoreLoc, "/"); - StaticItem sourceItem = runtime.getStaticItemById(sourceLocation.rackId()); + StaticItem sourceItem = ps.runtime.getStaticItemById(sourceLocation.rackId()); if (sourceItem == null) { return Model.newFail("fromStoreLoc storePoint not found!"); } - StaticItem targetItem = runtime.getStaticItemById(targetLocation.rackId()); + StaticItem targetItem = ps.runtime.getStaticItemById(targetLocation.rackId()); if (targetItem == null) { return Model.newFail("targetStoreLoc storePoint not found!"); } + ps.bizTask.setTaskFrom(fromStoreLoc); + ps.bizTask.setTaskTo(targetStoreLoc); + + CarryTask carryTask = new CarryTask( + ps.agvId, "N/A", ps.bizTask.getPriority(), + sourceLocation, + targetLocation + ); + + ps.runtime.pathPlannerMap.get(ps.agv.getT()) + .planCarryTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, carryTask); + + ps.agv.logicX = ps.fromItem.logicX; + ps.agv.logicY = ps.fromItem.logicY; + ps.agv.dispatchTask(ps.planSequence); + + return Model.newSuccess(ps.planSequence.toPrettyMap()); + } + + public static Model agvToCharger(@RequestBody Map params) { + throw new NotImplementedException("agvToCharger not implemented yet"); + } + + public static Object getCommonParamAndCreateBizTask(@RequestBody Map params) { + String projectUUID = Conv.asString(params.get("projectUUID")); + Long envId = Conv.asLong(params.get("envId")); + String agvId = Conv.asString(params.get("agvId")); + Map option = (Map) params.get("option"); + + if (Strings.isNullOrEmpty(projectUUID)) { + return Model.newFail("projectUUID Must not be empty"); + } + if (envId == null || envId < 0) { + return Model.newFail("envId Must not be empty"); + } + if (Strings.isNullOrEmpty(agvId)) { + return Model.newFail("agvId Must not be empty"); + } + if (option == null) { + option = Maps.newHashMap(); + } + + LogisticsRuntime runtime = LogisticsRuntimeService.INSTANCE.getByProjectEnv(projectUUID, envId); + ExecutorItem executorItem = runtime.executorItemMap.get(agvId); if (executorItem == null) { return Model.newFail("executor not found: " + agvId); } - if (!(executorItem instanceof PtrAgvItem)) { + if (!(executorItem instanceof PtrAgvItem agv)) { return Model.newFail("executor is not a PtrAgvItem id=" + agvId); } @@ -200,16 +262,16 @@ public class RcsController { if (option.get("forceStartWayPointId") != null) { fromItem = runtime.getStaticItemById(Conv.asString(option.get("forceStartWayPointId"))); } else { - fromItem = runtime.getStaticItemByLogicXY(((PtrAgvItem) executorItem).logicX, ((PtrAgvItem) executorItem).logicY); + fromItem = runtime.getStaticItemByLogicXY(agv.logicX, agv.logicY); } if (option.get("forceStartDirection") != null) { fromDirection = LCCDirection.fromString(Conv.asString(option.get("forceStartDirection"))); } else { - fromDirection = ((PtrAgvItem) executorItem).getLCCDirection(); + fromDirection = agv.getLCCDirection(); } if (fromItem == null) { - return Model.newFail("PtrAgvItem not found at current location: " + ((PtrAgvItem) executorItem).logicX + "_" + ((PtrAgvItem) executorItem).logicY + ", id=" + agvId); + return Model.newFail("PtrAgvItem not found at current location: " + agv.logicX + "_" + agv.logicY + ", id=" + agvId); } if (fromDirection == null) { return Model.newFail("PtrAgvItem unkown direction id=" + agvId); @@ -217,38 +279,31 @@ public class RcsController { RcsTaskBiz bizTask = new RcsTaskBiz(); bizTask.setBizTaskId(snowFlake.nextId()); - bizTask.setEnvId(1L); + bizTask.setEnvId(envId); bizTask.setBizType(BizTaskType.MOVE.toString()); bizTask.setLpn("N/A"); bizTask.setPriority(Conv.asInteger(option.get("priority"), 1)); - bizTask.setTaskFrom(fromStoreLoc); - bizTask.setTaskTo(targetStoreLoc); + bizTask.setTaskFrom(fromItem.getId()); bizTask.setAllocatedExecutorId(agvId); bizTask.setBizTaskPayload("N/A"); bizTask.setBizTaskErrorInfo("N/A"); bizTask.setBizTaskDescription("N/A"); bizTask.setBizTaskStatus(BizTaskStatus.WAITING_FOR_DISPATCH.toString()); - PlanTaskSequence planSequence = new PlanTaskSequence(agvId, runtime, bizTask, "demo"); - - CarryTask carryTask = new CarryTask( - agvId, "dummy", 1, - sourceLocation, - targetLocation - ); - - runtime.pathPlannerMap.get(executorItem.getT()) - .planCarryTask(planSequence, fromItem.getId(), fromDirection, carryTask); - - PtrAgvItem agvItem = (PtrAgvItem) executorItem; - agvItem.logicX = fromItem.logicX; - agvItem.logicY = fromItem.logicY; - agvItem.dispatchTask(planSequence); - - return Model.newSuccess(planSequence.toPrettyMap()); + PlanTaskSequence planSequence = new PlanTaskSequence(agvId, runtime, bizTask, "RcsScript"); + return new RcsCommonParam(projectUUID, envId, agvId, option, bizTask, runtime, agv, planSequence, fromItem, fromDirection); } - public static Model agvToCharger(@RequestBody Map params) { - throw new NotImplementedException("agvToCharger not implemented yet"); + public record RcsCommonParam( + String projectUUID, + Long envId, + String agvId, + Map option, + RcsTaskBiz bizTask, + LogisticsRuntime runtime, + PtrAgvItem agv, + PlanTaskSequence planSequence, + StaticItem fromItem, + LCCDirection fromDirection) { } } diff --git a/servo/src/main/java/com/yvan/workbench/model/entity/Model.java b/servo/src/main/java/com/yvan/workbench/model/entity/Model.java index d8ec861..ea17414 100644 --- a/servo/src/main/java/com/yvan/workbench/model/entity/Model.java +++ b/servo/src/main/java/com/yvan/workbench/model/entity/Model.java @@ -1,5 +1,7 @@ package com.yvan.workbench.model.entity; +import com.fasterxml.jackson.annotation.JsonInclude; + public class Model implements java.io.Serializable { private boolean success = false; private T data; @@ -43,4 +45,15 @@ public class Model implements java.io.Serializable { this.success = success; return this; } + + @JsonInclude(JsonInclude.Include.NON_NULL) + private Throwable exception; + + public Throwable getException() { + return exception; + } + + public void setException(Throwable exception) { + this.exception = exception; + } }