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/path2/Node.java b/servo/src/main/java/com/galaxis/rcs/plan/path/Node.java similarity index 92% rename from servo/src/main/java/com/galaxis/rcs/plan/path2/Node.java rename to servo/src/main/java/com/galaxis/rcs/plan/path/Node.java index 252bb11..6b865bf 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/Node.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/Node.java @@ -1,4 +1,4 @@ -package com.galaxis.rcs.plan.path2; +package com.galaxis.rcs.plan.path; import java.util.List; 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/path2/PathUtils.java b/servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java similarity index 99% rename from servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java rename to servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java index cfffe83..439cd4a 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java @@ -1,4 +1,4 @@ -package com.galaxis.rcs.plan.path2; +package com.galaxis.rcs.plan.path; import com.galaxis.rcs.common.enums.LCCDirection; 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/path2/State.java b/servo/src/main/java/com/galaxis/rcs/plan/path/State.java similarity index 91% rename from servo/src/main/java/com/galaxis/rcs/plan/path2/State.java rename to servo/src/main/java/com/galaxis/rcs/plan/path/State.java index bd7f3d7..bc9a087 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/State.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/State.java @@ -1,4 +1,4 @@ -package com.galaxis.rcs.plan.path2; +package com.galaxis.rcs.plan.path; import com.galaxis.rcs.common.enums.LCCDirection; diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java b/servo/src/main/java/com/galaxis/rcs/plan/path/StoreLink.java similarity index 91% rename from servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java rename to servo/src/main/java/com/galaxis/rcs/plan/path/StoreLink.java index f171d5d..d94923e 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/StoreLink.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/StoreLink.java @@ -1,4 +1,4 @@ -package com.galaxis.rcs.plan.path2; +package com.galaxis.rcs.plan.path; import com.galaxis.rcs.common.enums.LCCDirection; diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java 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/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/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/yvan/logisticsModel/PtrAgvConnectorThread.java b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnectorThread.java similarity index 98% rename from servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java rename to servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnectorThread.java index 97dd691..472c753 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java +++ b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvConnectorThread.java @@ -1,9 +1,9 @@ -package com.yvan.logisticsModel; +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.logisticsMonitor.task.DeviceTask; +import com.yvan.logisticsModel.LogisticsRuntime; import lombok.extern.slf4j.Slf4j; import org.eclipse.paho.mqttv5.common.MqttException; diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java similarity index 97% rename from servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java rename to servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java index 0b6b70f..313ad9d 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java +++ b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvDeviceTask.java @@ -1,4 +1,6 @@ -package com.yvan.logisticsModel; +package com.galaxis.rcs.ptr; + +import com.yvan.logisticsModel.StaticItem; import java.util.HashSet; import java.util.Set; diff --git a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java similarity index 98% rename from servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java rename to servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java index 2e5c610..aa1c4df 100644 --- a/servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java +++ b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java @@ -1,4 +1,4 @@ -package com.yvan.logisticsModel; +package com.galaxis.rcs.ptr; import com.fasterxml.jackson.annotation.JsonIgnore; import com.galaxis.rcs.common.entity.RcsTaskPlan; @@ -8,7 +8,6 @@ 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; @@ -16,7 +15,9 @@ 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 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; @@ -28,7 +29,6 @@ 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 @@ -83,7 +83,7 @@ public abstract class PtrAgvItem extends ExecutorItem { * 当前执行的任务规划列表 */ @JsonIgnore - private volatile PlanTaskSequence planTaskSequence; + public volatile PlanTaskSequence planTaskSequence; /** * 当前执行的设备任务列表 @@ -185,15 +185,16 @@ public abstract class PtrAgvItem extends ExecutorItem { @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(); + if (planTaskSequence != null) { + planTaskSequence = null; + } + if (!deviceTaskQueue.isEmpty()) { + deviceTaskQueue.clear(); + } + fireEvent(AgvEventType.PLAN_CANCEL, this); } @@ -740,7 +741,7 @@ public abstract class PtrAgvItem extends ExecutorItem { // 反向标记任务组 int lastIndex = deviceTaskList.size() - 1; - for (int i = deviceTaskList.size() - 1; i >= 0 ; i--) { + for (int i = deviceTaskList.size() - 1; i >= 0; i--) { PtrAgvDeviceTask d = deviceTaskList.get(i); if (d.isGroupEnd) { lastIndex = i; 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/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; + } }