feat(wcs): add deterministic route planning
This commit is contained in:
@@ -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<String> blockedResources) {
|
||||
Objects.requireNonNull(topology, "topology");
|
||||
String source = requireNode(topology, sourceNodeId, "source");
|
||||
String destination = requireNode(topology, destinationNodeId, "destination");
|
||||
Set<String> blocked = blockedResources == null
|
||||
? Collections.<String>emptySet()
|
||||
: new HashSet<String>(blockedResources);
|
||||
if (!source.equals(destination) && blocked.contains("NODE:" + destination)) {
|
||||
throw noRoute(source, destination);
|
||||
}
|
||||
|
||||
PriorityQueue<Candidate> open = new PriorityQueue<Candidate>(candidateOrder());
|
||||
Map<StateKey, Score> best = new HashMap<StateKey, Score>();
|
||||
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<Candidate> 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<String> nodeIds;
|
||||
private final List<String> resourceKeys;
|
||||
private final String pathKey;
|
||||
|
||||
private Candidate(String nodeId, int distanceCost, int turnCount,
|
||||
int estimatedTotalCost, int directionX, int directionY,
|
||||
List<String> nodeIds, List<String> 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<String> nodes = new ArrayList<String>();
|
||||
nodes.add(source);
|
||||
List<String> resources = new ArrayList<String>();
|
||||
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<String> nextNodes = new ArrayList<String>(nodeIds);
|
||||
nextNodes.add(neighbor);
|
||||
List<String> nextResources = new ArrayList<String>(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<Score> {
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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<String> nodeIds;
|
||||
private final List<String> resourceKeys;
|
||||
private final int distanceCost;
|
||||
private final int turnCount;
|
||||
private final long topologyVersion;
|
||||
|
||||
public RoutePlan(List<String> nodeIds, List<String> 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<String> getNodeIds() {
|
||||
return nodeIds;
|
||||
}
|
||||
|
||||
public List<String> getResourceKeys() {
|
||||
return resourceKeys;
|
||||
}
|
||||
|
||||
public int getDistanceCost() {
|
||||
return distanceCost;
|
||||
}
|
||||
|
||||
public int getTurnCount() {
|
||||
return turnCount;
|
||||
}
|
||||
|
||||
public long getTopologyVersion() {
|
||||
return topologyVersion;
|
||||
}
|
||||
|
||||
private static List<String> immutable(List<String> source, String field) {
|
||||
if (source == null) {
|
||||
throw new IllegalArgumentException(field + " must not be null");
|
||||
}
|
||||
return Collections.unmodifiableList(new ArrayList<String>(source));
|
||||
}
|
||||
}
|
||||
@@ -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<String> blockedResources);
|
||||
}
|
||||
@@ -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.<String>emptySet());
|
||||
|
||||
assertEquals(Arrays.asList("A", "B", "D"), route.getNodeIds());
|
||||
}
|
||||
|
||||
@Test
|
||||
void avoidsBlockedNodeAndNeverTreatsBlockedStartAsAnObstacle() {
|
||||
RoutePlan route = planner.plan(grid(), "A", "D",
|
||||
new HashSet<String>(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.<String>emptySet()));
|
||||
}
|
||||
|
||||
@Test
|
||||
void rejectsUnreachableDestination() {
|
||||
assertThrows(DomainRuleViolation.class, () -> planner.plan(grid(), "A", "D",
|
||||
new HashSet<String>(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.<ResourceDefinition>emptyList(),
|
||||
Collections.<String>emptyList(), Collections.<String>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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user