From af8566a72ef80895991dcdc11f5a893d412684a5 Mon Sep 17 00:00:00 2001 From: zgw <13623675548@163.com> Date: Mon, 10 Aug 2026 16:02:37 +0800 Subject: [PATCH] feat(wcs): add deterministic route planning --- .../wcs/domain/traffic/AStarRoutePlanner.java | 256 ++++++++++++++++++ .../ruoyi/wcs/domain/traffic/RoutePlan.java | 55 ++++ .../wcs/domain/traffic/RoutePlanner.java | 10 + .../domain/traffic/AStarRoutePlannerTest.java | 83 ++++++ 4 files changed, 404 insertions(+) create mode 100644 ruoyi-wcs/src/main/java/com/ruoyi/wcs/domain/traffic/AStarRoutePlanner.java create mode 100644 ruoyi-wcs/src/main/java/com/ruoyi/wcs/domain/traffic/RoutePlan.java create mode 100644 ruoyi-wcs/src/main/java/com/ruoyi/wcs/domain/traffic/RoutePlanner.java create mode 100644 ruoyi-wcs/src/test/java/com/ruoyi/wcs/domain/traffic/AStarRoutePlannerTest.java diff --git a/ruoyi-wcs/src/main/java/com/ruoyi/wcs/domain/traffic/AStarRoutePlanner.java b/ruoyi-wcs/src/main/java/com/ruoyi/wcs/domain/traffic/AStarRoutePlanner.java new file mode 100644 index 0000000..b385747 --- /dev/null +++ b/ruoyi-wcs/src/main/java/com/ruoyi/wcs/domain/traffic/AStarRoutePlanner.java @@ -0,0 +1,256 @@ +package com.ruoyi.wcs.domain.traffic; + +import com.ruoyi.wcs.domain.shared.DomainRuleViolation; +import com.ruoyi.wcs.domain.topology.EffectiveTopology; +import com.ruoyi.wcs.domain.topology.TopologyEdge; +import com.ruoyi.wcs.domain.topology.TopologyNode; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; +import java.util.HashMap; +import java.util.HashSet; +import java.util.List; +import java.util.Map; +import java.util.Objects; +import java.util.PriorityQueue; +import java.util.Set; + +public final class AStarRoutePlanner implements RoutePlanner { + + @Override + public RoutePlan plan(EffectiveTopology topology, String sourceNodeId, + String destinationNodeId, Set blockedResources) { + Objects.requireNonNull(topology, "topology"); + String source = requireNode(topology, sourceNodeId, "source"); + String destination = requireNode(topology, destinationNodeId, "destination"); + Set blocked = blockedResources == null + ? Collections.emptySet() + : new HashSet(blockedResources); + if (!source.equals(destination) && blocked.contains("NODE:" + destination)) { + throw noRoute(source, destination); + } + + PriorityQueue open = new PriorityQueue(candidateOrder()); + Map best = new HashMap(); + Candidate start = Candidate.start(source, + heuristic(topology, source, destination)); + open.add(start); + best.put(start.stateKey(), start.score()); + + while (!open.isEmpty()) { + Candidate current = open.poll(); + if (!current.score().equals(best.get(current.stateKey()))) { + continue; + } + if (destination.equals(current.nodeId)) { + return current.toRoutePlan(topology); + } + for (String neighbor : topology.neighbors(current.nodeId)) { + TopologyEdge edge = topology.edge(current.nodeId, neighbor); + if (blocked.contains(edge.getResourceKey()) + || blocked.contains("NODE:" + neighbor)) { + continue; + } + Candidate next = current.extend(topology, edge, neighbor, + heuristic(topology, neighbor, destination)); + Score existing = best.get(next.stateKey()); + if (existing == null || next.score().compareTo(existing) < 0) { + best.put(next.stateKey(), next.score()); + open.add(next); + } + } + } + throw noRoute(source, destination); + } + + private static Comparator candidateOrder() { + return Comparator.comparingInt(Candidate::getEstimatedTotalCost) + .thenComparingInt(Candidate::getDistanceCost) + .thenComparingInt(Candidate::getTurnCount) + .thenComparing(Candidate::getNodeId) + .thenComparing(Candidate::getPathKey); + } + + private static int heuristic(EffectiveTopology topology, + String source, String destination) { + TopologyNode first = topology.node(source); + TopologyNode second = topology.node(destination); + if (first == null || second == null + || !first.getFloorCode().equals(second.getFloorCode())) { + return 0; + } + return Math.abs(first.getX() - second.getX()) + + Math.abs(first.getY() - second.getY()); + } + + private static String requireNode(EffectiveTopology topology, + String nodeId, String role) { + if (nodeId == null || nodeId.trim().isEmpty() + || !topology.containsNode(nodeId.trim())) { + throw new DomainRuleViolation("unknown " + role + " node: " + nodeId); + } + return nodeId.trim(); + } + + private static DomainRuleViolation noRoute(String source, String destination) { + return new DomainRuleViolation("no route from " + source + " to " + destination); + } + + private static final class Candidate { + private final String nodeId; + private final int distanceCost; + private final int turnCount; + private final int estimatedTotalCost; + private final int directionX; + private final int directionY; + private final List nodeIds; + private final List resourceKeys; + private final String pathKey; + + private Candidate(String nodeId, int distanceCost, int turnCount, + int estimatedTotalCost, int directionX, int directionY, + List nodeIds, List resourceKeys, + String pathKey) { + this.nodeId = nodeId; + this.distanceCost = distanceCost; + this.turnCount = turnCount; + this.estimatedTotalCost = estimatedTotalCost; + this.directionX = directionX; + this.directionY = directionY; + this.nodeIds = nodeIds; + this.resourceKeys = resourceKeys; + this.pathKey = pathKey; + } + + private static Candidate start(String source, int heuristic) { + List nodes = new ArrayList(); + nodes.add(source); + List resources = new ArrayList(); + resources.add("NODE:" + source); + return new Candidate(source, 0, 0, heuristic, 0, 0, + nodes, resources, source); + } + + private Candidate extend(EffectiveTopology topology, TopologyEdge edge, + String neighbor, int heuristic) { + TopologyNode currentNode = topology.node(nodeId); + TopologyNode nextNode = topology.node(neighbor); + int nextDirectionX = Integer.compare(nextNode.getX(), currentNode.getX()); + int nextDirectionY = Integer.compare(nextNode.getY(), currentNode.getY()); + int nextTurns = turnCount; + if ((directionX != 0 || directionY != 0) + && (directionX != nextDirectionX || directionY != nextDirectionY)) { + nextTurns++; + } + int nextDistance = distanceCost + edge.getDistanceCost(); + List nextNodes = new ArrayList(nodeIds); + nextNodes.add(neighbor); + List nextResources = new ArrayList(resourceKeys); + nextResources.add(edge.getResourceKey()); + nextResources.add("NODE:" + neighbor); + return new Candidate(neighbor, nextDistance, nextTurns, + nextDistance + heuristic, nextDirectionX, nextDirectionY, + nextNodes, nextResources, pathKey + ">" + neighbor); + } + + private StateKey stateKey() { + return new StateKey(nodeId, directionX, directionY); + } + + private Score score() { + return new Score(distanceCost, turnCount, pathKey); + } + + private RoutePlan toRoutePlan(EffectiveTopology topology) { + return new RoutePlan(nodeIds, resourceKeys, distanceCost, + turnCount, topology.version()); + } + + private int getEstimatedTotalCost() { + return estimatedTotalCost; + } + + private int getDistanceCost() { + return distanceCost; + } + + private int getTurnCount() { + return turnCount; + } + + private String getNodeId() { + return nodeId; + } + + private String getPathKey() { + return pathKey; + } + } + + private static final class StateKey { + private final String nodeId; + private final int directionX; + private final int directionY; + + private StateKey(String nodeId, int directionX, int directionY) { + this.nodeId = nodeId; + this.directionX = directionX; + this.directionY = directionY; + } + + @Override + public boolean equals(Object other) { + if (!(other instanceof StateKey)) { + return false; + } + StateKey that = (StateKey) other; + return nodeId.equals(that.nodeId) + && directionX == that.directionX + && directionY == that.directionY; + } + + @Override + public int hashCode() { + return Objects.hash(nodeId, directionX, directionY); + } + } + + private static final class Score implements Comparable { + private final int distanceCost; + private final int turnCount; + private final String pathKey; + + private Score(int distanceCost, int turnCount, String pathKey) { + this.distanceCost = distanceCost; + this.turnCount = turnCount; + this.pathKey = pathKey; + } + + @Override + public int compareTo(Score other) { + int byDistance = Integer.compare(distanceCost, other.distanceCost); + if (byDistance != 0) { + return byDistance; + } + int byTurns = Integer.compare(turnCount, other.turnCount); + return byTurns != 0 ? byTurns : pathKey.compareTo(other.pathKey); + } + + @Override + public boolean equals(Object other) { + if (!(other instanceof Score)) { + return false; + } + Score that = (Score) other; + return distanceCost == that.distanceCost + && turnCount == that.turnCount + && pathKey.equals(that.pathKey); + } + + @Override + public int hashCode() { + return Objects.hash(distanceCost, turnCount, pathKey); + } + } +} diff --git a/ruoyi-wcs/src/main/java/com/ruoyi/wcs/domain/traffic/RoutePlan.java b/ruoyi-wcs/src/main/java/com/ruoyi/wcs/domain/traffic/RoutePlan.java new file mode 100644 index 0000000..31aa449 --- /dev/null +++ b/ruoyi-wcs/src/main/java/com/ruoyi/wcs/domain/traffic/RoutePlan.java @@ -0,0 +1,55 @@ +package com.ruoyi.wcs.domain.traffic; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +public final class RoutePlan { + private final List nodeIds; + private final List resourceKeys; + private final int distanceCost; + private final int turnCount; + private final long topologyVersion; + + public RoutePlan(List nodeIds, List resourceKeys, + int distanceCost, int turnCount, long topologyVersion) { + this.nodeIds = immutable(nodeIds, "nodeIds"); + this.resourceKeys = immutable(resourceKeys, "resourceKeys"); + if (this.nodeIds.isEmpty()) { + throw new IllegalArgumentException("route must contain at least one node"); + } + if (distanceCost < 0 || turnCount < 0 || topologyVersion <= 0L) { + throw new IllegalArgumentException("route metrics are invalid"); + } + this.distanceCost = distanceCost; + this.turnCount = turnCount; + this.topologyVersion = topologyVersion; + } + + public List getNodeIds() { + return nodeIds; + } + + public List getResourceKeys() { + return resourceKeys; + } + + public int getDistanceCost() { + return distanceCost; + } + + public int getTurnCount() { + return turnCount; + } + + public long getTopologyVersion() { + return topologyVersion; + } + + private static List immutable(List source, String field) { + if (source == null) { + throw new IllegalArgumentException(field + " must not be null"); + } + return Collections.unmodifiableList(new ArrayList(source)); + } +} diff --git a/ruoyi-wcs/src/main/java/com/ruoyi/wcs/domain/traffic/RoutePlanner.java b/ruoyi-wcs/src/main/java/com/ruoyi/wcs/domain/traffic/RoutePlanner.java new file mode 100644 index 0000000..9f0131b --- /dev/null +++ b/ruoyi-wcs/src/main/java/com/ruoyi/wcs/domain/traffic/RoutePlanner.java @@ -0,0 +1,10 @@ +package com.ruoyi.wcs.domain.traffic; + +import com.ruoyi.wcs.domain.topology.EffectiveTopology; + +import java.util.Set; + +public interface RoutePlanner { + RoutePlan plan(EffectiveTopology topology, String sourceNodeId, + String destinationNodeId, Set blockedResources); +} diff --git a/ruoyi-wcs/src/test/java/com/ruoyi/wcs/domain/traffic/AStarRoutePlannerTest.java b/ruoyi-wcs/src/test/java/com/ruoyi/wcs/domain/traffic/AStarRoutePlannerTest.java new file mode 100644 index 0000000..61024db --- /dev/null +++ b/ruoyi-wcs/src/test/java/com/ruoyi/wcs/domain/traffic/AStarRoutePlannerTest.java @@ -0,0 +1,83 @@ +package com.ruoyi.wcs.domain.traffic; + +import com.ruoyi.wcs.domain.shared.DomainRuleViolation; +import com.ruoyi.wcs.domain.topology.EffectiveTopology; +import com.ruoyi.wcs.domain.topology.ResourceDefinition; +import com.ruoyi.wcs.domain.topology.TopologyCompiler; +import com.ruoyi.wcs.domain.topology.TopologyDefinition; +import com.ruoyi.wcs.domain.topology.TopologyEdge; +import com.ruoyi.wcs.domain.topology.TopologyNode; +import org.junit.jupiter.api.Test; + +import java.util.Arrays; +import java.util.Collections; +import java.util.HashSet; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; + +class AStarRoutePlannerTest { + private final RoutePlanner planner = new AStarRoutePlanner(); + + @Test + void avoidsBlockedEdgeAndUsesStableNodeIdTieBreak() { + RoutePlan route = planner.plan(grid(), "A", "D", + Collections.singleton("EDGE:B:D")); + + assertEquals(Arrays.asList("A", "C", "D"), route.getNodeIds()); + assertEquals(Arrays.asList("NODE:A", "EDGE:A:C", "NODE:C", + "EDGE:C:D", "NODE:D"), route.getResourceKeys()); + assertEquals(2, route.getDistanceCost()); + assertEquals(1, route.getTurnCount()); + assertEquals(7L, route.getTopologyVersion()); + } + + @Test + void breaksEqualRouteCostsByNodeId() { + RoutePlan route = planner.plan(grid(), "A", "D", + Collections.emptySet()); + + assertEquals(Arrays.asList("A", "B", "D"), route.getNodeIds()); + } + + @Test + void avoidsBlockedNodeAndNeverTreatsBlockedStartAsAnObstacle() { + RoutePlan route = planner.plan(grid(), "A", "D", + new HashSet(Arrays.asList("NODE:A", "NODE:B"))); + + assertEquals(Arrays.asList("A", "C", "D"), route.getNodeIds()); + } + + @Test + void rejectsUnknownDestination() { + assertThrows(DomainRuleViolation.class, () -> planner.plan( + grid(), "A", "UNKNOWN", Collections.emptySet())); + } + + @Test + void rejectsUnreachableDestination() { + assertThrows(DomainRuleViolation.class, () -> planner.plan(grid(), "A", "D", + new HashSet(Arrays.asList("NODE:B", "NODE:C")))); + } + + private static EffectiveTopology grid() { + TopologyDefinition definition = new TopologyDefinition("FT546", 7L, true, + Arrays.asList( + node("A", 0, 0), node("B", 1, 0), + node("C", 0, 1), node("D", 1, 1)), + Arrays.asList( + edge("A", "B"), edge("B", "D"), + edge("A", "C"), edge("C", "D")), + Collections.emptyList(), + Collections.emptyList(), Collections.emptyList()); + return new TopologyCompiler().compile(definition); + } + + private static TopologyNode node(String id, int x, int y) { + return new TopologyNode(id, "F1", x, y, 1, false, false); + } + + private static TopologyEdge edge(String source, String target) { + return new TopologyEdge(source, target, 1, 1); + } +}